#pragma noinit
//PROXIMITY DEFINITIONS
#define IDLE 0
#define THRESHOLD 150
#define TRANSMITTING 1
//RIGHT SIDE MOTOR DEFINITIONS
#define RIGHT_DRIVE OUT_A
#define RIGHT_FWD OUT_REV
#define RIGHT_REV OUT_FWD
//LIFTING ARM DEFINITIONS
#define LIFT_ARM OUT_B
#define LIFT_UP OUT_FWD
#define LIFT_DOWN OUT_REV
//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 PROXY SENSOR_2
//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;
//proxy stuff
int IN_PROXIMITY, IN_AMBIENT;
int ClockPhase;
int ThisLight;
int LastLight;
int DiffLight;

task main(){

  //Configure the sensors
    //Bumper sensors
  SetSensor(RIGHT_BUMP,SENSOR_TOUCH);
  SetSensor(LEFT_BUMP,SENSOR_TOUCH); 
    //Proximity sensor
  SetSensorType(PROXY, SENSOR_TYPE_LIGHT);
  SetSensorMode(PROXY, SENSOR_MODE_RAW);
  SetTxPower(TX_POWER_HI);
  //Configure the Motors
  Off(RIGHT_DRIVE + LIFT_ARM + LEFT_DRIVE);
  SetPower(RIGHT_DRIVE + LIFT_ARM + LEFT_DRIVE, OUT_FULL);
  SetDirection(RIGHT_DRIVE, RIGHT_FWD);
  SetDirection(LIFT_ARM, LIFT_UP);
  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;
  start MainDriveMechanism;
  start LeftBumpMonitor;
  start RightBumpMonitor;
  //start up the proxy sensor
  start TransmitterClock;
  start ProximityDriver;
  Wait(100);  //let the proxy warm up
  start ProxyMonitor;
  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(150);
        LiftArm();
        Wait(300);
        LowerArm();
        break;
      case TURN_AROUND:
        ClearTimer(INACTIVITY_TIMER);
        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);
        LiftArm();
        Wait(300);
        LowerArm();
        driving_instructions=FORWARD;
        ClearTimer(INACTIVITY_TIMER);
        break;
      case BACKWARD:
        ClearTimer(INACTIVITY_TIMER);
        LiftArm();
        SetDirection(RIGHT_DRIVE, RIGHT_REV);
        SetDirection(LEFT_DRIVE, LEFT_REV);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(300);
        LowerArm();
        driving_instructions=FORWARD;
        ClearTimer(INACTIVITY_TIMER);
        break;
      case GO_LEFT:
        ClearTimer(INACTIVITY_TIMER);
        SetDirection(LEFT_DRIVE, LEFT_REV);
        SetDirection(RIGHT_DRIVE, RIGHT_FWD);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(200);
        LiftArm();
        LowerArm();
        driving_instructions=FORWARD;
        ClearTimer(INACTIVITY_TIMER);
        break;
      case GO_RIGHT:
        ClearTimer(INACTIVITY_TIMER);
        SetDirection(LEFT_DRIVE, LEFT_FWD);
        SetDirection(RIGHT_DRIVE, RIGHT_REV);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(200);
        LiftArm();
        LowerArm();
        driving_instructions=FORWARD;
        ClearTimer(INACTIVITY_TIMER);
        break;  
      case CLEAR_JAM:
        ClearTimer(INACTIVITY_TIMER);
        LiftArm();
        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);
        LowerArm();
        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 LeftBumpMonitor()
{
  while(Timer(LEFT_SIDE_SENSOR_TIMER) < 150)
  {
    if(LEFT_BUMP==1) //bumper still on (no contact)
    {
      ClearTimer(LEFT_SIDE_SENSOR_TIMER);
    }
    else //we are being touched on the left so avoid it if not doing something else
    {
      if(driving_instructions< GO_RIGHT)
      {
        stop MainDriveMechanism;
        Off(RIGHT_DRIVE + LIFT_ARM + LEFT_DRIVE);
        driving_instructions=GO_RIGHT;
        start MainDriveMechanism;
      }
    }
  }
}

task RightBumpMonitor()
{
   while(Timer(RIGHT_SIDE_SENSOR_TIMER) < 150)
  {
    if(RIGHT_BUMP==1) //bumper still on (no contact)
    {
      ClearTimer(RIGHT_SIDE_SENSOR_TIMER);
    }
    else //we are being touched on the right so avoid it if not doing something else
    {
      if(driving_instructions< GO_LEFT) 
      {
        stop MainDriveMechanism;
        Off(RIGHT_DRIVE + LIFT_ARM + LEFT_DRIVE);
        driving_instructions=GO_LEFT;
        start MainDriveMechanism;
      }
    }
  }
}

task ProxyMonitor()
{
  while(true)
  {
    if(IN_PROXIMITY>0)  //proxy has been triggered so turnaround if not doing something else
    {              
      if(driving_instructions< TURN_AROUND)
      {
        PlayTone(1000,3);
        stop MainDriveMechanism;
        Off(RIGHT_DRIVE + LIFT_ARM + LEFT_DRIVE);
        driving_instructions=TURN_AROUND;
        start MainDriveMechanism;
      }
    }
  }
}

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

sub LiftArm() {
  SetDirection(LIFT_ARM, LIFT_UP);
  OnFor(LIFT_ARM, 175);  //??? right number???
}

sub LowerArm() {
  SetDirection(LIFT_ARM, LIFT_DOWN);
  OnFor(LIFT_ARM, 175);  //??? right number???
  Float(LIFT_ARM);
  Wait(50);
  Off(LIFT_ARM);  
}

/**
* START PROXIMITY SENSOR CODE
**/ 

task TransmitterClock()
{
  while (true) { //20 per second
    SendMessage(255); //takes about 4 100ths of a second
    ClockPhase = TRANSMITTING;
    Wait(3); //guaranteed to be shorter than the pulse?
    ClockPhase = IDLE;
    Wait(2); //makes a nice round number
  }
}

task ProximityDriver()
{
  while (true) {
    while (ClockPhase==IDLE) {
    } //while
    IN_PROXIMITY = 0;
    while (ClockPhase==TRANSMITTING) {
    LastLight = ThisLight;
    ThisLight = PROXY;
    DiffLight = LastLight;
    DiffLight -= ThisLight; //LastLight-ThisLight
    if (DiffLight > THRESHOLD) { //an IR reflection
      IN_PROXIMITY += 1;
      IN_AMBIENT = LastLight;
   } else {
      IN_AMBIENT = ThisLight;
    }
  } //while
  } //while
}

/**
* END PROXIMITY SENSOR CODE
**/