/*
Car controled using subsumtion architecture
Version for the Small Car
*/

// ROTATION: Rotation Sensor for Steering
// FRONTBUMPER: Front Bumper
// BACKBUMPER: Back Bumper
#define ROTATION SENSOR_3
#define FRONTBUMPER SENSOR_2
#define BACKBUMPER SENSOR_1

// DRIVE: Drive Motor
// STEER: Steering Motor
#define DRIVE OUT_A
#define STEER OUT_B

#define REV OUT_REV
#define FWD OUT_FWD
// Before start set the steering motor to straight forward direction

#define MAX_LEFT 17
#define HALF_LEFT 8
#define MIN_RIGHT -17
#define HALF_RIGHT -8
#define STRAIGHT 0
#define LEFT OUT_REV
#define RIGHT OUT_FWD

#define COMMAND_NONE     0
#define COMMAND_FORWARD  1
#define COMMAND_REVERSE  2
#define COMMAND_LEFT     4
#define COMMAND_RIGHT    8
#define COMMAND_STOP    16
#define COMMAND_FLOAT   32
#define COMMAND_NOSTEER 64

int cruiseCommand = COMMAND_NONE;
int frontCommand = COMMAND_NONE;
int backCommand = COMMAND_NONE;
int motorCommand = COMMAND_NONE;
int t = 0;


task beeper(){
  while( true ) {
    PlaySound( SOUND_DOUBLE_BEEP );
    Wait(100);
  }
}

sub motorControl(){
  t = motorCommand & COMMAND_FORWARD;
  if ( t > 0 ) {
//     stop beeper;
     SetDirection( DRIVE, FWD );
     On( DRIVE);
  }
  t = motorCommand & COMMAND_REVERSE;
  if ( t > 0 ) {
    SetDirection( DRIVE, REV );
    On( DRIVE );
//    start beeper;
  }
  t = motorCommand & COMMAND_LEFT;
  if ( t > 0 ) {
    SetDirection( STEER, LEFT );
    On( STEER );
  }
  t = motorCommand & COMMAND_RIGHT;
  if ( t > 0 ) {
    SetDirection( STEER, RIGHT );
    On( STEER );
  }
  t =  motorCommand & COMMAND_FLOAT;
  if ( t > 0 ) {
      Float( DRIVE );
  }
  t = motorCommand & COMMAND_STOP;
  if ( t > 0 ) {
      Off( DRIVE );
  }
  t = motorCommand & COMMAND_NOSTEER;
  if ( t > 0 ) {
      Off( STEER );
  }
  
}

task cruise(){
  while ( true )
    cruiseCommand = COMMAND_FORWARD;
}

task avoidFront(){
  while( true) {
    if ( FRONTBUMPER == 1 ){
       frontCommand = COMMAND_REVERSE | COMMAND_LEFT;
       while( ROTATION < HALF_LEFT );
       frontCommand = COMMAND_REVERSE | COMMAND_NOSTEER;
       frontCommand = COMMAND_FLOAT | COMMAND_RIGHT;
       while( ROTATION > STRAIGHT );
       frontCommand = COMMAND_FORWARD | COMMAND_NOSTEER;
       Wait(20);
    } else
      frontCommand = COMMAND_NONE;
  }
}

task avoidBack(){
  while( true) {
    if ( BACKBUMPER == 1 ){
       if ( FRONTBUMPER == 0) {
          PlaySound( SOUND_DOUBLE_BEEP );
          backCommand = COMMAND_STOP;
          Wait(20);
          if ( ROTATION > 0 )
             backCommand |= COMMAND_RIGHT;
          else if (ROTATION < 0)
             backCommand |= COMMAND_LEFT;
          else if(ROTATION == 0)
             backCommand |= COMMAND_NOSTEER;
          while( ROTATION != 0 );
          backCommand = COMMAND_FORWARD | COMMAND_NOSTEER;
          until ( BACKBUMPER == 0 );
          Wait(20);
       }
    } else
      backCommand = COMMAND_NONE;
  }
}

task arbitrate(){
  while( true ){
    if ( cruiseCommand != COMMAND_NONE ) motorCommand = cruiseCommand;
    if ( frontCommand != COMMAND_NONE ) motorCommand = frontCommand;
    if ( backCommand != COMMAND_NONE ) motorCommand = backCommand;    
    motorControl();
  }
}

sub init() {
  SetSensor( ROTATION   , SENSOR_ROTATION );
  ClearSensor( ROTATION );
  SetSensor( FRONTBUMPER, SENSOR_TOUCH );
  SetSensor( BACKBUMPER, SENSOR_TOUCH );
  SetPower( DRIVE + STEER, 9 );

  cruiseCommand = COMMAND_NONE;
  frontCommand = COMMAND_NONE;
  backCommand = COMMAND_NONE;
}

task main(){
  init();
  start cruise;
  start avoidFront;
  start avoidBack;
  
  start arbitrate;
}