#include <stdio.h>
#include <vector>
#include <stdlib.h>

using namespace std;

typedef struct cvor
{
  int mat[3][3];
  int g;
  int h;
  cvor* roditelj;
}_cvor;

void stampaj(int mat[3][3])
{
  for(int i=0; i<3; i++)
  {
	for(int j=0; j<3; j++)
	  printf("%d ", mat[i][j]);
	printf("\n");
  }
}

int heuristika(int mat[3][3])
{
  int s = 0;
  for(int i=0; i<3; i++)
	for(int j=0; j<3; j++)
	  s += abs(mat[i][j]%3 - j) + abs(mat[i][j]/3 - i);
	
  return s;
}

void kopiraj(int mat1[3][3], int mat2[3][3])
{
  for(int i=0; i<3; i++)
	for(int j=0; j<3; j++)
	  mat1[i][j] = mat2[i][j];
}

int poredi(int mat1[3][3], int mat2[3][3])
{
  for(int i=0; i<3; i++)
	for(int j=0; j<3; j++)
	  if (mat1[i][j] != mat2[i][j])
		return 0;
	  
  return 1;
}

void nadji_sledbenike(_cvor* tekuci, vector<_cvor*>& sledbenici)
{
  int ind_i, ind_j;
  int sled_mat[3][3];
  
  for(int i=0; i<3; i++)
	for(int j=0; j<3; j++)
	  if (tekuci->mat[i][j] == 0)
	  {
		ind_i= i;
		ind_j = j;
		break;
	  }
	  
  kopiraj(sled_mat, tekuci->mat);
  
  if (ind_i + 1 < 3)
  {
	sled_mat[ind_i][ind_j] = sled_mat[ind_i+1][ind_j];
	sled_mat[ind_i+1][ind_j] = 0;
	
	_cvor* tmp = (_cvor*)malloc(sizeof(_cvor));
	tmp->g = tekuci->g + 1;
	tmp->h = heuristika(sled_mat);
	tmp->roditelj = tekuci;
	kopiraj(tmp->mat, sled_mat);
	
	sledbenici.push_back(tmp);
  }
  
  kopiraj(sled_mat, tekuci->mat);
  
  if (ind_j + 1 < 3)
  {
	sled_mat[ind_i][ind_j] = sled_mat[ind_i][ind_j+1];
	sled_mat[ind_i][ind_j+1] = 0;
	
	_cvor* tmp = (_cvor*)malloc(sizeof(_cvor));
	tmp->g = tekuci->g + 1;
	tmp->h = heuristika(sled_mat);
	tmp->roditelj = tekuci;
	kopiraj(tmp->mat, sled_mat);
	
	sledbenici.push_back(tmp);
  }
  
  kopiraj(sled_mat, tekuci->mat);
  
  if (ind_j - 1 >= 0)
  {
	sled_mat[ind_i][ind_j] = sled_mat[ind_i][ind_j-1];
	sled_mat[ind_i][ind_j-1] = 0;
	
	_cvor* tmp = (_cvor*)malloc(sizeof(_cvor));
	tmp->g = tekuci->g + 1;
	tmp->h = heuristika(sled_mat);
	tmp->roditelj = tekuci;
	kopiraj(tmp->mat, sled_mat);
	
	sledbenici.push_back(tmp);
  }
  
  kopiraj(sled_mat, tekuci->mat);
  
  if (ind_i - 1 >= 0)
  {
	sled_mat[ind_i][ind_j] = sled_mat[ind_i - 1][ind_j];
	sled_mat[ind_i - 1][ind_j] = 0;
	
	_cvor* tmp = (_cvor*)malloc(sizeof(_cvor));
	tmp->g = tekuci->g + 1;
	tmp->h = heuristika(sled_mat);
	tmp->roditelj = tekuci;
	kopiraj(tmp->mat, sled_mat);
	
	sledbenici.push_back(tmp);
  }
}

int astar(int start[3][3], int finis[3][3], vector<_cvor*>& putanja)
{
  int put_nadjen = 0;
  int j;
  _cvor* tmp = (_cvor*)malloc(sizeof(_cvor));
  tmp->g = 0;
  tmp->h = heuristika(start);
  tmp->roditelj = NULL;
  kopiraj(tmp->mat, start);
  
  vector<_cvor*> otvorena_lista;
  vector<_cvor*> zatvorena_lista;
  
  otvorena_lista.push_back(tmp);
  
  _cvor* tekuci;
  
  
  while(otvorena_lista.size() > 0)
  {
	int ind_min = 0;
	for(int i = 1; i< otvorena_lista.size(); i++)
	  if (otvorena_lista[i]->g + otvorena_lista[i]->h < otvorena_lista[ind_min]->g + otvorena_lista[ind_min]->h)
		ind_min = i;
	  
	tekuci = otvorena_lista[ind_min];
	
	if (poredi(tekuci->mat, finis))
	{
	  put_nadjen = 1;
	  break;
	}
	
	vector<_cvor*> sledbenici;
	
	nadji_sledbenike(tekuci, sledbenici);
	
	for(int i = 0; i<sledbenici.size(); i++)
	{
	  //stampaj(sledbenici[i]->mat);
	  
	  for(j=0; j<zatvorena_lista.size(); j++)
		if (poredi(sledbenici[i]->mat, zatvorena_lista[j]->mat))
		  break;
		
	  if (j < zatvorena_lista.size())
		continue;
	  
	  for(j=0; j<otvorena_lista.size(); j++)
		if (poredi(sledbenici[i]->mat, otvorena_lista[j]->mat))
		  break;
		
	  if (j < otvorena_lista.size())
	  {
		if (sledbenici[i]->g + sledbenici[i]->h < otvorena_lista[j]->g + otvorena_lista[j]->h)
		{
		  otvorena_lista[j]->g = sledbenici[i]->g;
		  otvorena_lista[j]->h = sledbenici[i]->h;
		  otvorena_lista[j]->roditelj = tekuci;
		}
	  }
	  else
		otvorena_lista.push_back(sledbenici[i]);
	}
	
	zatvorena_lista.push_back(tekuci);
	otvorena_lista.erase(otvorena_lista.begin() + ind_min);
  }  
  
  
  
  while(tekuci != NULL)
  {
	putanja.push_back(tekuci);
	tekuci = tekuci->roditelj;
  }
  
  return put_nadjen;
}

int main()
{
  int start[3][3] = {0,3,1,
                     7,5,4,
                     8,6,2};
					 
  int cilj[3][3] = {0, 1, 2, 
                    3, 4, 5,
					6, 7, 8};
					
  vector<_cvor*> putanja;

  if (astar	(start, cilj, putanja)) 
  {
	for(int i=putanja.size() - 1; i >=0 ; i--)
	{
	  stampaj(putanja[i]->mat);
	  printf("\n");
	}
  }	  
  else
	printf("Nije moguce sloziti\n");
  
  return 0;
}