/* 
   Here is one possible way to implement a subsumption style
   architecture in Interactive C code.  This method is described in
   Mobile Robots by Jones and Flynn publised in 1993 by AK Peters.
 
   Each behavior has a pair of global variables associated with it: a
   command and a flag.  When the flag is true, the command represents
   the action chosen by the behavior.  An arbitrator gives control to
   the highest priority behavior whose flag is true.
 
   In this simple example, there are three behaviors: cruise which
   always wants to go forward, follow which turns towards light, and
   escape which backs and turns away from obstacles.  Cruise has the
   lowest priority and escape has the highest priority.  Each behavior
   is implemented as a separate IC process.

   Each process in IC is by default given 5 ticks.  This is enough time
   to complete multiple iterations of each behavior's loop.  To
   implement subsumption successfully, we must have each behavior give
   up control immediately after a single loop iteration.  To do this, a
   call to the function defer() is added at the end of each loop.  
   
   It is important to note that IC only seems to be able to handle a
   maximum of 4 processes running simulataneously.  This program uses 3
   processes, but it would be relatively easy to eliminate the cruise
   behavior so as to have more room for other more interesting behaviors.

*/

#define Llight 2
#define Rlight 0
#define Lmotor 3
#define Rmotor 1
#define Lbump 9
#define Rbump 7

#define FORWARD 0
#define BACKWARD 1
#define TURNLEFT 2
#define TURNRIGHT 3
#define BACKthenLEFT 4
#define BACKthenRIGHT 5

int cruise_command;
int cruise_output_flag;
int follow_command;
int follow_output_flag;
int escape_command;
int escape_output_flag;
int motor_input;

/* 
   The order in which the behavior results are checked determines the
   priority. The first checked have low priority and last checked have
   high priority. This is called in motor_driver.
*/
void arbitrate() {
  if (cruise_output_flag)
    motor_input = cruise_command;
  if (follow_output_flag)
    motor_input = follow_command;
  if (escape_output_flag)
    motor_input = escape_command;
}

void motor_driver(){
  while (1) {
    arbitrate();
    if (motor_input == FORWARD) {
      printf("FORWARD\n");
      motor(Lmotor, 100);
      motor(Rmotor, 100);
    }
    else if (motor_input == BACKWARD) {
      printf("BACKWARD\n");
      motor(Lmotor, -100);
      motor(Rmotor, -100);
    }
    else if (motor_input == TURNLEFT) { 
      printf("TURNLEFT\n");
      motor(Lmotor, -100);
      motor(Rmotor, 100);
    }
    else if (motor_input == TURNRIGHT) {
      printf("TURNRIGHT\n");
      motor(Lmotor, 100);
      motor(Rmotor, -100);
    }
    else if (motor_input == BACKthenLEFT) {
      printf("BACKthenLEFT\n");
      motor(Lmotor, -100);
      motor(Rmotor, -100);
      sleep(.6);
      motor(Lmotor, -100);
      motor(Rmotor, 100);
      sleep(.4);
    }
    else if (motor_input == BACKthenRIGHT) {
      printf("BACKthenRIGHT\n");
      motor(Lmotor, -100);
      motor(Rmotor, -100);
      sleep(.6);
      motor(Lmotor, 100);
      motor(Rmotor, -100);
      sleep(.4);          
    }
    else {
      printf("MOTORS OFF\n");
      motor(Lmotor, 0);
      motor(Rmotor, 0);
    }
  }
}

void cruise() {
  while(1) {
    cruise_command = FORWARD;
    cruise_output_flag = 1;
    defer();
  }
}

int abs(int value) {
  if (value >= 0)
    return(value);
  else return(-1*value);
}

void follow() {
  int Llightval, Rlightval;
  int delta, threshold;
    
  /* you should experiment with the proper threshold */
  threshold = 5;
  while(1) {
    Llightval = analog(Llight);
    Rlightval = analog(Rlight);
    delta = Rlightval - Llightval;
    if (abs(delta) > threshold) {
      if (delta > 0)
	follow_command = TURNLEFT;
      else
	follow_command = TURNRIGHT;
      follow_output_flag = 1;
    }
    else
      follow_output_flag = 0;
    defer();
  }
}

void escape() {
  int Lbumpval, Rbumpval;
  
  while (1) {
    Lbumpval = digital(Lbump);
    Rbumpval = digital(Rbump);
    if (Lbumpval && Rbumpval) {
      escape_command = BACKthenLEFT;
      escape_output_flag = 1;
    }
    else if (Lbumpval) {
      escape_command = BACKthenRIGHT;
      escape_output_flag = 1;
    }
    else if (Rbumpval) {
      escape_command = BACKthenLEFT;
      escape_output_flag = 1;
    }
    else
      escape_output_flag = 0;
    defer();
  }
}

/* Initiate all the processes. */
void main() {
  start_process(cruise());
  start_process(follow());
  start_process(escape());
  motor_driver();
}