/*  A maze solving robot by Ben Axelrod

The robot uses the A* method to mark cells and determine shortest path.  This program
is actually very small, however, the choose function is huge.  This is because this
is where all of the robot's maze solving strategy is stored.  This strategy is as 
follows:  

1. Check for walls (so you know where you absolutly can and can not go)
2. If a cell around you is 2 more than your current cell or 2 less than
     your current cell, go there.  This ensures that when the robot 
     finds a longer path, it will update it.  Even if the path you it 
     is on needs the updating.  Some of this is taken care of in the 
     main search task.  (Even though the check for +2 and -2 is the same, 
     different things happen in each scenario.  If another cell is -2, 
     then the robot moves onto it, records its index# then updates the 
     new path.  If the cell is +2, the robot proceeds just like it was a 
     0 cell, overwriting the path's numbers.)
3. If there is a zero cell, go there.  This keeps the robot finding new 
     paths.
4. If there is still more than one way to go, randomly choose one.

In all of these "checks" above, the robot will go straight first if it can.  Hopefully,
these checks will sufficiently narrow down the choice of paths for the robot from 
potentially 3 to 1 every time.  If not there is a randomizer that will choose one path
from more than one.  This isn't the most elagent code, but it works.  Another neat check
that was not implemented here, might be to head in the general direction of the goal.

Also there is no "better path" checking in this program.  Once the robot has found the 
goal cell, it does not look to see if it has found all the paths leading to the goal.  
It then takes the fastest path it knows of all the way back home.  I tried to eliminate 
this problem in the check function by having the robot always going over old paths and 
going where there is a 0.  That is, hopefully it will find the fastest path before it 
finds the goal.

This program is just the brains of the robot.  I have not yet spent time into a reliable
wall sensing method and cell positioning.  I tested this robot with 3 touch sensors and 
at every intersection I had to press the touch sensors where the robot could go.  It was
also nessisary to consantly re-position the robot so that it was centered in the cells.
*/

#pragma init Initialize()
#define Rgoal 1  //the row of the goal cell
#define Cgoal 4  //the column of the goal cell
#define Rstart 4  //the row of the start cell
#define Cstart 0  //the column of the start cell
#define mazesize 5  //5 is the max!
#define backmark 25 //what to mark the cells on your way out

#define leftmotor OUT_C
#define rightmotor OUT_A
#define spintime 26
#define fwdtime 48

#define sum (paths[0]+paths[1]+paths[2])
#define Do180 {TurnLeft(); TurnLeft();}

#define Array2D(r,c) Array1D[mazesize*r+c]  //2D to 1D mapping function

int Array1D[mazesize*mazesize],R,C,Dr,Dc; 

/*
R and C are the current cell
values for R and C     [R,C]
[0,0][0,1][0,2][0,3][0,4]
[1,0][1,1][1,2][1,3][1,4]
[2,0][2,1][2,2][2,3][2,4]
[3,0][3,1][3,2][3,3][3,4]
[4,0][4,1][4,2][4,3][4,4]


Dr and Dc are the direction the robot is facing
 (Dr,Dc) |  Compas
---------|---------
 (-1,0)  |  North
 (0,1)   |  East
 (1,0)   |  South
 (0,-1)  |  West
 
 
Updating R and C when changing cells
 Forward:
    R=Dr+R;
    C=Dc+C;
 Left:
    temp=Dr;
    Dr=(-1)*Dc;
    Dc=temp;
 Right:
    temp=Dc;
    Dc=(-1)*Dr;
    Dr=temp;
    
    
To look at the contents of another cell around you:
current cell value:  Array2D(R,C)
forward cell value:  Array2D((R+Dr),(C+Dc))
left cell value:     Array2D((R-Dc),(C+Dr)) 
right cell value:    Array2D((R+Dc),(C-Dr))
*/

void Initialize()
{
  SetPower(leftmotor + rightmotor,5);
  SetSensor(SENSOR_1,SENSOR_TOUCH);
  SetSensor(SENSOR_2,SENSOR_TOUCH);
  SetSensor(SENSOR_3,SENSOR_TOUCH);
  int x;
  for (x=0;x<mazesize*mazesize;x++)
    Array1D[x]=0;
  R=Rstart;        //initialize everything
  C=Cstart;
  Dr=-1;
  Dc=0;  
}

task main()
{
  int index;  //index is the counter that you mark the cells with
  
  until (SENSOR_1==1)
    index=Random(25);
  Wait(50);
    
  Array2D(R,C)=1;
  index=2;
  until (R==Rgoal && C==Cgoal) 
  { 
    check(); //looks at the 3 cells surrounding bot, determines where to go, and turns the bot
    DriveFwd(); //moves forward 1 square and updates R and C
    if (Array2D(R,C)==0 || Array2D(R,C)>=index)  
    { 
      Array2D(R,C)=index;  //mark cell 
      index++;     //increment index
    } 
    else 
    {
      if (Array2D(R,C)<index-2)//if you found a faster path, 180 so you can update old path
        Do180;
      index=Array2D(R,C)+1;  //use the faster path's index number
    }
    
  } 
  backtrack();
}


void check()
{
  int paths[3];
  
  paths[0]=0;
  paths[1]=0;
  paths[2]=0;
  ClearTimer(0);
  PlaySound(0);
  while(Timer(0)<20)     //0 for wall, 1 for path
  {
    if (SENSOR_1 == 1)
      paths[0]=1;
    if (SENSOR_2 == 1)
      paths[1]=1;
    if (SENSOR_3 == 1)
      paths[2]=1;
  }
  PlaySound(0);
  Wait(10);
  
  if ( sum > 1 )
  {   
    //check2    go there if the cell is more than +- 2  !!!but not zero!!!
    if (paths[1]==1 && Array2D((R+Dr),(C+Dc))!=0) //check fwd
      if ( Array2D((R+Dr),(C+Dc))>Array2D(R,C)+1 || Array2D((R+Dr),(C+Dc))<Array2D(R,C)-1 )
      {
        paths[0]=0;
        paths[1]=1;
        paths[2]=0;
      }
    if (paths[0]==1 && Array2D((R-Dc),(C+Dr))!=0) //check left
      if ( Array2D((R-Dc),(C+Dr))>Array2D(R,C)+1 || Array2D((R-Dc),(C+Dr))<Array2D(R,C)-1 )
      {
        paths[0]=1;
        paths[1]=0;
        paths[2]=0;
      }
    if (paths[2]==1 && Array2D((R+Dc),(C-Dr))!=0) //check right
      if ( Array2D((R+Dc),(C-Dr))>Array2D(R,C)+1 || Array2D((R+Dc),(C-Dr))<Array2D(R,C)-1 )
      {
        paths[0]=0;
        paths[1]=0;
        paths[2]=1;
      }
    if (sum>1)
    {
      //check 3    go there if there is a zero
      if ( paths[1]==1 && Array2D((R+Dr),(C+Dc))==0 )  //check fwd
      {
        paths[0]=0;
        paths[1]=1;
        paths[2]=0;
      }
      if ( paths[0]==1 && Array2D((R-Dc),(C+Dr))==0 ) //check left
      {
        paths[0]=1;
        paths[1]=0;
        paths[2]=0;
      }
      if ( paths[2]==1 && Array2D((R+Dc),(C-Dr))==0 ) //check right
      {
        paths[0]=0;
        paths[1]=0;
        paths[2]=1;
      }
      if (sum>1)  //check4    random time!
      {
        if (sum==3) // [1 1 1]
        {
          paths[0]=0;
          paths[1]=0;
          paths[2]=0;
          paths[Random(2)]=1;
        }
        if ( paths[0]==1 && paths[1]==1)  // [1 1 0]
        {
          if (Random(1))
          {
            paths[0]=1;
            paths[1]=0;
            paths[2]=0;
          }
          else
          {
            paths[0]=0;
            paths[1]=1;
            paths[2]=0;
          }
        }
        if ( paths[0]==1 && paths[2]==1)  // [1 0 1]
        {
          if (Random(1))
          {
            paths[0]=1;
            paths[1]=0;
            paths[2]=0;
          }
          else
          {
            paths[0]=0;
            paths[1]=0;
            paths[2]=1;
          }
        }
        if ( paths[1]==1 && paths[2]==1 ) // [0 1 1]
        {
          if (Random(1))
          {
            paths[0]=0;
            paths[1]=1;
            paths[2]=0;
          }
          else
          {
            paths[0]=0;
            paths[1]=0;
            paths[2]=1;
          }
        } 
      }
    }
  }
  
  if ( sum == 0 ) 
    Do180;
    
  if ( paths[0] == 1 )
    TurnLeft();
  else if ( paths[2] == 1 )
    TurnRight();
}

void backtrack()
{
  PlaySound(5);
  int paths[3];
  Do180;
  Wait(50);  
  
  until (R==Rstart && C==Cstart)
  {
    Array2D(R,C)=Array2D(R,C)+backmark;
    PlaySound(0);
    paths[0]=0;
    paths[1]=0;
    paths[2]=0;
    ClearTimer(0);
    while(Timer(0)<20)     //0 for wall, 1 for path
    {
      if (SENSOR_1 == 1)
        paths[0]=1;
      if (SENSOR_2 == 1)
        paths[1]=1;
      if (SENSOR_3 == 1)
        paths[2]=1;
    }
    PlaySound(0); 
    Wait(10);  
   
    if ( Array2D((R-Dc),(C+Dr))==(Array2D(R,C)-backmark-1) && paths[0]==1)
      TurnLeft();
    else if ( Array2D((R+Dc),(C-Dr))==(Array2D(R,C)-backmark-1) && paths[2]==1)
      TurnRight();
      
    DriveFwd();
  }
  
  Array2D(R,C)=Array2D(R,C)+backmark;
  PlaySound(5);
  Do180;
  Wait(100);
  
  until (R==Rgoal && C==Cgoal)
  {
    if ( Array2D((R-Dc),(C+Dr))== Array2D(R,C)+1 )
      TurnLeft();
    else if ( Array2D((R+Dc),(C-Dr))== Array2D(R,C)+1 )
      TurnRight();
    DriveFwd();
    Wait(25);
  }
  PlaySound(5);
}


void TurnLeft()
{
  int temp;
  temp=Dr;
  Dr=(-1)*Dc;
  Dc=temp;
  OnRev(rightmotor);
  OnRev(leftmotor);
  Wait(spintime);
  Off(rightmotor+leftmotor);
}

void TurnRight()
{
  int temp;
  temp=Dc;
  Dc=(-1)*Dr;
  Dr=temp;
  OnFwd(rightmotor);
  OnFwd(leftmotor);
  Wait(spintime);
  Off(rightmotor+leftmotor);
}

void DriveFwd()
{
  R=Dr+R;
  C=Dc+C;
  OnFwd(leftmotor);
  OnRev(rightmotor);
  Wait(fwdtime);
  Off(rightmotor);
  Off(leftmotor);
}