#include <math.h>

// Declaring the grid and its variations ( Visited and Hueristic and dynamic path )
  int grid[7][8]= {{1,1,1,1,1,1,1,1},
                   {1,0,1,0,0,0,0,1},
                   {1,0,1,0,0,0,0,1},
                   {1,0,1,0,0,0,0,1},
                   {1,0,1,0,0,1,0,1},
                   {1,0,0,0,0,1,0,1},
                   {1,1,1,1,1,1,1,1},};

int visited[7][8]={{1,1,1,1,1,1,1,1},
                   {1,0,1,0,0,0,0,1},
                   {1,0,1,0,1,0,0,1},
                   {1,0,1,0,1,0,0,1},
                   {1,0,1,0,1,1,0,1},
                   {1,0,0,0,1,1,0,1},
                   {1,1,1,1,1,1,1,1},};
double hueristic[7][8];


// Declaring start goal and various other variables
int startx=1;
int starty=1;
int goalx=5;
int goaly=6;
int locx;
int locy;
int index;
double costarray[4] ;
double f;
int g = 1;
int maxval = 100;
void setup()
{
  Serial.begin(115200);
  //calculating hueristic 
  
  if((goaly < 8 && goalx < 7 ) || (goalx > -1 && goaly > -1 ) )
  {for(int i = 0; i <8; i++)
  {for(int j = 0; j <7; j++)
      {hueristic[j][i] = sqrt( ((j-goalx)*(j-goalx)) + ((i-goaly)*(i-goaly)) );
       if (visited[j][i] == 1)
        {
          hueristic[j][i] = hueristic[j][i] + 50 ;
        }}}}
  //------------------------------------------------// Uncomment Region to view the hueristic grid
  for(int i = 0; i <7; i++)
  {for(int j = 0; j <8; j++)
      {
        Serial.print(hueristic[i][j]);
        Serial.print("  ");
      }
      Serial.println();
      }
 //  for(;;);
  //------------------------------------------------    
 
 locx = startx;
 locy = starty;
 }
void loop()
{ // 0 index for up. 1 for right. 2 for down. 3 for left. overhere we assign our costs and prioritize our robots movement.
  costarray[0] = g + hueristic[locx-1][locy];
  costarray[1] = g + hueristic[locx][locy+1];
  costarray[2] = g + hueristic[locx+1][locy]; 
  costarray[3] = g + hueristic[locx][locy-1];
  //----------------------------------------------------------------------------------------------------------------------

  //Finding node with least cost
  for(int i=0;i<4;i++)
     { if(costarray[i] < maxval)
        {index = i;
        maxval = costarray[i];}}
  //----------------------------
  visited[locx][locy]=1;
  //Serial.print(index);
  //Serial.println();
  
  //move your position to the new location
  if(index == 0)
  {locx = locx-1;
 
    }
      else if(index == 1)
      {locy = locy+1;
        }
          else if(index == 2)
          {locx = locx+1;
             }
                else if(index == 3)
                {locy = locy-1;
                   }
  Serial.print("(");
  Serial.print(locx);
  Serial.print(",");
  Serial.print(locy);
  Serial.print(")");
  Serial.println();
  if(locx == goalx && locy == goaly)
  {Serial.println("target found YA PIECE OF HORSE DUNG ");
  for(;;);
  }
  
  
}