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

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;

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

int LINE_SENSOR_LEFT = 18;
int LINE_SENSOR_CENTER = 17;
int LINE_SENSOR_RIGHT = 16;
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("\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("\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 Half turn");
    sleep(.2);
    while (1) {
      if (start_button()) {
	calibrate_half_turn();
	break;
      } else if (stop_button()) {
	break;
      }
      sleep(.01);
    }
  }
}

void main_test() {
  ORIENTATION = 0;

  start_process(arm_catapult());

  orient_robot();

  start_process(triggered_to_load_ball());

  go_direction(0,1);
  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,0);
  sleep(1.);
  go_direction(0,-1);
  sleep(2.5);
  half_turn();

  ORIENTATION = 1;
  sleep(.5);
  
  wait_for_arm();

  go_across();

  ORIENTATION = 0;
  go_direction(0,-1);
  sleep(.2);
  go_direction(0,0);
  turn(90);
  sleep(2.);
  turn(-90);
  ORIENTATION = 1;
  go_direction(0,-1);
  sleep(1.);
  turn(180);
  ORIENTATION = 0;
  go_direction(0,0);
}

void wait_for_arm() {
  while (!(digital(ARM_LOADED_SENSOR))) {
    go_direction(0,0);
    sleep(.01);
  }
}

void go_across() {
  int old_dist;

  old_dist = read_encoder(WHEEL_ENCODER);
  go_direction(0,1);
  sleep(.5);
  while ((old_dist != read_encoder(WHEEL_ENCODER)) && (ball_loaded == 0)) {
    old_dist = read_encoder(WHEEL_ENCODER);
    sleep(.3);
  }
  if (ball_loaded == 1) {
    sleep(2.);
    go_direction(0,-1);
    sleep(.1);
    go_direction(0,0);
    go_across();
  }
  sleep(.2);
}

void half_turn() {
  int a;


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

/* ***********************************************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 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 (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(.25);
    turn(90);
    /* 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(.25);
    turn(-90);
    /* turn 90 right (CW) */
  } else if (back_left == front_right && back_right == back_left) {
    OUR_COLOR = back_left * 2 - 1;
    /* turn 0 */
  }
  printf(" C: %d",OUR_COLOR);
}

void turn(int angle) {
  float fudge_modifier = 0.;

  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) {
      sleep(.01);
    }
  } else if (abs(angle) == 180) {
    reset_encoder(WHEEL_ENCODER);
    while (read_encoder(WHEEL_ENCODER) < TURN_180_DIST) {
      sleep(.01);
    }
  }
  go_direction(0,0);
  sleep(.25);
}

/* 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 line_follow() {
  int our_alignment;
  int blah;

  if (OUR_COLOR * analog(LINE_SENSOR_LEFT) < OUR_COLOR * (highval[LINE_SENSOR_LEFT] + lowval[LINE_SENSOR_LEFT]) / 2) {
    printf("\error! not on line!");
    /* return; */
  }

  while (1) { /* time restraint/check for wall */
    /*our_alignment = standardize_sensor(LINE_SENSOR_LEFT) - standardize_sensor(LINE_SENSOR_RIGHT);*/
    our_alignment = standardize_sensor(LINE_SENSOR_LEFT) - standardize_sensor(LINE_SENSOR_RIGHT);
    if (abs(our_alignment) > 30) {
      sleep(.01);
      blah = (standardize_sensor(LINE_SENSOR_LEFT) - standardize_sensor(LINE_SENSOR_RIGHT));
      sleep(.01);
      blah = (blah + (standardize_sensor(LINE_SENSOR_LEFT) - standardize_sensor(LINE_SENSOR_RIGHT))) / 2;
      if (abs(blah) < 30) { /* avoid negative sleep vals */
	blah = 30;
      }
      printf("\nA:%d B:%d I:%d",blah,(standardize_sensor(LINE_SENSOR_LEFT) - standardize_sensor(LINE_SENSOR_RIGHT)),our_alignment);
      go_direction(our_alignment/abs(our_alignment) * 40,1);
      sleep(.015 * ((float) (abs(blah) - 30)));
      go_direction(0,1);
      sleep(.03 * ((float) (abs(blah) - 30)));
      go_direction(-1 * our_alignment/abs(our_alignment) * 40,1);
      sleep(.01 * ((float) (abs(blah) - 30)));
    }
    go_direction(0,1);
    /*go_direction(current_dir - our_alignment,1);
    printf("\n%d",current_dir - our_alignment);*/
    sleep(.01);
  }
}

void line_follow_2() {
  int our_alignment;
  int done = 0;
  while (1) {
    our_alignment = standardize_sensor(LINE_SENSOR_LEFT) - standardize_sensor(LINE_SENSOR_RIGHT);
    if (!on_line() && (abs(our_alignment) < 10)) {
      while(!near_line()) {
	go_direction(0,1);
	sleep(0.01);
      }
      if (our_alignment > 0) {
	while(!right_of_line()) {
	  go_direction(0,1);
	  sleep(0.01);
	}
      } else {
	while(!left_of_line()) {
	  go_direction(0,1);
	  sleep(0.01);
	}
      }
    }
    if (on_line()) {
      go_direction(0,1);
      sleep(.01);
    } else {
      done = 1;
      /* to the left of the line */
      if (our_alignment > 0) {
	/*
	while(near_line()) {
	  go_direction(100,1);
	  sleep(0.01);
	}
	*/
	go_direction(100,1);
	sleep(0.8);
	while(!near_line()) {
	  go_direction(-20,1);
	  sleep(0.01);
	}
	while(!on_line()) {
	  go_direction(-40,1);
	  sleep(0.01);
	}
	/* to the right of the line */
      } else {
	/*
	while(near_line()) {
	  go_direction(-100,1);
	  sleep(0.01);
	}
	*/
	go_direction(-100,1);
	sleep(0.75);
	while(!near_line()) {
	  go_direction(20,1);
	  sleep(0.01);
	}
	while(!on_line()) {
	  go_direction(40,1);
	  sleep(0.01);
	}
      }
    }
  }
}

void line_follow_3() {
  int our_alignment;
  while (1) {
    our_alignment = standardize_sensor(LINE_SENSOR_LEFT) - standardize_sensor(LINE_SENSOR_RIGHT);
    if (on_line() || (abs(our_alignment) < 10)) {
      go_direction(0,1);
      sleep(0.01);
    } else {
      if (our_alignment > 0) {
	while(!on_line()) {
	  go_direction(30,1);
	  sleep(0.01);
	}
	while(!on_line()) {
	  go_direction(-10,1);
	  sleep(0.01);
	}
      } else {
	while(!on_line()) {
	  go_direction(-30,1);
	  sleep(0.01);
	}
	while(!on_line()) {
	  go_direction(10,1);
	  sleep(0.01);
	}
      }
    }
  }
}

int on_line() {
  return (OUR_COLOR * analog(LINE_SENSOR_CENTER) < OUR_COLOR * (highval[LINE_SENSOR_CENTER] + lowval[LINE_SENSOR_CENTER]) / 2);
}

int left_of_line() {
  return ((OUR_COLOR * analog(LINE_SENSOR_RIGHT) < OUR_COLOR * (highval[LINE_SENSOR_RIGHT] + lowval[LINE_SENSOR_RIGHT]) / 2));
}

int right_of_line() {
  return ((OUR_COLOR * analog(LINE_SENSOR_LEFT) < OUR_COLOR * (highval[LINE_SENSOR_LEFT] + lowval[LINE_SENSOR_LEFT]) / 2));
}

int near_line() {
  return ((OUR_COLOR * analog(LINE_SENSOR_RIGHT) < OUR_COLOR * (highval[LINE_SENSOR_RIGHT] + lowval[LINE_SENSOR_RIGHT]) / 2) || (OUR_COLOR * analog(LINE_SENSOR_LEFT) < OUR_COLOR * (highval[LINE_SENSOR_LEFT] + lowval[LINE_SENSOR_LEFT]) / 2));
}

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) || (digital(ARM_LOADED_SENSOR) == 0)) {
      sleep(.01);
    }
    load_ball();
  }
}

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

  /*********  change me  *******/

  while (ORIENTATION == 0) {
    sleep(.1);
  }
  fire_ball();
  ball_loaded = 0;
}


void fire_ball () {
  run_servo(GEAR_SERVO,600L,3600,2300);
}