#define STEER_PORT 0

#define GATE_L_PORT 4
#define GATE_R_PORT 5

#define SHAFT_L_PORT 7
#define SHAFT_R_PORT 8

#define MOTOR_LF_PORT 1
#define MOTOR_LB_PORT 3
#define MOTOR_RF_PORT 0
#define MOTOR_RB_PORT 2

#define START_LIGHT_PORT 16

#define IR_N_PORT 9
#define IR_E_PORT 10
#define IR_S_PORT 11
#define IR_W_PORT 12

#define CALIBRATE_SWITCH 13
#define LF_GATE_SENSOR_PORT 14
#define RT_GATE_SENSOR_PORT 15
#define LF_BACK_SENSOR_PORT 13
#define RT_BACK_SENSOR_PORT 31

#define LF_FRONT_LED_PORT 20
#define RT_FRONT_LED_PORT 21
#define LF_BACK_LED_PORT 22
#define RT_BACK_LED_PORT 23

#define DISTANCE_90_LEFT 14
#define DISTANCE_90_RIGHT 16
#define DISTANCE_180 45
#define PIVOT_90_LEFT 38
#define PIVOT_90_RIGHT 40

/************************* MAIN FUNCTION ************************/
 /*global variables*/
int lf_front_led_value, 
  rt_front_led_value, 
  lf_back_led_value, 
  rt_back_led_value;


void main() {
  int orientation;

  /* start code */
  ir_transmit_off();
  calibrate();
  start_machine2(START_LIGHT_PORT,10);
  printf("\nDK-31: RUNNING");
 
  enable_actuators();
  orientation = orient();
  sleep(0.1);
  primary(orientation);
  secondary();
  final();
  
}
/*********************** PRIMARY STRATEGY **********************/

void primary(int orient)
{
  if (/*east*/orient == 2)
    {
      veer_backward(2100,35);
      sleep(0.5);
      right_180();
      servo_open();
      sleep(0.1);
      straight(44);
    }
  else
    {
      if (/*north*/orient == 1)
	{
	  sleep(0.5);
	  right_90();
	}
      else if (/*south*/orient == 3)
	{
	  sleep(0.5);
	  left_90();
	}
      straight(40);
      servo_open();
      sleep(0.1);
      straight(44);
    }

  servo_close();
  sleep(0.5);

  reverse(210);
  error_fix_primary(error_check());
  servo_open();
  sleep(0.2);
}

/************************ SECONDARY STRATEGY *********************/
void secondary() {
  if(digital(IR_W_PORT)) {
    pivot_left_90();
    sleep(0.3);
    servo_close();
    pivot_right_90();
    secondary_1();
  }
  else if (digital(IR_N_PORT)) {
    pivot_left_90();
    secondary_2_1();
    
    if (digital(IR_E_PORT)) {
      secondary_2_3();
      veer_forward(2335,50);
      servo_open();
      straight(133); 
      sleep(0.2);
      left_90();
      veer_backward(3500,30);
      reverse(10);
      sleep(0.2);
      straight(110);
      sleep(0.2);
      left_90();
    }
    else {
      secondary_2_2();
    }
  }
}

/* secondary-1 strategy, see play diagram for more info */
void secondary_1() {
  /* go straight until line detected */
  veer_forward(2350,50);
  /* check sensors within gate - another function */
  servo_open();
  straight(148); 
  sleep(0.2);
  left_90();
  veer_backward(3800,50);
  reverse(5);
  sleep(0.2);
  straight(100);
  servo_close();
  sleep(0.2);
  left_90();
  /* check sensors within gate """ */
}

/* secondary 2-1 strategy, see play diagram for more info */
void secondary_2_1() {
  straight(30);
  sleep(0.5);
  servo_close();
}

/* secondary 2-2 strategy, see play diagram for more info */
void secondary_2_2() {
  veer_forward(2700,80);
  sleep(0.4);
  straight(70);
  /*veer_forward(2200,90);*/
  servo_open();
  straight(160);
  servo_close();
  right_180();
}

/* secondary_3 strategy, see play diagram for more info */
void secondary_2_3() {
  veer_backward(2100,60);
  sleep(0.3);
  pivot_right_90();
}

/*************************** FINAL STRATEGY ******************/
void final() {
  if (digital(IR_N_PORT)) {
    pivot_left_90();
    servo_open();
    straight(110);
    right_90();
    straight(110);
    sleep(0.3);
    reverse(110);
    right_90();
    straight(110);
    left_90();
    final();
  }
  else {
    veer_forward(2390,110);
    servo_open();
    sleep(0.5);
    reverse(120);
    final();
  }
}


/************************ ORIENTATION FUNCTIONS *******************/


/* put robot on our side, calibrate by pressing stop button*/
void calibrate() {
  while (!digital(CALIBRATE_SWITCH)) {
    printf("DK-31: CALIBRATE &  BATTERY");

    /*printf("%d,%d,%d,%d",analog(LF_FRONT_LED_PORT),
      analog(RT_FRONT_LED_PORT),analog(LF_BACK_LED_PORT),
      analog(RT_BACK_LED_PORT));*/
    printf("\n");
  }
  
  lf_front_led_value = analog(LF_FRONT_LED_PORT);
  rt_front_led_value = analog(RT_FRONT_LED_PORT);
  lf_back_led_value = analog(LF_BACK_LED_PORT);
  rt_back_led_value = analog(RT_BACK_LED_PORT);
}


/* returns value of direction robot is facing 
   north = 1, east = 2, south = 3, west = 4 */
int orient() {
  int sample_lf_front = analog(LF_FRONT_LED_PORT);
  int sample_rt_front = analog(RT_FRONT_LED_PORT);
  int sample_lf_back = analog(LF_BACK_LED_PORT);
  int sample_rt_back = analog(RT_BACK_LED_PORT);
  
  int north_diff = abs(sample_rt_front - rt_front_led_value);
  int east_diff = abs(sample_rt_back - rt_back_led_value);
  int south_diff = abs(sample_lf_back - lf_back_led_value);
  int west_diff = abs(sample_lf_front - lf_front_led_value);
  
  return greatest(north_diff,east_diff,south_diff,west_diff);
}

/* returns greatest value, 1 if it is north, 2 if it is east, 
   3 if it is south, and 4 if it is west */
int greatest(int n, int e, int s, int w) {
  int maximum = n;

  if (e > maximum) 
    maximum = e;
  if (s > maximum)
    maximum = s;
  if (w > maximum)
    maximum = w;
  
  if (maximum == n)
    return 1;
  if (maximum == e)
    return 2;
  if (maximum == s)
    return 3;
  if (maximum == w)
    return 4;
}



/********************* SERVO & STEER FUNCTIONS ******************/

/* open gates */
void servo_open()
{
  servo(GATE_L_PORT, 1400);
  servo(GATE_R_PORT, 2800);
}

/* close gates */
void servo_close()
{
  servo(GATE_L_PORT, 2800);
  servo(GATE_R_PORT, 1300);
}

/* enables servos and shaft encoders */
void enable_actuators() {
  enable_servos();
  servo(GATE_L_PORT,3800);
  servo(GATE_R_PORT,450);
  enable_encoder(SHAFT_L_PORT);
  enable_encoder(SHAFT_R_PORT);
}

/* turns on motors, arguments (int lf_speed, int rt_speed)*/
void motors (int lf_speed, int rt_speed) {
  motor(MOTOR_LF_PORT, lf_speed);
  motor(MOTOR_RF_PORT, rt_speed);
  motor(MOTOR_LB_PORT, lf_speed);
  motor(MOTOR_RB_PORT, rt_speed);
}


/* turns 90 degrees left in position */
void left_90() {
  float secs;
  secs = seconds();
  servo(STEER_PORT,800);
  reset_encoder(SHAFT_L_PORT);
  reset_encoder(SHAFT_R_PORT);
  sleep(0.2);
  
  while (read_encoder(SHAFT_L_PORT) < DISTANCE_90_LEFT &&
	 read_encoder(SHAFT_R_PORT) < DISTANCE_90_LEFT) {
    if (right_shaft())
      motors(-100,0);
    if (left_shaft())
      motors(0,100);
    else motors(-100,100);
    while (seconds() >= (secs + 3.0)) {
      if ((read_encoder(SHAFT_L_PORT) < (DISTANCE_90_LEFT / 2)) || 
	  (read_encoder(SHAFT_R_PORT) < (DISTANCE_90_LEFT / 2))) {
	reverse(30);
	sleep(0.4);
	straight(40);
	printf("to");
      }		
    }
    sleep(0.01);
  }
  ao();
}

/* turns 90 degrees right in position */
void right_90() {
  servo(STEER_PORT,3800);
  reset_encoder(SHAFT_L_PORT);
  reset_encoder(SHAFT_R_PORT);
  sleep(0.2);

  while (read_encoder(SHAFT_L_PORT) < DISTANCE_90_RIGHT &&
	 read_encoder(SHAFT_R_PORT) < DISTANCE_90_RIGHT) {
    if (left_shaft())
      motors(0,-100);
    if (right_shaft())
      motors(100,0);
    else motors(100,-100);
    sleep(0.01);
  }
  ao();
}

/* pivots 90 degrees on the left wheel */
void pivot_left_90()
{
  servo(STEER_PORT,800);
  reset_encoder(SHAFT_L_PORT);
  reset_encoder(SHAFT_R_PORT);
  sleep(0.2);
  
  while (read_encoder(SHAFT_R_PORT) < PIVOT_90_LEFT)
    {
      motors(0,100);
      sleep(0.01);
    }
  ao();
}

/* turns 90 degrees on the right wheel */
void pivot_right_90() {
  servo(STEER_PORT,3800);
  reset_encoder(SHAFT_L_PORT);
  reset_encoder(SHAFT_R_PORT);
  sleep(0.2);

  while (read_encoder(SHAFT_L_PORT) < PIVOT_90_RIGHT) {

      motors(100,0);
      sleep(0.01);
  }
  ao();
}


/* turns -90 degrees on the right wheel */
void right_pivot_anti_90() {
  servo(STEER_PORT,3800);
  reset_encoder(SHAFT_L_PORT);
  reset_encoder(SHAFT_R_PORT);
  sleep(0.2);

  while (read_encoder(SHAFT_L_PORT) < PIVOT_90_RIGHT) {

      motors(-100,0);
      sleep(0.01);
  }
  ao();
}


/* moves forward, arguments (int distance) */
void straight(int distance) {
  servo(STEER_PORT,2300);
  reset_encoder(SHAFT_L_PORT);
  reset_encoder(SHAFT_R_PORT);
  
  while (read_encoder(SHAFT_L_PORT) < distance &&
	 read_encoder(SHAFT_R_PORT) < distance) {
    if (left_shaft())
      motors(80,100);
    if (right_shaft())
      motors(100,80);
    else motors(100,100);
    sleep(0.01);
  }
  ao();
}


/* moves backwards, arguments (int distance) */
void reverse(int distance) {
  servo(STEER_PORT,2300);
  reset_encoder(SHAFT_L_PORT);
  reset_encoder(SHAFT_R_PORT);
  
  while (read_encoder(SHAFT_L_PORT) < distance &&
	 read_encoder(SHAFT_R_PORT) < distance) {
    if (left_shaft())
      motors(-80,-100);
    if (right_shaft())
      motors(-100,-80);
    else motors(-100,-100);
    sleep(0.01);
  }
  ao();
}


/* turns left or right backwards, arguments (int angle, int distance)
   turn left angle < 2300
   turn right angle > 2300 */
void veer_backward (int angle, int distance) {
  servo(STEER_PORT,angle);
  reset_encoder(SHAFT_L_PORT);
  reset_encoder(SHAFT_R_PORT);
  
  while (read_encoder(SHAFT_L_PORT) < distance &&
	 read_encoder(SHAFT_R_PORT) < distance) {
    if (left_shaft())
      motors(-80,-100);
    if (right_shaft())
      motors(-100,-80);
    else motors(-100,-100);
    sleep(0.01);
  }
  ao();
}

/* turns left or right forwards, arguments (int angle, int distance)
   turn left angle < 2300
   turn right angle > 2300 */
void veer_forward (int angle, int distance) {
  servo(STEER_PORT,angle);
  reset_encoder(SHAFT_L_PORT);
  reset_encoder(SHAFT_R_PORT);
  
  while (read_encoder(SHAFT_L_PORT) < distance &&
	 read_encoder(SHAFT_R_PORT) < distance) {
    if (left_shaft())
      motors(80,100);
    if (right_shaft())
      motors(100,80);
    else motors(100,100);
    sleep(0.01);
  }
  ao();
}


/* turns 180 degrees left in position */
void left_180() {
  servo(STEER_PORT,800);
  reset_encoder(SHAFT_L_PORT);
  reset_encoder(SHAFT_R_PORT);
  sleep(0.2);
  
  while (read_encoder(SHAFT_L_PORT) < DISTANCE_180 &&
	 read_encoder(SHAFT_R_PORT) < DISTANCE_180) {
    if (right_shaft())
      motors(-100,80);
    if (left_shaft())
      motors(-80,100);
    else motors(-100,100);
    sleep(0.01);
  }
  ao();
}


/* turns 180 degrees right in position */
void right_180() {
  servo(STEER_PORT,3800);
  reset_encoder(SHAFT_L_PORT);
  reset_encoder(SHAFT_R_PORT);
  sleep(0.2);

  while (read_encoder(SHAFT_L_PORT) < DISTANCE_180 &&
	 read_encoder(SHAFT_R_PORT) < DISTANCE_180) {
    if (left_shaft())
      motors(80,-100);
    if (right_shaft())
      motors(100,-80);
    else motors(100,-100);
    sleep(0.01);
  }
  ao();
}

/* checks if right shaft encoder value is higher than left encoder value*/
int right_shaft() {
  if (read_encoder(SHAFT_R_PORT) > read_encoder(SHAFT_L_PORT))
    return 1;
  else
    return 0;
}

/* checks if left shaft encoder value is higher than right encoder value*/
int left_shaft() {
   if (read_encoder(SHAFT_L_PORT) > read_encoder(SHAFT_R_PORT))
    return 1;
  else
    return 0;
}

/************************ ERROR CHECKING (SENSORS) FUNCTIONS **********************/

/* detects time out in shaft encoders*/



/* makes analog ports into digital ports */
int digitalize (int port) {
  if (analog(port) >= 100)
    return 0;
  else
    return 1;
}

/* checks all the sensors */
int error_check() {
  if (digital(LF_BACK_SENSOR_PORT)) 
    return 1;
  if (digitalize(RT_BACK_SENSOR_PORT))
    return 2;
  else
    return 0;
}

/* fixes error in the primary strategy, arguments (int error) */
void error_fix_primary (int error) {
  if (error == 0) 
    return;
  if (error == 1) {
    sleep(0.5);
    straight(15);
    sleep(0.5);
    veer_backward(2600,20);
    error_fix_primary(error_check());
  }
  if (error == 2) {
    sleep(0.5);
    straight(15);
    sleep(0.5);
    veer_backward(2000,20);
    error_fix_primary(error_check());
  }
}