#define RIGHT_SENSOR SENSOR_1
#define LEFT_SENSOR SENSOR_4
#define RIGHT_MOTOR OUT_A
#define LEFT_MOTOR OUT_C
#define BOTH_MOTORS OUT_AC
#define POWER 100
#define NOMINAL_TURN_TIME 200
#define TIME_TO_RANDOM_TURN 2000
#define NARROW_HALLWAY_TIME 1000

mutex moveMutex;

inline void advanceImpactTimes(long &impact1, long &impact2, long &impact3, long &impact4) {
	impact1=impact2;
	impact2=impact3;
	impact3=impact4;
	impact4=CurrentTick();
}

inline void resetImpactTimes(long &impact1, long &impact2, long &impact3, long &
impact4) {
	impact1=0;
	impact2=5000;
	impact3=10000;
	impact4=15000;
}

inline void backUp(int time) {
   OnRev(OUT_AC,POWER);
   Wait(time);
}
inline void turnLeft(int time) {
   OnFwd(RIGHT_MOTOR,POWER);
   OnRev(LEFT_MOTOR,POWER);
   Wait(time);
}
inline void turnRight(int time) {
   OnFwd(LEFT_MOTOR,POWER);
   OnRev(RIGHT_MOTOR,POWER);
   Wait(time);
}

task checkSensors() {

     //Set the default turn time
     int turnTime = NOMINAL_TURN_TIME;
     
     //Setup four impact timestamps
     long impact1,impact2,impact3,impact4;
     
     //Set the impact timestamps to known values which won't affect the program
     resetImpactTimes(impact1,impact2,impact3,impact4);

     while(true) {
          //Do this when the right sensor is pressed
          if(RIGHT_SENSOR == 1) {
               //Move all the impact times back one step
               advanceImpactTimes(impact1,impact2,impact3,impact4);
               
               //check for corner situation
               if((impact4-impact1)<TIME_TO_RANDOM_TURN) {
                    //Notify the user that corner escape was activated
                    PlayTone(750,500);
                    
                    //Set a random turn time between 300 and 500 (about 90 degrees and 150 degrees)
                    turnTime = Random(200)+NOMINAL_TURN_TIME+100;
                    
                    //Reset as if we are now in a new maze since we (hopefully) turned completely around
                    resetImpactTimes(impact1,impact2,impact3,impact4);
	             }
               else { //assign default turn time
                 	  turnTime = NOMINAL_TURN_TIME;
               }
               Acquire(moveMutex);
               backUp(300);
               turnLeft(turnTime);
               Release(moveMutex);
          }
          if(LEFT_SENSOR == 1) {
               //Move all the impact times back one step
               advanceImpactTimes(impact1,impact2,impact3,impact4);
               //check for corner situation
	             if((impact4-impact1)<TIME_TO_RANDOM_TURN) {
                    //Notify the user that corner escape was activated
                    PlayTone(750,500);
                    
                    //Set a random turn time between 300 and 500 (about 90 degrees and 150 degrees)
                    turnTime = Random(200)+NOMINAL_TURN_TIME+100;
                    
                    //Reset as if we are now in a new maze since we (hopefully) turned completely around
                    resetImpactTimes(impact1,impact2,impact3,impact4);
	             }
               else { //assign default turn time
                    turnTime = NOMINAL_TURN_TIME;
               }
               Acquire(moveMutex);
               backUp(300);
               turnRight(turnTime);
               Release(moveMutex);
          }
     }
}
task moveForward() {
     while(true) {
         Acquire(moveMutex);
         OnFwd(BOTH_MOTORS,POWER);
         Release(moveMutex);
     }
}

task main() {
     //Intialize sensors
     SetSensorTouch(IN_1); //right sensor
     SetSensorTouch(IN_4); //left sensor
     
     //setup tasks
     Precedes(checkSensors,moveForward);
}