#pragma noinit
//RIGHT SIDE MOTOR DEFINITIONS
#define RIGHT_DRIVE OUT_A
#define RIGHT_FWD OUT_REV
#define RIGHT_REV OUT_FWD
//LIFTING ARM DEFINITIONS
#define OFFENSE OUT_B
#define TRACKS OUT_REV
#define JOUST OUT_FWD
//LEFT SIDE MOTOR DEFINITIONS
#define LEFT_DRIVE OUT_C
#define LEFT_FWD OUT_FWD
#define LEFT_REV OUT_REV
//SENSOR DEFINITIONS
#define RIGHT_BUMP SENSOR_1
#define LEFT_BUMP SENSOR_3
#define STALL_SENSOR SENSOR_2
#define STALL_THRESHOLD 3
//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
#define RIGHT_SIDE_SENSOR_TIMER 2  //used to check if the sensor is broke
#define LEFT_SIDE_SENSOR_TIMER 3  //used to check if the sensor is broke

//GLOBALS
int driving_instructions;
int clearing_mechanism;
int stall_value;
int track_count;

task main(){

  //Configure the sensors
    //Bumper sensors
  SetSensor(RIGHT_BUMP,SENSOR_TOUCH);
  SetSensor(LEFT_BUMP,SENSOR_TOUCH); 
    //Stall sensor
  SetSensorType(STALL_SENSOR, SENSOR_TYPE_LIGHT);
  SetSensorMode(STALL_SENSOR, SENSOR_MODE_RAW);
  //Configure the Motors
  Off(RIGHT_DRIVE + OFFENSE + LEFT_DRIVE);
  SetPower(RIGHT_DRIVE + OFFENSE + LEFT_DRIVE, OUT_FULL);
  SetDirection(RIGHT_DRIVE, RIGHT_FWD);
  SetDirection(OFFENSE, TRACKS);
  SetDirection(LEFT_DRIVE, LEFT_FWD);
  ClearTimer(RIGHT_SIDE_SENSOR_TIMER);
  ClearTimer(LEFT_SIDE_SENSOR_TIMER);
  ClearTimer(INACTIVITY_TIMER);  
  //start driving
  driving_instructions = FORWARD;
  clearing_mechanism=BACKWARD;
  track_count=0;
  useTracks();
  Wait(400);  //give me time to start it and set it down
  stall_value=STALL_SENSOR;
  start MainDriveMechanism;
  start LeftBumpMonitor;
  start RightBumpMonitor;
  start StallMonitor;
      
}

task MainDriveMechanism()
{
  while(true)
  {
    switch(driving_instructions)
    {
      case FORWARD:
        useTracks();
        SetDirection(RIGHT_DRIVE, RIGHT_FWD);
        SetDirection(LEFT_DRIVE, LEFT_FWD);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(300);
        break;
      case TURN_AROUND:
        useJoust();
        SetDirection(LEFT_DRIVE, LEFT_FWD);
        SetDirection(RIGHT_DRIVE, RIGHT_REV);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(100);
        SetDirection(RIGHT_DRIVE, RIGHT_FWD);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(300);
        driving_instructions=FORWARD;
        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:
        useTracks();
        SetDirection(LEFT_DRIVE, LEFT_REV);
        SetDirection(RIGHT_DRIVE, RIGHT_FWD);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(400);
        driving_instructions=FORWARD;
        break;
      case GO_RIGHT:
        useJoust();
        SetDirection(LEFT_DRIVE, LEFT_FWD);
        SetDirection(RIGHT_DRIVE, RIGHT_REV);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(400);
        driving_instructions=FORWARD;
        break;  
      case CLEAR_JAM:
        useJoust();
        if(clearing_mechanism==BACKWARD)
        {
          SetDirection(LEFT_DRIVE, LEFT_REV);
          SetDirection(RIGHT_DRIVE, RIGHT_REV);
          On(RIGHT_DRIVE + LEFT_DRIVE);
          Wait(200);
          SetDirection(LEFT_DRIVE, LEFT_FWD);
          SetDirection(RIGHT_DRIVE, RIGHT_REV);
          On(RIGHT_DRIVE + LEFT_DRIVE);
          Wait(200);
          SetDirection(LEFT_DRIVE, LEFT_REV);
          SetDirection(RIGHT_DRIVE, RIGHT_FWD);
          On(RIGHT_DRIVE + LEFT_DRIVE);
          Wait(100);
          clearing_mechanism=FORWARD;
          driving_instructions=BACKWARD;
        }
        else{
          SetDirection(LEFT_DRIVE, LEFT_FWD);
          SetDirection(RIGHT_DRIVE, RIGHT_FWD);
          On(RIGHT_DRIVE + LEFT_DRIVE);
          Wait(100);
          SetDirection(LEFT_DRIVE, LEFT_REV);
          SetDirection(RIGHT_DRIVE, RIGHT_REV);
          On(RIGHT_DRIVE + LEFT_DRIVE);
          Wait(100);
          clearing_mechanism=BACKWARD;
          driving_instructions=TURN_AROUND;
        }
        break;       
    }
  }
}

task LeftBumpMonitor()
{
  while(true)
  {
    if(LEFT_BUMP==1) //we are being touched on the left so avoid it
    {
      if(driving_instructions!=GO_RIGHT){
        useTracks();
        stop MainDriveMechanism;
        Off(RIGHT_DRIVE + LEFT_DRIVE);
        driving_instructions=GO_RIGHT;
        start MainDriveMechanism;
      }
    }
  }
}

task RightBumpMonitor()
{
   while(true)
  {
    if(RIGHT_BUMP==1) //we are being touched on the right so avoid it 
    {
      if(driving_instructions!=GO_LEFT){
        useTracks();
        stop MainDriveMechanism;
        Off(RIGHT_DRIVE + LEFT_DRIVE);
        driving_instructions=GO_LEFT;
        start MainDriveMechanism;
      }
    }
  }
}

task StallMonitor()
{
   while(true)
  {
    if(abs(STALL_SENSOR-stall_value) > STALL_THRESHOLD)
    {
      stall_value=STALL_SENSOR;
      ClearTimer(INACTIVITY_TIMER);
    }
    else if( Timer(INACTIVITY_TIMER) > 30)  //we may be stuck
    {
      PlayTone(2000,1);
      stop MainDriveMechanism;
      Off(RIGHT_DRIVE + LEFT_DRIVE);
      driving_instructions=CLEAR_JAM;
      start MainDriveMechanism;
      ClearTimer(INACTIVITY_TIMER);
    }
  }
}

sub useTracks() {
  if(track_count < 5)  //there is the possibility to get stuck on the wall moving enough to not fire iniactivity sensor; so turn off tracks every once and a while
  {
    track_count++;
    SetDirection(OFFENSE, TRACKS);
    On(OFFENSE);
  }
  else
  {
    track_count=0;
    SetDirection(OFFENSE, JOUST);
    On(OFFENSE); 
  }
}

sub useJoust() {
  SetDirection(OFFENSE, JOUST);
  On(OFFENSE); 
}