// A simple autonomous robot with 4-steps repeat path avoidance + IR remote control

#define IR_RIGHT   18   // Right IR sensor on Pin 18
#define IR_LEFT    19   // Left  IR sensor on Pin 19

#define PATH_NUM   4    // Total no. of recorded steps
#define LEFT       100  // Just a value to indicate turning left
#define RIGHT      200  // Just a value to indicate turning right

#include "motor.h"


int Path[PATH_NUM];     // Storage for steps
int p_idx;              // index to Path[] array

main()
{
   writes("auto", 500);
   reset_path();
   loop {
     switch(ircode()) {
     case 4: turn_left(); break;
     case 6: turn_right(); break;
     case 8: drive(-1, -1, 1000); break;    // backward
     case 0: stop_all(); writes("end", 2000); exit(); break;  // stop & exit
     }
     
     if(same_path()) {
        writes("same", 500);
        if(Path[p_idx] == LEFT) { turn_left(); turn_left(); } else { turn_right(); turn_right(); }
     }

     drive(1, 1, 0); // step forward, smaller delay value --> faster response to sensors

     if(!pinstate(IR_LEFT) &&  pinstate(IR_RIGHT)) { path(RIGHT); turn_right(); } // obstacle on left
     if( pinstate(IR_LEFT) && !pinstate(IR_RIGHT)) { path(LEFT);  turn_left();  } // obstacle on right
     if(!pinstate(IR_LEFT) && !pinstate(IR_RIGHT)) drive(-1, -1, 2000);           // obstacle ahead, go backward
   }
}

turn_left()
{
   drive(-1, -1, 500); // backward
   drive(-1,  1, 500); // turn left
}

turn_right()
{
   drive(-1, -1, 500); // backward
   drive( 1, -1, 500); // turn right
}

path(dir)
{ // record path
   p_idx++;
   p_idx %= PATH_NUM;
   Path[p_idx] = dir;
}

same_path()
{
   if(Path[0] != Path[1] && Path[0] == Path[2] && Path[1] == Path[3]) {
      reset_path();
      return(1);
   }
   else return(0);
}

reset_path()
{
   int   i;

   for(i=0; i < PATH_NUM; i++) {
      Path[i] = i;
   }
   p_idx = -1;
}