/* Team 45: chicken.c
 */

/* port numbers */
#define START_LIGHT_PORT 31
#define RIGHT_LIGHT_PORT 2
#define LEFT_LIGHT_PORT 3
#define LEFT_SERVO_PORT 2
#define RIGHT_SERVO_PORT 1
#define LEFT_MOTOR_PORT 2 
#define RIGHT_MOTOR_PORT 1 
#define FRONT_DIGITAL_PORT 12


/* important settings */
#define LSERVO_DEGREE_0 1700
#define LSERVO_DEGREE_45 2700
#define LSERVO_DEGREE_90 3500
#define RSERVO_DEGREE_0 1400
#define RSERVO_DEGREE_45 2400
#define RSERVO_DEGREE_90 3400

#define LEFT_MOTOR_FWD -100
#define RIGHT_MOTOR_FWD -86

#define LTURN_DURATION .63
#define RTURN_DURATION .58

/* regular macros */
#define WHITE -1
#define BLACK 260

/* global variables */
int left_light_thresh, right_light_thresh;
int am_white; /* boolean to tell if our ball color is white */

void main() {
  ir_transmit_off();  
  calibrate();
  start_machine(START_LIGHT_PORT);
  enable_servos();
  orient();
  go();
  alloff();
}

void calibrate()
{
  printf("begin calibrate press start\n");
  wait_start();

  /* for debug: begin 
  right_light_thresh = 150;
  left_light_thresh = 150;
  debug: end */

  calibrate_right_light();
  calibrate_left_light();

  printf("calibrate done. press start\n");
  wait_start();

  printf("light: r-%d, l-%d.\n", right_light_thresh, left_light_thresh);
  wait_start();
}

void orient() {
  /* orients the robot towards the holes */

  int prev_linput, prev_rinput, prev_match;
  int linput, rinput, match;

  /* read until you get same color on both sides */
  while (!color_match()) turn_left();
 
  printf("DSFgsfgdg\n");
  if (left_color() == WHITE) { 
	printf("i am white\n");
    am_white = 1;
    white_orient();     
  } else {
	printf("i am black\n");
    am_white = 0;
    black_orient();     
  } 
}

void white_orient() {
  /* originally, rear light sensors are on matching colors */
  while (color_match()) turn_left();
  /* now, should be facing holes */
}

void black_orient() {
  /* originally, rear light sensors are on matching colors */
  while (color_match()) turn_right();
  /* now, should be facing holes */
}

int color_match() {
  /* true = 1, false = 0 */

  int linput, rinput, match;
  linput = left_color();
  rinput = right_color();

  match = 0;
  if (linput == rinput) {
    match = 1;
  }
  
  /* for debug
  printf("r: %d, l: %d, match: %d\n", rinput, linput, match);
  wait_start();
  */

  return match;
}

int left_color() {
  int input, retVal;
  input = analog(LEFT_LIGHT_PORT);
  if (input > left_light_thresh) {
	retVal = BLACK;
  } else {
    retVal = WHITE;
  }
  return retVal;
}

int right_color() {
  int input, retVal;
  input = analog(RIGHT_LIGHT_PORT);
  if (input > right_light_thresh) {
	retVal = BLACK;
  } else {
    retVal = WHITE;
  }
  return retVal;
}

void go() {

  /* real navigation goes here; should be facing holes */

  while (! front_sensor_pressed()) {
    go_forward(0.1);
  }

  if (am_white) {
    turn_right();
  } else {
    turn_left();
  }
  while (1) {
    go_forward(5.0);
    stop(0.1);
    go_backward(5.0);
  }

}

int front_sensor_pressed() {
  int input;

  input = digital(FRONT_DIGITAL_PORT);
  input = digital(FRONT_DIGITAL_PORT);
  return input;
}

void go_forward(float duration) {
  printf("go forward\n");
  reset_motors_and_servos();
  sleep(duration);
  stop(0.1);
}

void go_backward(float duration) {
  printf("go backward\n");
  servo(LEFT_SERVO_PORT, LSERVO_DEGREE_0);
  servo(RIGHT_SERVO_PORT, RSERVO_DEGREE_0);
  sleep(0.1);
  motor(LEFT_MOTOR_PORT, -LEFT_MOTOR_FWD); 
  motor(RIGHT_MOTOR_PORT, -RIGHT_MOTOR_FWD);
  sleep(duration);
  stop(0.1);
}

void turn_left() {
  printf("turn left\n");
  servo(LEFT_SERVO_PORT, LSERVO_DEGREE_45);
  servo(RIGHT_SERVO_PORT, RSERVO_DEGREE_45);
  sleep(0.5);  
  motor(LEFT_MOTOR_PORT, -LEFT_MOTOR_FWD);
  motor(RIGHT_MOTOR_PORT, RIGHT_MOTOR_FWD);
  sleep(LTURN_DURATION);
  stop(0.1);
}

void turn_right() {
  printf("go right\n");
  servo(LEFT_SERVO_PORT, LSERVO_DEGREE_45);
  servo(RIGHT_SERVO_PORT, RSERVO_DEGREE_45);
  sleep(0.5);  
  motor(LEFT_MOTOR_PORT, LEFT_MOTOR_FWD);
  motor(RIGHT_MOTOR_PORT,-RIGHT_MOTOR_FWD);
  sleep(RTURN_DURATION);
  stop(0.1);
}

void stop(float duration) {
  motor(LEFT_MOTOR_PORT, 0); 
  motor(RIGHT_MOTOR_PORT, 0);
  sleep(duration);
}

void reset_motors_and_servos()
{
  servo(LEFT_SERVO_PORT, LSERVO_DEGREE_0);
  servo(RIGHT_SERVO_PORT, RSERVO_DEGREE_0);
  sleep(0.1);
  motor(LEFT_MOTOR_PORT, LEFT_MOTOR_FWD); 
  motor(RIGHT_MOTOR_PORT, RIGHT_MOTOR_FWD);
}

void calibrate_right_light()
{
  int white, black;

  printf("do right white  press start\n");
  wait_start();
  white = ave_light(RIGHT_LIGHT_PORT);
  printf("rw - %d, press start\n", white);
  wait_start();

  printf("do right black  press start\n");
  wait_start();
  black = ave_light(RIGHT_LIGHT_PORT);
  printf("rb - %d, press start\n", black);
  wait_start();
 
  right_light_thresh =  (white + black) / 2;  
  printf("r thresh - %d, press start\n", right_light_thresh);
  wait_start();
}

void calibrate_left_light()
{
  int white, black;

  printf("do left  white  press start\n");
  wait_start();
  white = ave_light(LEFT_LIGHT_PORT);
  printf("lw - %d, press start\n", white);
  wait_start();

  printf("do left  black  press start\n");
  wait_start();
  black = ave_light(LEFT_LIGHT_PORT);
  printf("lb - %d, press start\n", black);
  wait_start();
 
  left_light_thresh =  (white + black) / 2;  
  printf("l thresh - %d, press start\n", left_light_thresh);
  wait_start();
}

int ave_light (int p) 
{
  int n = 10;
  int sum_inputs = 0, ave_input = 0;
  int i;

  printf("getting data\n");

  for (i = 0; i < n; i++) {
    sum_inputs += analog(p);
    sleep(0.1);
  }
  return ave_input = sum_inputs/n;
}

void wait_start()
{
  while (!press_start());
}

int press_start()
{
  if (!start_button()) return 0;
  while (start_button());
  return 1;
}

int press_stop()
{
  if (!stop_button()) return 0;
  while (stop_button());
  return 1;
}