/* global variable declarations */
int ORIENTATION = 0;

/*   MOTORS  
*/  

int LEFT_MOTOR = 0;
int RIGHT_MOTOR = 1;

float ARM_MAX_TIME = 10.;

/* SERVOS 
You want to turn
LOAD_SERVO from 4000-2200
GEAR_SERVO from 2500-3100
TURNING_SERVO from TURNING_SETTING to STRAIGHT_SETTING
*/  

int TURNING_SERVO = 2;
int LOAD_SERVO = 0;
int GEAR_SERVO = 1;

persistent int STRAIGHT_SETTING = 1900;
persistent int TURN_90_DIST = 45;
persistent int TURN_180_DIST = 93;
persistent int HALF_TURN_DIST = 66;
persistent float TURN_90_TIME = 1.5;
persistent float TURN_180_TIME = 2.8;
persistent float CCW_ADJUST = 0.;
persistent float HALF_TURN_TIME = 5.6;

int line_process;

int ball_loaded = 0;
int ball_held = 0;
int balls_fired = 0;
int req_orient = 0;
int badsensor = 0;

persistent int lowval[30];
persistent int highval[30];

int LINE_SENSOR_LEFT = 18;
int LINE_SENSOR_CENTER = 20;
int LINE_SENSOR_RIGHT = 20;
int LIGHT_SENSOR_FRONT_RIGHT = 6;
int LIGHT_SENSOR_FRONT_LEFT = 4;
int LOAD_SENSOR = 29;
int START_LIGHT_SENSOR = 24;
int ARM_LOADED_SENSOR = 27;
int WHEEL_ENCODER = 7;

int OUR_COLOR = 1; /* 1 = black or -1 = white */
 
void main() {
  int a_process;
  int i;
  int j;

  ir_transmit_off();
  
  if (TURN_90_TIME < 1. || TURN_90_TIME > 3.) {
    TURN_90_TIME = 1.45;
  }
  if (TURN_180_TIME < 1. || TURN_180_TIME > 5.) {
    TURN_180_TIME = 2.8;
  }

  init_robot();

  i = analog(16);
  
  while (1) {
    /*    printf("\ncAlibrAte color");
    sleep(.2);
    while (1) {
      if (stop_button()) {
	break;
      } else if (start_button()) {
	calibrate_color();
	break;
      }
      sleep(.01);
    }*/
    printf("\ncAlibrAte color");
    sleep(.2);
    while (1) {
      if (stop_button()) {
	break;
      } else if (start_button()) {
	calibrate_color_dumb();
	break;
      }
      sleep(.01);
    }
    printf("\nStart Everything");
    sleep(.2);
    while (1) {
      if (start_button()) {
	sleep(.5);
	disable_servos();
	start_machine(START_LIGHT_SENSOR);
	init_robot();
	main_test();
	break;
      } else if (stop_button()) {
	break;
      }
      sleep(.01);
    }
    printf("\nOrient");
    sleep(.2);
    while (1) {
      if (start_button()) {
	orient_robot();
	sleep(1.);
	break;
      } else if (stop_button()) {
	break;
      }
      sleep(.01);
    }
    printf("\nShow Sensors");
    sleep(.2);
    while (1) {
      if (start_button()) {
	a_process = start_process(show_sensor_data());
	stop_press();
	kill_process(a_process);
	break;
      } else if (stop_button()) {
	break;
      }
      sleep(.01);
    }
    printf("\nChoose BAD sensor");
    sleep(.2);
    while (1) {
      if (start_button()) {
	sleep(.5);
	choose_bad_sensor();
	break;
      } else if (stop_button()) {
	break;
      }
      sleep(.01);
    }
    printf("\ncAlibrAte 90 turn");
    sleep(.2);
    while (1) {
      if (start_button()) {
	sleep(.5);
	calibrate_90_turn();
	break;
      } else if (stop_button()) {
	break;
      }
      sleep(.01);
    }
    printf("\ncAlibrAte 180 turn");
    sleep(.2);
    while (1) {
      if (start_button()) {
	sleep(.5);
	calibrate_180_turn();
	break;
      } else if (stop_button()) {
	break;
      }
      sleep(.2);
    }
    printf("\ncAlibrAte straight");
    sleep(.2);
    while (1) {
      if (start_button()) {
	sleep(.5);
	calibrate_straight();
	break;
      } else if (stop_button()) {
	break;
      }
      sleep(.2);
    }
    printf("\nCalibrate Half turn");
    sleep(.2);
    while (1) {
      if (start_button()) {
	calibrate_half_turn();
	break;
      } else if (stop_button()) {
	break;
      }
      sleep(.01);
    }
    printf("\nPlay Catch");
    sleep(.2);
    while (1) {
      if (start_button()) {
	i = start_process(catch());
	stop_press();
	kill_process(i);
	break;
      } else if (stop_button()) {
	break;
      }
      sleep(.01);
    }
  }
}

void main_test() {
  int old_dist;

  start_process(arm_catapult());
  start_process(hail_mary());
  start_process(fire_controller());

  orient_robot();

  start_process(triggered_to_load_ball());

  go_direction(0,1);
  sleep(3.);
  old_dist = read_encoder(WHEEL_ENCODER);
  sleep(1.);
  while (old_dist != read_encoder(WHEEL_ENCODER)) {
    old_dist = read_encoder(WHEEL_ENCODER);
    sleep(.3);
  }
/*while (OUR_COLOR * analog(LINE_SENSOR_CENTER) < OUR_COLOR * (highval[LINE_SENSOR_CENTER] + lowval[LINE_SENSOR_CENTER]) / 2) {
    sleep(.01);
  }*/
  go_direction(0,-1);
  sleep(4.5);
  half_turn();

  sleep(.3);

  go_across(); /* orientation had better be 0 */
  
  go_direction(0,-1);
  sleep(.4);
  go_direction(0,0);
  turn_float(90,.5);
  sleep(2.);
  if (req_orient == 1 && ORIENTATION != 2) {
    turn(-90);
    sleep(1.);
    turn(90);
  }
  turn(90);
  
  while (seconds() < 23.) {
    sleep(.1);
  }

  go_across(); /* orientation had better be 2 */
  
  go_direction(0,-1);
  sleep(2.);
  servo(LOAD_SERVO,3500);
  turn(180);
  go_direction(20,1);
  sleep(1.5);
  go_direction(0,0);

  beep();
}

void go_across() {
  int old_dist;
  int older_dist;
  int i;
  int go = 1;

  old_dist = read_encoder(WHEEL_ENCODER);
  go_direction(0,1);
  sleep(.5);
  
  while (1) {
    go = 1;
    older_dist = old_dist;
    old_dist = read_encoder(WHEEL_ENCODER);
    sleep(.3);
    if (req_orient) {
      if (ORIENTATION == 2) {
	turn(180);
	while (req_orient == 1) {
	  if (seconds() > 53.) {
	    go = 2;
	  }
	  sleep(.5);
	}
	turn(180);
      }
      old_dist = -1;
    } else if (ball_held) {
      go = 0;
      old_dist = -1;
    } else if (older_dist == read_encoder(WHEEL_ENCODER)) {
      break;
    }
    if (go == 1) {
      go_direction(0,1);
    } else if (go == 2) {
      go_direction(20,1);
    } else {
      go_direction(0,0);
    }
  }
}

void half_turn() {
  int a = 0;

  go_direction(0,0);
  sleep(0.3);
  reset_encoder(WHEEL_ENCODER);
  while (a < HALF_TURN_DIST) {
    a = read_encoder(WHEEL_ENCODER);
    go_direction(-1 * (20 + 60 * a / HALF_TURN_DIST),1);
    sleep(.01);
  }
  go_direction(0,0);

  ORIENTATION = (ORIENTATION + 1) % 4;
}

/* ***********************************************general functions 
motor/servo functions
  run_motor
    forward
    backward
    right
    left
    run_arm
  run_servo
  
turning functions
   turn_around
   turn_towards
   turn_away
   turn_some
misc functions
   kill_it
   watch_stop
   check_sensor
*/

/* 
direction ranges from -100 - 100 with positive numbers being left
*/
void go_direction(int directionangle, int directionfwd) {
  int servo_val;
  motor(LEFT_MOTOR,directionfwd * (100 + 2 * directionangle));
  motor(RIGHT_MOTOR,directionfwd * (100 - 2 * directionangle));
  servo_val = STRAIGHT_SETTING - 20 * directionangle;
  if (servo_val < 0) {
    servo_val = servo_val + 4000;
  } else if (servo_val > 4000) {
    servo_val = servo_val - 4000;
  }
  servo(TURNING_SERVO,servo_val);
}

void go_direction_float(int directionangle, float directionfwd) {
  int servo_val;
  motor(LEFT_MOTOR,(int) (directionfwd * (float) (100 + 2 * directionangle)));
  motor(RIGHT_MOTOR,(int) (directionfwd * (float) (100 - 2 * directionangle)));
  servo_val = STRAIGHT_SETTING - 20 * directionangle;
  if (servo_val < 0) {
    servo_val = servo_val + 4000;
  } else if (servo_val > 4000) {
    servo_val = servo_val - 4000;
  }
  servo(TURNING_SERVO,servo_val);
}

void run_servo (int n, long time, int start, int finish) {
   servo(n, start);
   msleep(time);
   servo(n,finish);
}

/* ************************************************specific activities 
driving functions
  init_robot
  orient_robot
  wall_follow
  line_follow
catapult functions
  arm_catapult
  load_ball
  fire_ball  

*/


void init_robot () {
  enable_servos();
  servo(LOAD_SERVO,1600);
  servo(GEAR_SERVO,2300);
  servo(TURNING_SERVO,STRAIGHT_SETTING);
  enable_encoder(WHEEL_ENCODER);
  reset_encoder(WHEEL_ENCODER);
}



void orient_robot() {
  int front_left;
  int front_right;
  int back_left;
  int back_right;

  front_left = (analog(LIGHT_SENSOR_FRONT_LEFT) > (highval[LIGHT_SENSOR_FRONT_LEFT] + lowval[LIGHT_SENSOR_FRONT_LEFT]) / 2);
  front_right = (analog(LIGHT_SENSOR_FRONT_RIGHT) > (highval[LIGHT_SENSOR_FRONT_RIGHT] + lowval[LIGHT_SENSOR_FRONT_RIGHT]) / 2);
  back_left = (analog(LINE_SENSOR_LEFT) > (highval[LINE_SENSOR_LEFT] + lowval[LINE_SENSOR_LEFT]) / 2);
  back_right = ( analog(LINE_SENSOR_RIGHT) > (highval[LINE_SENSOR_RIGHT] + lowval[LINE_SENSOR_RIGHT]) / 2);
  
  printf("\n%d %d %d %d",front_left,front_right,back_left,back_right);

  if (badsensor == 0) {
    if (front_left == front_right && front_left == back_left) {
      OUR_COLOR = front_right * 2 - 1;
      go_direction(0,-1);
      sleep(4.);
      turn(180);
      /* turn 180 */
    } else if (front_left == front_right && front_left == back_right) {
      OUR_COLOR = front_left * 2 - 1;
      go_direction(0,-1);
      sleep(.15);
      turn(90);
      go_direction(0,1);
      sleep(1.);
      go_direction(0,0);
      /* turn 90 left (CCW) */
    } else if (front_left == back_right && front_left == back_left) {
      OUR_COLOR = front_left * 2 - 1;
      go_direction(0,-1);
      sleep(.15);
      turn(-90);
      go_direction(0,1);
      sleep(1.);
      go_direction(0,0);
      /* turn 90 right (CW) */
    } else if (back_left == front_right && back_right == back_left) {
      OUR_COLOR = back_left * 2 - 1;
      go_direction(0,1);
      sleep(1.);
      go_direction(0,0);
      /* turn 0 */
    }
  } else if (badsensor == LINE_SENSOR_RIGHT) {
    if (front_left == front_right && front_left == back_left) {
      OUR_COLOR = front_right * 2 - 1;
      go_direction(0,-1);
      sleep(4.);
      turn(180);
      /* turn 180 */
    } else if (front_left == front_right && front_left != back_left) {
      OUR_COLOR = front_left * 2 - 1;
      go_direction(0,-1);
      sleep(.15);
      turn(90);
      go_direction(0,1);
      sleep(1.);
      /* turn 90 left (CCW) */
    } else if (front_left != front_right && front_left == back_left) {
      OUR_COLOR = front_left * 2 - 1;
      go_direction(0,-1);
      sleep(.15);
      turn(-90);
      go_direction(0,1);
      sleep(1.);
      /* turn 90 right (CW) */
    } else if (back_left == front_right && front_left!= back_left) {
      OUR_COLOR = back_left * 2 - 1;
      go_direction(0,1);
      sleep(1.);
      /* turn 0 */
    }
  } else if (badsensor == LINE_SENSOR_LEFT) {
    if (front_left == front_right && front_left != back_right) {
      OUR_COLOR = front_right * 2 - 1;
      go_direction(0,-1);
      sleep(4.);
      turn(180);
      /* turn 180 */
    } else if (front_left == front_right && front_left == back_right) {
      OUR_COLOR = front_left * 2 - 1;
      go_direction(0,-1);
      sleep(.15);
      turn(90);
      go_direction(0,1);
      sleep(1.);
      /* turn 90 left (CCW) */
    } else if (front_left == back_right && front_left != front_right) {
      OUR_COLOR = front_left * 2 - 1;
      go_direction(0,-1);
      sleep(.15);
      turn(-90);
      go_direction(0,1);
      sleep(1.);
      /* turn 90 right (CW) */
    } else if (back_left == front_right && back_right != front_left) {
      OUR_COLOR = back_right * 2 - 1;
      go_direction(0,1);
      sleep(1.);
      /* turn 0 */
    }
  }
  printf(" C: %d",OUR_COLOR);
  ORIENTATION = 3;
}

void turn(int angle) {
  float start_time;
  int break_distance;

  start_time = seconds();

  go_direction(100,0);
  sleep(.25);
  go_direction(angle / abs(angle) * 100,-1);
  if (abs(angle) == 90) {
    reset_encoder(WHEEL_ENCODER);
    while (read_encoder(WHEEL_ENCODER) < TURN_90_DIST) {
      if (seconds() > start_time + 5.) {
	break_distance = read_encoder(WHEEL_ENCODER);
	go_direction(0,0);
	sleep(.2);
	go_direction(0,-1);
	sleep(.3);
	go_direction(100,0);
	sleep(.2);
	go_direction(angle / abs(angle) * 100,-1);
	reset_encoder(WHEEL_ENCODER);
	while (read_encoder(WHEEL_ENCODER) + break_distance < TURN_90_DIST) {
	  sleep(.01);
	}
	break;
      }
      sleep(.01);
    }
  } else if (abs(angle) == 180) {
    reset_encoder(WHEEL_ENCODER);
    while (read_encoder(WHEEL_ENCODER) < TURN_180_DIST) {
      if (seconds() > start_time + 7.) {
	break_distance = read_encoder(WHEEL_ENCODER);
	go_direction(0,0);
	sleep(.2);
	go_direction(0,-1);
	sleep(.3);
	go_direction(100,0);
	sleep(.2);
	go_direction(angle / abs(angle) * 100,-1);
	reset_encoder(WHEEL_ENCODER);
	while (read_encoder(WHEEL_ENCODER) + break_distance < TURN_180_DIST) {
	  sleep(.01);
	}
	break;
      }
      sleep(.01);
    }
  }
  go_direction(0,0);
  sleep(.25);
  ORIENTATION = (ORIENTATION + angle / 90 + 4) % 4;
}

void turn_float(int angle,float speed) {
  float start_time;
  int break_distance;

  start_time = seconds();

  go_direction(100,0);
  sleep(.25);
  go_direction_float(angle / abs(angle) * 100,-1. * speed);
  if (abs(angle) == 90) {
    reset_encoder(WHEEL_ENCODER);
    while (read_encoder(WHEEL_ENCODER) < TURN_90_DIST) {
      if (seconds() > start_time + 5.) {
	break_distance = read_encoder(WHEEL_ENCODER);
	go_direction(0,0);
	sleep(.2);
	go_direction(0,-1);
	sleep(.3);
	go_direction(100,0);
	sleep(.2);
	go_direction_float(angle / abs(angle) * 100,-1.);
	reset_encoder(WHEEL_ENCODER);
	while (read_encoder(WHEEL_ENCODER) + break_distance < TURN_90_DIST) {
	  sleep(.01);
	}
	break;
      }
      sleep(.01);
    }
  } else if (abs(angle) == 180) {
    reset_encoder(WHEEL_ENCODER);
    while (read_encoder(WHEEL_ENCODER) < TURN_180_DIST) {
      if (seconds() > start_time + 7.) {
	break_distance = read_encoder(WHEEL_ENCODER);
	go_direction(0,0);
	sleep(.2);
	go_direction(0,-1);
	sleep(.3);
	go_direction(100,0);
	sleep(.2);
	go_direction_float(angle / abs(angle) * 100,-1.);
	reset_encoder(WHEEL_ENCODER);
	while (read_encoder(WHEEL_ENCODER) + break_distance < TURN_180_DIST) {
	  sleep(.01);
	}
	break;
      }
      sleep(.01);
    }
  }
  go_direction(0,0);
  sleep(.25);
  ORIENTATION = (ORIENTATION + angle / 90 + 4) % 4;
}

/* return 100 if we are over our color (the line)
   or return 0 if we are over opponent's color (everywhere else)
*/
int standardize_sensor(int sensor_num) {
  int val;
  val = analog(sensor_num);
  val = 100 * (val - lowval[sensor_num]) / (highval[sensor_num] - lowval[sensor_num]);
  if (OUR_COLOR == -1) {
    val = 100 - val;
  }
  return val;
}


void arm_catapult() {
  float start_time;

  while (1) {    
    start_time = seconds();
    while ((digital(ARM_LOADED_SENSOR) == 0) && (seconds() < start_time + ARM_MAX_TIME)) {
      motor(2,100);
      motor(3,100);
      sleep(.01);
    }
    if (seconds() > start_time + ARM_MAX_TIME) {
      /* uh-oh... */
      break;
    }
    off(2);
    off(3);
    sleep(.1);
  }
}

void triggered_to_load_ball() {
  while (1) {
    servo(LOAD_SERVO,4000);
    
    while (digital(LOAD_SENSOR) == 0) {
      sleep(.01);
    }
    
    if (ball_loaded || (!(digital(ARM_LOADED_SENSOR)))) {
      hold_ball();
    } else {
      load_ball();
    }
  }
}

void hold_ball() {
  servo(LOAD_SERVO,3000);
  ball_held = 1;

  while (ball_loaded == 1 || (!(digital(ARM_LOADED_SENSOR)))) {
    if (!(digital(LOAD_SENSOR))) {
      sleep(.5);
      if (!(digital(LOAD_SENSOR))) {
	servo(LOAD_SERVO,4000);
	ball_held = 0;
	return;
      }
    }
    sleep(.1);
  }
  
  ball_held = 0;
  load_ball();
}

void load_ball() {
  servo(LOAD_SERVO,2000);
  sleep(1.);
  servo(LOAD_SERVO,4000);
  sleep(.5);
  ball_loaded = 1;
}


void fire_ball () {
  run_servo(GEAR_SERVO,600L,3300,2300);
  ball_loaded = 0;
  balls_fired++;
}

void hail_mary() {
  while (seconds() < 52.) {
    sleep(.1);
  }
  while (seconds() < 58.) {
    if (ball_held && ball_loaded) {
      fire_ball();
    }
    sleep(.1);
  }
  if (ball_loaded) {
    fire_ball();
  }
}

void fire_controller() {
  while (1) {
    sleep(.5);

    req_orient = 0;

    if (ball_loaded) {
      if (balls_fired == 0) {
	if (ORIENTATION == 0) {
	  sleep(1.);
	  fire_ball();
	} else {
	  req_orient = 1;
	}
      } else if (ball_held) {
	if (ORIENTATION == 0) {
	  fire_ball();
	} else {
	  req_orient = 1;
	}
      }
      if (seconds() > 50. && (ball_loaded || ball_held)) {
	req_orient = 1;
      }
    }  
  }
}

void catch() {
  start_process(triggered_to_load_ball());
  start_process(arm_catapult());
  while (1) {
    if (ball_loaded || ball_held) {
      turn(-90);
      while (ball_loaded == 0) {
	sleep(.1);
      }
      fire_ball();
      turn(90);
    }
    sleep(.1);
  }
}