#pragma noinit
//PROXIMITY DEFINITIONS
#define IDLE 0
#define THRESHOLD 110
#define TRANSMITTING 1
//RIGHT SIDE MOTOR DEFINITIONS
#define RIGHT_DRIVE OUT_A
#define RIGHT_FWD OUT_FWD
#define RIGHT_REV OUT_REV
//LEFT SIDE MOTOR DEFINITIONS
#define LEFT_DRIVE OUT_C
#define LEFT_FWD OUT_FWD
#define LEFT_REV OUT_REV
//SENSOR DEFINITIONS
#define BACK_BUMP SENSOR_3
#define BACK_BUMP_FIRING 1
#define BACK_BUMP_NOT_FIRING 0
#define FRONT_BUMP SENSOR_1
#define FRONT_BUMP_FIRING 0
#define FRONT_BUMP_NOT_FIRING 1
#define PROXY SENSOR_2
//DRIVING INSTRUCTIONS (ordered by priority, lowest num is lowest proiority)
#define FORWARD 0
#define TURN_AROUND 1  
#define GO_LEFT 2
#define GO_RIGHT 3
#define BACKWARD 4
//TIMERS (0-3)
#define INACTIVITY_TIMER 1  //used to measure amount of time with no sensory input
#define FRONT_SENSOR_TIMER 2  //used to clear old sensor firings
#define BACK_SENSOR_TIMER 3  //used to clear old sensor firings

//GLOBALS
int driving_instructions;
int NEAR_FRONT_EDGE;
int NEAR_BACK_EDGE;
//proxy stuff
int IN_PROXIMITY, IN_AMBIENT;
int ClockPhase;
int ThisLight;
int LastLight;
int DiffLight;

task main(){

  //Configure the sensors
    //Bumper sensors
  SetSensor(BACK_BUMP,SENSOR_TOUCH);
  SetSensor(FRONT_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 + LEFT_DRIVE);
  SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_HALF-2);
  SetDirection(RIGHT_DRIVE, RIGHT_FWD);
  SetDirection(LEFT_DRIVE, LEFT_FWD);
  ClearTimer(INACTIVITY_TIMER);  
  ClearTimer(FRONT_SENSOR_TIMER);
  ClearTimer(BACK_SENSOR_TIMER);
  //start driving
  driving_instructions = FORWARD;
  start MainDriveMechanism;
  start FrontBumpMonitor;
  start BackBumpMonitor;
  //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(400);
        SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_HALF-2);
        break;
      case TURN_AROUND:
        ClearTimer(INACTIVITY_TIMER);
        SetDirection(LEFT_DRIVE, LEFT_REV);
        SetDirection(RIGHT_DRIVE, RIGHT_FWD);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(100);
        SetDirection(RIGHT_DRIVE, LEFT_FWD);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        driving_instructions=FORWARD;
        ClearTimer(INACTIVITY_TIMER);
        SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_HALF-2);
        break;
      case BACKWARD:
        ClearTimer(INACTIVITY_TIMER);
        SetDirection(RIGHT_DRIVE, RIGHT_REV);
        SetDirection(LEFT_DRIVE, LEFT_REV);
        On(RIGHT_DRIVE + LEFT_DRIVE);
        Wait(400);
        driving_instructions=FORWARD;
        ClearTimer(INACTIVITY_TIMER);
        SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_HALF-2);
        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;
        ClearTimer(INACTIVITY_TIMER);
        SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_HALF-2);
        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;
        ClearTimer(INACTIVITY_TIMER);
        SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_HALF-2);
        break;  
    }
  }
}

task FrontBumpMonitor()
{
  while(true)
  {
    if(FRONT_BUMP==FRONT_BUMP_NOT_FIRING) //bumper still on (no contact)
    {
      if( Timer(FRONT_SENSOR_TIMER) > 20)
      {
        NEAR_FRONT_EDGE=0;
      }
    }else if(NEAR_BACK_EDGE==1){  //this check needed becuase tipping backward will fire the front sensor
    }else
    {  //we are heading off the table from the front so back up
      driving_instructions=BACKWARD;
      SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_FULL);
      ClearTimer(FRONT_SENSOR_TIMER);
      if(NEAR_FRONT_EDGE!=1){
        NEAR_FRONT_EDGE=1;
        PlayTone(1000,3);
        stop MainDriveMechanism;
        Off(RIGHT_DRIVE + LEFT_DRIVE);
        SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_FULL);
        driving_instructions=BACKWARD;
        start MainDriveMechanism;
      }
    }
  }
}

task BackBumpMonitor()
{
  while(true)
  {
    if(BACK_BUMP==BACK_BUMP_NOT_FIRING) //bumper still on (no contact)
    {
      if( Timer(BACK_SENSOR_TIMER) > 20)
      {
        NEAR_BACK_EDGE=0;
      }
    }else if(NEAR_FRONT_EDGE==1){  //this check needed becuase tipping forward will fire the back sensor
    }else  //we are heading off the table from the back so go forward
    {
      driving_instructions=FORWARD;
      SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_FULL);
      ClearTimer(BACK_SENSOR_TIMER);
      if(NEAR_BACK_EDGE!=1){
        NEAR_BACK_EDGE=1;
        PlayTone(1000,3);
        stop MainDriveMechanism;    
        Off(RIGHT_DRIVE + LEFT_DRIVE);
        SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_FULL);
        driving_instructions=FORWARD;
        start MainDriveMechanism;
      }
    }
  }
}

task ProxyMonitor()
{
  while(true)
  {
    if(IN_PROXIMITY>0)  //proxy has been triggered so drive forward to push others off
    {       
      PlayTone(1000,3);     
      if((NEAR_FRONT_EDGE==0) &&(driving_instructions>FORWARD)){  //we only want to go forward if the we arent over the edge
        stop MainDriveMechanism;
        Off(RIGHT_DRIVE + LEFT_DRIVE);
        SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_FULL);
        driving_instructions=FORWARD;
        start MainDriveMechanism;
       }
    }else  SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_HALF-2);
  }
}

task TimerMonitor()
{
  ClearTimer(INACTIVITY_TIMER);
  while(true)
  {
    if( (Timer(INACTIVITY_TIMER) > 80)  && (IN_PROXIMITY<=0) &&(NEAR_FRONT_EDGE==0) &&(NEAR_BACK_EDGE==0) ) //time to change directions if we arent pushing
    {
      PlayTone(2000,1);
      stop MainDriveMechanism;
      Off(RIGHT_DRIVE + LEFT_DRIVE);
      driving_instructions=1+ Random(3);  //select a random direction to troll for others
      ClearTimer(INACTIVITY_TIMER);
      SetPower(RIGHT_DRIVE + LEFT_DRIVE, OUT_HALF-2);
      start MainDriveMechanism;
    }
  }
}

/**
* 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
**/