/*
 *behavior.c
 *Andrew McDonnell and Patrick Vinograd
 *CS154 Extinguisher project
 */

int mode;
int SEARCH_MODE = 0;
int DESTROY_MODE = 1;

int BEHAVIOR_COUNT = 3;
long SLEEP_TIME = 1000L;

/*
 *behavior control vectors
 */
int behavior_vector[3];
int param_vector[3];
int process_vector[3];

/*
 *behaviors
 */
int AVOID  = 0;
int SEEK   = 1;
int WANDER = 2;

/*
 *actions
 */
int ACT_NOTHING    = 0;
int ACT_STOP       = 1;
int ACT_FORWARD    = 2;
int ACT_REVERSE    = 3;
int ACT_TURN       = 4;
int ACT_BACK_LEFT  = 5;
int ACT_BACK_RIGHT = 6;
int ACT_MOVE       = 7;
int ACT_SLEEP      = 8;

/*
 *turning factors
 */

int RIGHT_FACTOR   =  1;
int LEFT_FACTOR    = -1;
int PANIC_RIGHT_FACTOR =  2;
int PANIC_LEFT_FACTOR  = -2;

/*
 *sensors
 */

int LEFT_IR   = 255;
int RIGHT_IR  = 255;
int CENTER_IR = 255;

/*
 *speeds
 */

int LOW_SPEED    = 59;
int CRUISE_SPEED = 69;
int PANIC_SPEED  = 80;
int GOTTA_TAKE_A_DUMP_SPEED = 100;

void main() {
  printf("\nPress START");
  start_press();
  mode = SEARCH_MODE;
  start_process(manager());
}


float secs;
/*
 *The manager process arbitrates between the various behaviors by performing
 *the actions specified by the highest-priority active behavior.  
 */
void manager() {

  /* debug
     behavior_vector[AVOID] = ACT_NOTHING;
     behavior_vector[WANDER] = ACT_NOTHING;
     end debug */

  process_vector[AVOID]  = start_process(avoid_collision());
  process_vector[SEEK]   = start_process(seek_candle_new());
  process_vector[WANDER] = start_process(wander());


  while (mode == SEARCH_MODE) {
    int current_action = ACT_NOTHING;
    int parameter = 0;
    /*
     *find first behavior with non-null action
     */
    /*hog_processor();*/
    int i, j;
    for (i = 0; i < BEHAVIOR_COUNT; i++) {
      if (current_action != ACT_NOTHING)
	break;
      if (behavior_vector[i] != ACT_NOTHING) {
	current_action = behavior_vector[i];
	parameter = param_vector[i];
	break;
      }
    }
    /*for (j = 0; j < BEHAVIOR_COUNT; j++) {
      behavior_vector[j] = ACT_NOTHING;
      }*/
    if (current_action == ACT_NOTHING) {
      printf("\nACT_NOTHING (%d)", i);
      alloff();
    }
    else if (current_action == ACT_STOP) {
      printf("\nACT_STOP (%d)", i);
      alloff();
    }
    else if (current_action == ACT_FORWARD) {
      printf("\nACT_FORWARD (%d)", i);
      
      motor(1,parameter);
      motor(2,(parameter * 145/140));

    }
    else if (current_action == ACT_REVERSE) {
      printf("\nACT_REVERSE (%d)", i);
      motor(1,(-1 * parameter));
      motor(2,(-1 * parameter));

    }
    else if (current_action == ACT_TURN) {
      printf("\nACT_TURN (%d): %d", i, parameter);
      
      /*
	motor(2,parameter);
	motor(1,(-1 * parameter));
      */
      turn(parameter);    
    }
    else if (current_action == ACT_BACK_LEFT) {
      hog_processor();
      printf("\nACT_BACK_LEFT (%d): %d", i, (-1 * parameter));
      secs = seconds();
      while (seconds() < secs + 1.0) {
	motor(1, (-1 * CRUISE_SPEED));
	motor(2, (-1 * CRUISE_SPEED));
      }
      turn(-1 * parameter);
    }
    else if (current_action == ACT_BACK_RIGHT) {
      hog_processor();
      printf("\nACT_BACK_RIGHT (%d): %d", i, parameter);
      secs = seconds();
      while (seconds() < secs + 1.0) {
	motor(1, (-1 * CRUISE_SPEED));
	motor(2, (-1 * CRUISE_SPEED));
      }
      turn(parameter);
    }
    else if (current_action == ACT_MOVE) {
      printf("\nACT_MOVE (%d): %d", i, parameter);
      move(parameter);
      defer();
    }
    else if (current_action == ACT_SLEEP) {
      printf("\nACT_SLEEP (%d): %d", i, SLEEP_TIME);
      msleep(SLEEP_TIME);
    }
  }
  /*
   *done searching.  kill all processes and destroy.
   */
  
  for (i = 0; i < BEHAVIOR_COUNT; i++) {
    kill_process(process_vector[i]);
  }
  alloff();
  destroy();
}

int flame, pn;
void destroy() {
  pn = start_process(average_all_ir());
  printf("\nTARGET          ACQUIRED");
  tone(220.0, 0.5);
  tone(600.0, 0.5);
  tone(220.0, 0.5);
  flame = 1;
  while (flame) {
    printf("\nCENTER IR:%d", CENTER_IR);
    motor(3, 100);
    sleep(2.0);
    if (CENTER_IR > 240) {
      sleep(3.0);
      motor(3, 0);
      sleep(3.0);    
      printf("!");
      if (CENTER_IR > 240) {
	flame = 0;
	alloff();
	
      }
    }
  }
  printf("\nTARGET          ELIMINATED");
  tone(600.0, 0.5);
  tone(220.0, 0.5);
  tone(600.0, 0.5);
  kill_process(pn);
    
}

int j;
void pan() {
  for (j = 0; j < 3; j++) {
    turn(-10);
    turn(10);
  }
}

/*
 *The avoid_collision behavior monitors the bump sensors to watch for
 *collisions.  If a collision is detected, the robot will backup and turn.
 */
void avoid_collision() {
  while(1) {
    behavior_vector[AVOID] = ACT_NOTHING;

    if (left_bump() && right_bump()) {
      alloff();

      behavior_vector[AVOID] = ACT_BACK_LEFT;
      param_vector[AVOID]    = 60;
    }

    if (left_bump()) {
      alloff();

      behavior_vector[AVOID] = ACT_BACK_RIGHT;
      param_vector[AVOID]    = 30;
      defer();
    }

    if (right_bump()) {
      alloff();

      behavior_vector[AVOID] = ACT_BACK_LEFT;
      param_vector[AVOID]    = 30;
      defer();
    }
  }
}

/*
 *The wander behavior causes the robot to stay in continual motion.  
 */
void wander() {
  while(1) {
    behavior_vector[WANDER] = ACT_FORWARD;
    param_vector[WANDER]    = LOW_SPEED;
    defer();
  }
}

/*
 *The seek_candle behavior causes the robot to monitor IR sensors and tend
 *toward increased IR activity.  This behavior will also cause the robot to 
 *drop to DESTROY_MODE once the candle is found.  
 */

int happy;
int proc_num;

void seek_candle() {

  proc_num = start_process(average_all_ir());

  while(1) {

    behavior_vector[SEEK] = ACT_NOTHING;

    if (LEFT_IR < 240 && RIGHT_IR < 240) {
      /*
       *candle entered sensor range in front
       *stop all motors, then proceed to fan range
       */
      /*printf("\n!%d/%d//%d!", LEFT_IR, RIGHT_IR, CENTER_IR);
       */
      alloff();
      /*
	tone(100.0, 0.5);
	tone(100.0, 0.5);
	tone(220.0, 0.5);      
      */
      /* debug 
	 start_press();
	 end debug */
      
      happy = 1;
      behavior_vector[SEEK] = ACT_MOVE;
      param_vector[SEEK]    = 15;      

      while(happy) {
	if (((RIGHT_IR - LEFT_IR) > 35) ||
	    ((LEFT_IR - RIGHT_IR) > 35)) {
	  /*
	   *candle veered to right or left
	   *resume seeking
	   */
	  /*
	    tone(500.0, .5);
	    tone(500.0, .5);
	    tone(220.0, .5);
	    happy = 0;
	  */
	  alloff();

	  /* debug 
	  printf("\nCHANGING COURSE");
	  start_press();
	   end debug */

	  }
	if (CENTER_IR < 50) {
	  /*
	   *candle in range, enter destroy mode
	   */
	  alloff();
	  kill_process(proc_num);
	  mode = DESTROY_MODE;
	  return;
	}
      }
    }
    if (RIGHT_IR < 240 || LEFT_IR < 240) {
      int diff = LEFT_IR - RIGHT_IR;
      /*
       *candle entered sensor range more on one side than the other
       *stop all motors, then turn to face candle
       */
      
      alloff();
       
      
      /* debug 
      printf("\nWAITING TO TURN");
      start_press();
      printf("\nTURNING");
       end debug */

      happy = 1;
      behavior_vector[SEEK] = ACT_TURN;

      param_vector[SEEK] = diff / 4;      
      /*
	if (diff > 0) {
	param_vector[SEEK] = RIGHT_FACTOR * LOW_SPEED;
	}
	else {
	param_vector[SEEK] = LEFT_FACTOR * LOW_SPEED;
	}
      */
      while(happy) {
	diff = LEFT_IR - RIGHT_IR;
	if (diff < 20) {
	  /*
	   *approximately facing candle
	   *defer to previous loop
	   */
	  alloff();
	  
	  happy = 0;

	  /* debug 
	     printf("\nFACING CANDLE");
	     start_press();
	     end debug */
	}
      }
    }      
  }
}


void seek_candle_new() {

  proc_num = start_process(average_all_ir());

  while(1) {
    /* behavior_vector[SEEK] = ACT_NOTHING;*/
    if (RIGHT_IR < 240 || LEFT_IR < 240) {
      int diff = LEFT_IR - RIGHT_IR;
      if (diff < -25 || diff > 25) {
	/*alloff();*/
	behavior_vector[SEEK] = ACT_SLEEP;
	behavior_vector[SEEK] = ACT_TURN;
	param_vector[SEEK] = diff / 12;
	behavior_vector[SEEK] = ACT_SLEEP;

      }
      else if (CENTER_IR < 50) {
	behavior_vector[SEEK] = ACT_STOP;
	sleep(1.0);
	if (CENTER_IR > 50)
	  break;
	alloff();
	kill_process(proc_num);
	mode = DESTROY_MODE;
	return;
      }
      else if (diff <= 25 ||
	       diff >= -25) {
	/*
	 *approximately facing candle
	 */
	/*alloff();*/
	behavior_vector[SEEK] = ACT_MOVE;
	param_vector[SEEK] = 8;
	kill_process(process_vector[WANDER]);
	behavior_vector[WANDER] = ACT_NOTHING;
      }
      else
	behavior_vector[SEEK] = ACT_NOTHING;
    }      
  }
}