#pragma noinit
//RIGHT SIDE MOTOR DEFINITIONS
#define RIGHT_DRIVE OUT_A
#define RIGHT_FWD OUT_REV
#define RIGHT_REV OUT_FWD
//LEFT SIDE MOTOR DEFINITIONS
#define LEFT_DRIVE OUT_C 
#define LEFT_FWD OUT_FWD
#define LEFT_REV OUT_REV
//DRIVING INSTRUCTIONS (ordered by priority, lowest num is lowest proiority)
#define FORWARD 0
#define TURN_AROUND 1  
#define BACKWARD 2
#define GO_LEFT 3
#define GO_RIGHT 4
#define CLEAR_JAM 5
//TIMERS (0-3)
#define INACTIVITY_TIMER 1  //used to measure amount of time with no sensory input

//GLOBALS
int driving_instructions;
int clearing_mechanism;

task main(){

  //Configure the Motors
  Off(RIGHT_DRIVE + LEFT_DRIVE);
  SetPower(RIGHT_DRIVE +LEFT_DRIVE, OUT_FULL);
  SetDirection(RIGHT_DRIVE, RIGHT_FWD);
  SetDirection(LEFT_DRIVE, LEFT_FWD);
  ClearTimer(INACTIVITY_TIMER);  
  //start driving
  driving_instructions = FORWARD;
  clearing_mechanism=BACKWARD;
  start MainDriveMechanism;
  start TimerMonitor;
      
}

task MainDriveMechanism()
{
  while(true)
  {
    switch(driving_instructions)
    {
      case FORWARD:
        SetDirection(RIGHT_DRIVE, RIGHT_FWD);
        SetDirection(LEFT_DRIVE, LEFT_FWD);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(300);
        break;
      case TURN_AROUND:
        ClearTimer(INACTIVITY_TIMER);
        SetDirection(LEFT_DRIVE, LEFT_FWD);
        SetDirection(RIGHT_DRIVE, RIGHT_REV);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(500);
        SetDirection(RIGHT_DRIVE, RIGHT_FWD);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        driving_instructions=FORWARD;
        ClearTimer(INACTIVITY_TIMER);
        break;
      case BACKWARD:
        SetDirection(RIGHT_DRIVE, RIGHT_REV);
        SetDirection(LEFT_DRIVE, LEFT_REV);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(300);
        driving_instructions=FORWARD;
        break;
      case GO_LEFT:
        ClearTimer(INACTIVITY_TIMER);
        SetDirection(LEFT_DRIVE, LEFT_REV);
        SetDirection(RIGHT_DRIVE, RIGHT_FWD);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(200);
        driving_instructions=FORWARD;
        break;
      case GO_RIGHT:
        ClearTimer(INACTIVITY_TIMER);
        SetDirection(LEFT_DRIVE, LEFT_FWD);
        SetDirection(RIGHT_DRIVE, RIGHT_REV);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(200);
        driving_instructions=FORWARD;
        break;  
      case CLEAR_JAM:
        ClearTimer(INACTIVITY_TIMER);
        SetDirection(LEFT_DRIVE, LEFT_REV);
        SetDirection(RIGHT_DRIVE, RIGHT_REV);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(500);
        SetDirection(LEFT_DRIVE, LEFT_FWD);
        SetDirection(RIGHT_DRIVE, RIGHT_REV);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(200);
        ClearTimer(INACTIVITY_TIMER);
        SetDirection(LEFT_DRIVE, LEFT_REV);
        SetDirection(RIGHT_DRIVE, RIGHT_FWD);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(100);
        driving_instructions=BACKWARD;
        ClearTimer(INACTIVITY_TIMER);
        break;       
    }
  }
}


task TimerMonitor()
{
  ClearTimer(INACTIVITY_TIMER);
  while(true)
  {
    if(( Timer(INACTIVITY_TIMER) > 40)&&( clearing_mechanism==BACKWARD)  ) //we may be stuck
    {
      PlayTone(2000,1);
      clearing_mechanism=FORWARD;
      stop MainDriveMechanism;
      Off(RIGHT_DRIVE + LEFT_DRIVE);
      driving_instructions=Random(5);
      start MainDriveMechanism;
    }
    if(( Timer(INACTIVITY_TIMER) > 80) &&( clearing_mechanism==FORWARD)) //we may still be stuck
    {
      ClearTimer(INACTIVITY_TIMER);
      PlayTone(5000,1);
      clearing_mechanism=BACKWARD;
      stop MainDriveMechanism;
      Off(RIGHT_DRIVE + LEFT_DRIVE);
      driving_instructions=FORWARD;
      start MainDriveMechanism;
    }
  }
}