//GLOBAL VARIABLES grid world dimensions
const int x_size = 10;
const int y_size = 5;

//GLOBAL ARRAY representation of grid world using a 2-Dimensional array
//0  = open space
//1  = barrier
//2  = goal
//99 = robot
int map[x_size][y_size] =
 {{0,0,0,0,0},
  {0,1,99,1,0},
  {0,1,1,1,0},
  {0,0,0,0,0},
  {0,0,0,0,0},
  {0,0,0,0,0},
  {0,0,0,0,0},
  {0,0,2,0,0},
  {0,0,0,0,0},
  {0,0,0,0,0}};

//FUNCTION move forward for a variable number of grid blocks
void moveForward(int blocks)
{
  //convert number of blocks to encoder counts
  //wheel circumference = 17.6 cm
  //one block = 23.7 cm / 35.cm
  int countsToTravel = (35/17.6)*(360)*blocks;

  //encoder target for countsToTravel
  nMotorEncoder[motorB] = 0;
  nMotorEncoder[motorC] = 0;
  nMotorEncoderTarget[motorB] = countsToTravel;
  nMotorEncoderTarget[motorC] = countsToTravel;
  motor[motorB] = 50;
  motor[motorC] = 50;
  while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorC] != runStateIdle) {}

  //stop for half second at end of movement
  motor[motorB] = 0;
  motor[motorC] = 0;
  wait1Msec(500);
}

//FUNCTION left point turn 90 degrees
void turnLeft90()
{
  //distance one wheel must travel for 90 degree point turn = 10.68 cm
  //wheel circumference = 17.6 cm
  int countsToTravel = (8.6/17.6)*(360);

  //encoder target for countsToTravel
  nMotorEncoder[motorB] = 0;
  nMotorEncoder[motorC] = 0;
  nMotorEncoderTarget[motorB] = countsToTravel;
  nMotorEncoderTarget[motorC] = countsToTravel;
  motor[motorB] = 50;
  motor[motorC] = -50;
  while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorC] != runStateIdle) {}

  //stop for half second at end of movement
  motor[motorB] = 0;
  motor[motorC] = 0;
  wait1Msec(500);
}

//FUNCTION right point turn 90 degrees
void turnRight90()
{
  //distance one wheel must travel for 90 degree point turn = 10.68 cm
  //wheel circumference = 17.6 cm
  int countsToTravel = (8.6/17.6)*(360);

  //encoder target for countsToTravel
  nMotorEncoder[motorB] = 0;
  nMotorEncoder[motorC] = 0;
  nMotorEncoderTarget[motorB] = countsToTravel;
  nMotorEncoderTarget[motorC] = countsToTravel;
  motor[motorB] = -50;
  motor[motorC] = 50;
  while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorC] != runStateIdle) {}

  //stop for half second at end of movement
  motor[motorB] = 0;
  motor[motorC] = 0;
  wait1Msec(500);
}

//FUNCTION print wavefront map to NXT screen
void PrintWavefrontMap()
{
  int printLine = y_size-1;
  for(int y = 0; y < y_size; y++)
  {
    string printRow = "";
    for(int x=0; x < x_size; x++)
    {
      if(map[x][y] == 99)
        printRow = printRow + "R ";
      else if(map[x][y] == 2)
        printRow = printRow + "G ";
      else if(map[x][y] == 1)
        printRow = printRow + "X ";
      else if(map[x][y] < 10)
        printRow = printRow + map[x][y] + " ";
      else if(map[x][y] == '*')
        printRow = printRow + "* ";
      else
        printRow = printRow + map[x][y];
    }
    nxtDisplayString(printLine, printRow);
    printLine--;
  }
}

//FUNCTION wavefront algorithm to find most efficient path to goal
void WavefrontSearch()
{
  int goal_x, goal_y;
  bool foundWave = true;
  int currentWave = 2; //Looking for goal first

  while(foundWave == true)
  {
    foundWave = false;
    for(int y=0; y < y_size; y++)
    {
      for(int x=0; x < x_size; x++)
      {
        if(map[x][y] == currentWave)
        {
          foundWave = true;
          goal_x = x;
          goal_y = y;

          if(goal_x > 0) //This code checks the array bounds heading WEST
            if(map[goal_x-1][goal_y] == 0)  //This code checks the WEST direction
              map[goal_x-1][goal_y] = currentWave + 1;

          if(goal_x < (x_size - 1)) //This code checks the array bounds heading EAST
            if(map[goal_x+1][goal_y] == 0)//This code checks the EAST direction
              map[goal_x+1][goal_y] = currentWave + 1;

          if(goal_y > 0)//This code checks the array bounds heading SOUTH
            if(map[goal_x][goal_y-1] == 0) //This code checks the SOUTH direction
              map[goal_x][goal_y-1] = currentWave + 1;

          if(goal_y < (y_size - 1))//This code checks the array bounds heading NORTH
            if(map[goal_x][goal_y+1] == 0) //This code checks the NORTH direction
              map[goal_x][goal_y+1] = currentWave + 1;
        }
      }
    }
    currentWave++;
    PrintWavefrontMap();
    wait1Msec(500);
  }
}

//FUNCTION follow most efficient path to goal
//and update screen map as robot moves
void NavigateToGoal()
{
  //Store our Robots Current Position
  int robot_x, robot_y;

  //First - Find Goal and Target Locations
  for(int x=0; x < x_size; x++)
  {
    for(int y=0; y < y_size; y++)
    {
      if(map[x][y] == 99)
      {
        robot_x = x;
        robot_y = y;
      }
    }
  }

  //Found Goal and Target, start deciding our next path
  int current_x = robot_x;
  int current_y = robot_y;
  int current_facing = 0;
  int next_Direction = 0;
  int current_low = 99;

  while(current_low > 2)
  {
    current_low = 99; //Every time, reset to highest number (robot)
    next_Direction = current_facing;
    int Next_X = 0;
    int Next_Y = 0;

    //Check Array Bounds West
    if(current_x > 0)
      if(map[current_x-1][current_y] < current_low && map[current_x-1][current_y] != 1) //Is current space occupied?
    {
      current_low = map[current_x-1][current_y];  //Set next number
      next_Direction = 3; //Set Next Direction as West
      Next_X = current_x-1;
      Next_Y = current_y;
    }

    //Check Array Bounds East
    if(current_x < (x_size -1))
      if(map[current_x+1][current_y] < current_low && map[current_x+1][current_y] != 1) //Is current space occupied?
    {
      current_low = map[current_x+1][current_y];  //Set next number
      next_Direction = 1; //Set Next Direction as East
      Next_X = current_x+1;
      Next_Y = current_y;
    }

    //Check Array Bounds South
    if(current_y > 0)
      if(map[current_x][current_y-1] < current_low && map[current_x][current_y-1] != 1)
    {
      current_low = map[current_x][current_y-1];  //Set next number
      next_Direction = 2; //Set Next Direction as South
      Next_X = current_x;
      Next_Y = current_y-1;
    }

    //Check Array Bounds North
    if(current_y < (y_size - 1))
      if(map[current_x][current_y+1] < current_low && map[current_x][current_y+1] != 1) //Is current space occupied?
    {
      current_low = map[current_x][current_y+1];  //Set next number
      next_Direction = 0; //Set Next Direction as North
      Next_X = current_x;
      Next_Y = current_y+1;
    }

    //Okay - We know the number we're heading for, the direction and the coordinates.
    current_x = Next_X;
    current_y = Next_Y;
    map[current_x][current_y] = '*';

    //Track the robot's heading
    while(current_facing != next_Direction)
    {
      if (current_facing > next_Direction)
      {
        turnLeft90();
        current_facing--;
      }
      else if(current_facing < next_Direction)
      {
        turnRight90();
        current_facing++;
      }
    }
    moveForward(1);
    PrintWavefrontMap();
    wait1Msec(500);
  }
}

task main()
{
  WavefrontSearch();    //Build map of route with wavefront algorithm
  NavigateToGoal(); //Follow most efficient path to goal
  wait1Msec(5000);  //Leave time to view the LCD screen
}