//Robot source code
//return Home

#define LIGHT  IN_1
#define BUTTON IN_2
#define FRONT  IN_3
#define LIGHT_SENSOR_OFFSET 5

#define TURN_SPEED  7
#define DRIVE_SPEED 7
#define REV_SPEED   7

#define FALSE 0
#define TRUE  1

#define UP    0
#define RIGHT 1
#define DOWN  2
#define LEFT  3

#define HEIGHT 3     /* the height of the maze */
#define WIDTH  4    /* the width of the maze */

#define HEADSTART 50  /* used to make turns smoother */
#define NUMREADS 2    /* number of light readings to average in FollowLine() */
#define BREAK 0xFFFF  /* used for data logging */

int WHITELOW;  int WHITEHI;  /* These are our color values for the */
int GRAYLOW;   int GRAYHI;  /* light sensor.  They are initialized */
int BLACKLOW;  int BLACKHI;  /* in SetupStuff() */

int bitmap;    /* binary string, represents which squares the robot has been to */
int bitmask;   /* binary string, used by CheckMap() */
int tempExp;   /* used to simulate shift-left */
int tempBitmap;/* used to simulate shift-left */

int i;         /* integer, the vertical coordinate of the robot */
int j;         /* integer, the horizontal coordinate of the robot */
int tempI;     /* integer, used by CheckMap(); */
int tempJ;     /* integer, used by CheckMap(); */

int Direction; /* integer between 0 and 3, code for the direction the robot is currently facing */
int DesiredDirection;  /* integer between 0 and 3, used by BackUp() */

int NewPath;   /* boolean, used in the main loop */

int visited;   /* boolean, set by CheckMap() for use in the main loop */
int wall;      /* boolean, set by CheckWall() for use in the main loop */

// we're assuming that a crude four-deep stack is enough for our tiny maze
int lastmove;  /* integer between 0 and 3 */
int lastmove2; /* integer between 0 and 3 */
int lastmove3; /* integer between 0 and 3 */
int lastmove4; /* integer between 0 and 3 */
 

/*********************************************
* main
**********************************************
* see documentation (README)
*********************************************/
task main
{
    int n;

    SetupStuff();
    // pause for a bit
    Sleep(50);
    while (true)
    {
        start MarkPath;
        start FollowLine;
        Fwd(OUT_A + OUT_C,DRIVE_SPEED);
        wait(FRONT < 300);
        Off(OUT_A+OUT_C);
        Sleep(20);
        stop FollowLine;
        stop MarkPath;
        // when we hit a wall, we should back up a bit
        Rev(OUT_A+OUT_C, REV_SPEED);
        Sleep(10);
        Off(OUT_A+OUT_C);

        // Here's the path-choosing loop.  We just hit a wall, so we know that straight ahead isn't an option.
        NewPath = FALSE;
        n = 0;
        while (NewPath == FALSE)
        {
            n += 1;
            TurnLeft();
            CheckMap();
            if (visited == FALSE)
                CheckWall();
            // NewPath = !(wall || visited)
            // -- but there isn't any ! operator, so we invert manually
            NewPath = (wall | visited);
            if(NewPath == FALSE)
                NewPath = TRUE;
            else NewPath = FALSE;
            // re-initialize for the next loop
            wall = FALSE;
            visited = FALSE;

            if (NewPath == FALSE && (n == 3))
            {
                // We don't set n=0 because we didn't just hit a wall, so we can't eliminate any choices.
                n = -1;
                BackUp();
            }
        }
    }
}
 

/*********************************************
* CheckMap()
**********
* Checks the internal bitmap representation of our maze, to see if the square
* our robot is facing has already been visited or not.
* PRECONDITION:   the robot is facing the square you want to check.
* POSTCONDITION:  visited is TRUE if the robot has already been in that square, otherwise it is FALSE.
**********************************************/
sub CheckMap()
{
    tempI = i;   tempJ = j;

    if(Direction == DOWN)
        tempI += 1;
    else if(Direction == RIGHT)
        tempJ += 1;
    else if(Direction == UP)
        tempI -= 1;
    else if(Direction == LEFT)
        tempJ -= 1;

    // bitmask = 1 << (4*tempI + (3-tempJ));
    bitmask = 1;
    tempExp = (4*tempI + (3-tempJ));
    repeat (tempExp) bitmask *= 2;

    visited = bitmap & bitmask;
    if (visited != 0) visited = TRUE;
    // we can save time by not checking boundary walls
    if ( (tempI >= HEIGHT) || (tempI < 0) || (tempJ >= WIDTH) || (tempJ < 0) )
        visited = TRUE;
}
 

/*********************************************
* CheckWall()
**********
* Checks the area ahead, to see if we can move forward without bumping into a wall.
* PRECONDITION:   the robot is facing the square you want to check.
* POSTCONDITION:  wall is TRUE if the square is blocked by a wall, otherwise FALSE.
**********************************************/
sub CheckWall()
{
    // Move forward until we hit either a wall or a black gridline (= no wall).
    Fwd(OUT_A + OUT_C, DRIVE_SPEED);
    wait((FRONT < 300) || (LIGHT > BLACKLOW));
    Off(OUT_A+OUT_C);
    Sleep(10);
    // if we didn't hit a black line, then we must have hit a wall
    if (LIGHT < BLACKLOW)
        wall = TRUE;
    else
        wall = FALSE;

    // we want to back up a bit after we're done
    Rev(OUT_A+OUT_C, REV_SPEED);
    Sleep(35);
    Off(OUT_A+OUT_C);
}
 

/*********************************************
* BackUp()
**********
* This subroutine will bring the robot to the square it just came from, and pop the stack of recent moves
**********************************************/
sub BackUp()
{
    // We want to turn to the opposite direction of our last move
    DesiredDirection = (lastmove + 2) & 0x0003;

    if (  DesiredDirection == ((Direction + 1) & 0x0003)  )
        TurnRight();
    else if (  DesiredDirection == ((Direction + 3) & 0x0003)  )
        TurnLeft();
    else if (  DesiredDirection == ((Direction + 2) & 0x0003)  )
        Turn180();
    else if (  DesiredDirection == Direction  )
        /* do nothing */;

    // Now that we're facing the right direction, we can move into the square.  Since we don't know
    // exactly how far to move, we'll just wait until we see the next gridline (or a wall) and then
    // retreat a bit.
    start FollowLine;
    Fwd(OUT_A+OUT_C, DRIVE_SPEED);
    wait(LIGHT > BLACKLOW);
    Sleep(20);

    wait((FRONT < 300) || (LIGHT > BLACKLOW));
    stop FollowLine;
    Off(OUT_A+OUT_C);
    Sleep(10);
    // We want to back up a bit after we're done
    Rev(OUT_A+OUT_C, REV_SPEED);
    Sleep(35);
    Off(OUT_A+OUT_C);

    if(Direction == DOWN)
        i += 1;
    else if(Direction == RIGHT)
        j += 1;
    else if(Direction == UP)
        i -= 1;
    else if(Direction == LEFT)
        j -= 1;

    // Finally, we update our stack
    lastmove = lastmove2;
    lastmove2 = lastmove3;
    lastmove3 = lastmove4;
}
 

/*********************************************
* TurnLeft()
**********
* This subroutine will rotate the robot 90° to the left, by looking for the gray calibrating line
**********************************************/
// TurnLeft() is INLINE to enable multitasking
inline TurnLeft()
{
    // Direction = (Direction + 3) % 4;
    // We can't do MOD, so we AND by 3
    Direction = (Direction + 3) & 0x0003;
    Fwd(OUT_C, TURN_SPEED);
    Rev(OUT_A, TURN_SPEED);
    // give it time to move off the current line
    Sleep(HEADSTART);
    wait(LIGHT > GRAYLOW);
    Off(OUT_A + OUT_C);
    Sleep(100);
}
 

/*********************************************
* TurnRight()
**********
* This subroutine will rotate the robot 90° to the right, by looking for the gray calibrating line
**********************************************/
// TurnRight() is INLINE to enable multitasking
inline TurnRight()
{
    // Direction = (Direction + 3) % 4;
    // We can't do MOD, so we AND by 3
    Direction = (Direction + 3) & 0x0003;
    Fwd(OUT_A, TURN_SPEED);
    Rev(OUT_C, TURN_SPEED);
    // give it time to move off the current line
    Sleep(HEADSTART);
    wait(LIGHT > GRAYLOW);
    Off(OUT_A + OUT_C);
}
 

/*********************************************
* Turn180()
**********
* This subroutine will rotate the robot 180°
**********************************************/
// Turn180() is INLINE to enable multitasking
inline Turn180()
{
    Direction = (Direction + 2) & 0x0003;
    Fwd(OUT_A, TURN_SPEED);
    Rev(OUT_C, TURN_SPEED);
    // give it time to move off the current line
    Sleep(HEADSTART);
    wait(LIGHT > GRAYLOW);
    // Now we want to skip the first line, and wait for the second.
    Sleep(HEADSTART);
    wait(LIGHT > GRAYLOW);
    Off(OUT_A + OUT_C);
}
 

/*********************************************
* UpdateMap()
**********
* Updates the internal bitmap representation of our maze, as well as our current coordinate variables.
* Also pushes a move onto our stack.
**********************************************/
sub UpdateMap()
{
    if(Direction == DOWN)
        i += 1;
    else if(Direction == RIGHT)
        j += 1;
    else if(Direction == UP)
        i -= 1;
    else if(Direction == LEFT)
        j -= 1;

    // bitmap += 1 << (4*i + (3-j));
    tempExp = (4*i + (3-j));
    tempBitmap = 1;
    repeat (tempExp) tempBitmap *= 2;
    bitmap += tempBitmap;

    // update the stack
    lastmove4 = lastmove3;
    lastmove3 = lastmove2;
    lastmove2 = lastmove;
    lastmove = Direction;
}
 

/*********************************************
* SetupStuff()
**********
* This subroutine will initialize our variables, activate the sensors, and allow us to manually
* set the values for WHITE, GRAY, and BLACK.
**********************************************/
sub SetupStuff()
{
    i = 0; j = 0;
    // mark that we've visited the starting square
    bitmap = 8; //8 = 0000 0000 1000
    Direction = DOWN;
    SensorType(LIGHT,STYPE_LIGHT);
    SensorMode(LIGHT,SMODE_RAW);
    SensorType(FRONT,STYPE_SWITCH);
    SensorMode(FRONT,SMODE_RAW);

   // since ambient light is highly variable, we want to be able to define our color
   // values each time the program is run.  The following values have been
   // determined from trial and error.  The commented out code can be activated
   // to allow the robot to take readings when it's run
   Display(1);
/*
   wait(FRONT < 300);
*/
   WHITELOW = 650; WHITEHI = 687;
   //WHITE = LIGHT - LIGHT_SENSOR_OFFSET;
/*
   PlaySound(0);
   Sleep(50);
*/
//      wait(FRONT < 300);

   GRAYLOW = 708;
   GRAYHI = 735;
//   GRAYLOW = LIGHT - LIGHT_SENSOR_OFFSET;

//   PlaySound(0);
    Sleep(100);
/*
      wait(FRONT < 300);
*/
   BLACKLOW = 750;  BLACKHI = 790;
   //BLACK = LIGHT - LIGHT_SENSOR_OFFSET;
/*
   PlaySound(0);
   Sleep(50);
*/
}
 
 
 

/*********************************************
* MarkPath
**********************************************
* This task will update the bitmap every time a black gridline is passed.
*********************************************/
task MarkPath
{
    while (true)
    {
        wait(LIGHT > BLACKLOW);
        UpdateMap();
        PlaySound(1);
        // give it a sec to move off the line
        Sleep(30);
    }
}
 

/*********************************************
* FollowLine
**********************************************
* This task will keep the robot traveling in a straight line when it is moving forward.
*********************************************/
task FollowLine
{
  // What we need is to follow the gray line formed by the inner calibrating
  // grids, and watch out for white. Black is okay, this just means a grid line.

    int avLight;

    while(true)
    {
        avLight = 0;
        repeat (NUMREADS) avLight += LIGHT;
        avLight /= NUMREADS;
        if (avLight < GRAYLOW)
        {
           PlaySound(0);
           // We're off track.  Check to the right.
           Fwd(OUT_A, TURN_SPEED);
           Rev(OUT_C, TURN_SPEED);
           ClearTimer(0);
            wait( (LIGHT > GRAYLOW) || (Timer(0) > 1) );
           Off(OUT_A+OUT_C);
            if (LIGHT < GRAYLOW)
            {
                // Still lost.  Go back the other way.
              Fwd(OUT_C, TURN_SPEED);
              Rev(OUT_A, TURN_SPEED);
              ClearTimer(0);
              wait( (LIGHT > GRAYLOW) || (Timer(0) > 2) );
              Off(OUT_A+OUT_C);
              if (LIGHT < GRAYLOW)
                    // It's time to manually re-align.
                  wait(BUTTON < 300);
            }
           Fwd(OUT_A+OUT_C, DRIVE_SPEED);
        }
    }
}