/* note, change all sleeps to msleeps */
int clock90L = 44;
int clock90R = 47;
int counter90L = 45;
int counter90R = 48;
int add180 = 7;

int side = 0;
int bump_port = 9;
int white_cutoff = 100;
int black_cutoff = 150;
int LED_PORT1 = 2;
int LED_PORT2 = 3;
int LED_PORT3 = 6;
int orientation = 0;
int obstacle = 0;
int on_error = 0;
long servo_sleep = 400l;

void main2() {
  enable_servos();
  cali_45();
}


void main() {
  ir_transmit_off();
  servo(5,2650);
  i_turn_servos(0l);
  enable_servos();
  calibrate_LED();
  calibrate_turn();
  start_machine(16);
  ir_transmit_on();
  enable_servos();
  side = orient();
  /*side = 1;*/
  msleep(100l);
  obstacle = find_obstacle();
  /*  while(!press_start()){
    obstacle = knob();
    printf("\n Obstacle: %d", obstacle);
    sleep(.1);
    }*/
  printf("\n Obstacle: %d", obstacle);
   
  if (!side) {
    if (obstacle == 4)
      get_black_far();
    else if (obstacle == 3)
      get_black_near();
    else
      get_black_close();
  } else {
    if (obstacle == 4)
      get_white_far();
    else if (obstacle == 3)
      get_white_near();
    else
      get_white_close();
  }

  open_scissors();

  msleep(50l);
  /*i_forw_servos(servo_sleep);
  left_motors(60);
  right_motors(60);
  msleep(325l);
  ao();
  msleep(75l);
  go_straight2(0,0,60);
  msleep(75l);
  i_side_servos(servo_sleep);
  */
  ir_follow();



}

void get_black_far() {

  on_error = 0;
  i_forw_servos(servo_sleep);
  all_back();
  msleep(500l);
  ao();
  msleep(100l);

  servo(0,1825);
  servo(3,2250);
  msleep(servo_sleep);

  go_straight(1,0,100);
  msleep(100l);

  i_forw_servos(servo_sleep);
  go_straight(1,0,202 );
  
  i_turn_servos(servo_sleep);
  r_click(clock90L + 5, clock90R + 5, 1);
  msleep(100l);
  
  i_forw_servos(servo_sleep);

  /*servo(0,2500);
  servo(3,2850);
  msleep(servo_sleep);*/
  
  go_straight(1,bump_port,1);
  

  msleep(150l);
  go_straight(1,bump_port, 1);
  on_error = 0;

  msleep(150l);
  go_straight2(0,0,40);
  msleep(100l);
  i_turn_servos(servo_sleep);
  r_click(3,3,0);
  msleep(50l);
  i_side_servos(servo_sleep);
  go_straight(1,0,190);

}

void get_black_near() {
  on_error = 1;
  i_side_servos(servo_sleep);
  go_straight(0,0,145);
  msleep(50l);
  go_straight(1,0,30);
  msleep(100l);
  i_forw_servos(servo_sleep);
  
  all_back();
  msleep(700l);
  ao();
  msleep(100l);

  servo(0,1825);
  servo(3,2250);
  msleep(servo_sleep);

  go_straight(1,0,90);
  msleep(100l);

  i_forw_servos(servo_sleep);
  go_straight(1,0,185 );
  msleep(100l);
  
  i_turn_servos(servo_sleep);
  r_click(clock90L + 12, clock90R + 12, 1);
  msleep(100l);
  
  i_forw_servos(servo_sleep);
  go_straight(1,bump_port,1);

  msleep(150l);

  go_straight(1,bump_port,1);

  msleep(150l);

  go_straight2(0,0,40);
  on_error = 0;
  msleep(100l);
  i_turn_servos(servo_sleep);
  r_click(3,3,0);
  msleep(50l); 
  i_side_servos(servo_sleep);
  go_straight(1,0,190);
  
}

void get_black_close() {
  on_error = 1;
  servo(0,1825);
  servo(3,2250);
  msleep(servo_sleep);

  go_straight(1,0,138);
  msleep(100l);
  
  i_turn_servos(servo_sleep);
  r_click(clock90L+ 5,clock90R + 5, 1);
  msleep(100l);
  
  i_forw_servos(servo_sleep);
  go_straight(1,bump_port,1);

  msleep(150l);

  go_straight(1,bump_port,1);

  on_error = 0;
  msleep(150l);
  go_straight2(0,0,40);
  msleep(100l);
  i_turn_servos(servo_sleep);
  r_click(3,3,0);
  msleep(50l);
  i_side_servos(servo_sleep);
  go_straight(1,0,290);


}


void get_white_far() {
  on_error = 1;
  i_forw_servos(servo_sleep);
  all_back();
  msleep(600l);
  ao();
  msleep(100l);

  servo(0,3425);
  servo(3,3850);
  msleep(servo_sleep);

  go_straight(1,0,116);
  msleep(100l);

  i_forw_servos(servo_sleep);
  go_straight(1,0,200 );
  
  i_turn_servos(servo_sleep);
  r_click(counter90L + 4, counter90R + 4, 0);
  msleep(100l);
  
  i_forw_servos(servo_sleep);

  /*servo(0,2500);
  servo(3,2850);
  msleep(servo_sleep);*/
  
  go_straight(1,bump_port,1);
  
  msleep(150l);
  go_straight(1,bump_port,1);

  on_error = 1;
  msleep(150l);

  go_straight2(0,0,40);
  msleep(100l);
  i_side_servos(servo_sleep);
  go_straight(0,0,190);

}

void get_white_near() {
  on_error = 0;
  i_side_servos(servo_sleep);
  go_straight2(1,0,153);
  msleep(100l);
  go_straight(0,0,28);
  msleep(100l);
    
  i_forw_servos(servo_sleep);

  all_back();
  msleep(750l);
  ao();
  msleep(100l);

  servo(0,3425);
  servo(3,3850);
  msleep(servo_sleep);

  go_straight(1,0,90);
  msleep(92l);

  i_forw_servos(servo_sleep);
  go_straight(1,0,185 );
  msleep(125l);
  
  i_turn_servos(servo_sleep);
  r_click(counter90L + 12, counter90R + 12, 0);
  msleep(125l);
  
  i_forw_servos(servo_sleep);
  go_straight(1,bump_port,1);
  msleep(150l);
  go_straight(1,bump_port,1);

  on_error =1;
  msleep(150l);
  go_straight2(0,0,40);
  msleep(100l);
  i_side_servos(servo_sleep);
  go_straight(0,0,190);

}

void get_white_close() {
  on_error = 0;
  servo(0,3425);
  servo(3,3850);
  msleep(servo_sleep);

  go_straight(1,0,128);
  msleep(100l);
  
  i_turn_servos(servo_sleep);
  r_click(counter90L+ 5,counter90R + 5, 0);
  msleep(100l);
  
  i_forw_servos(servo_sleep);
  go_straight(1,bump_port,1);
  msleep(150l);
  go_straight(1,bump_port,1);  

  on_error =1;
  msleep(150l);
  go_straight2(0,0,40);
  msleep(100l);
  i_side_servos(servo_sleep);
  go_straight(0,0,290);

}



void r_click(int turncountl, int turncountr, int turnclock) {
  int leftval = -100;
  int rightval = 100;
  int count1 = 0;
  int count2 = 0;

  enable_encoder(7);
  enable_encoder(8);
  if (turnclock) {
    leftval = 100;
    rightval = -100;
  }
  
  left_motors(leftval);
  right_motors(rightval);
  
  while ((count1 < turncountr) || count2 < turncountl) {
    count1 = read_encoder(7);
    count2 = read_encoder(8);
    if (count1 >= turncountr) 
      right_motors(0);
    if (count2 >= turncountl)
      left_motors(0);
  }

}


int find_obstacle(){
  int leftDist = analog(5);
  int rightDist = analog(4);
  
  if(leftDist > 50)
    return (3 + side);
  if(rightDist > 50)
    return (4 - side);
  if(leftDist < 10)
    return (2 - side);
  if(rightDist < 10)
    return (1 + side);
  if((leftDist < 30) && (leftDist > 20) && ((rightDist < 20) || (rightDist > 30)))
    return (0 - 1 - side);
  if((rightDist < 30) && (rightDist > 20) && ((leftDist < 20) || (leftDist > 30)))
    return (side - 2);
  return 0;
}



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;
}


void left_motors(int power) {
  motor(5,power);
  motor(2,power);
  motor(3,power);
}
 
void right_motors(int power) {
  motor(4,power);
  motor(0,power);
  motor(1,power);
}


void i_forw_servos(long delay)
{
  servo(3, 3000);
  servo(0, 2650);
  msleep(delay);
}

void i_side_servos(long delay)
{
  servo(3, 1125);
  servo(0, 815);
  msleep(delay);
}

void i_turn_servos(long delay) {  
  servo(3, 3500);
  servo(0, 2150);
  msleep(delay);
}




int go_straight(int forw, int port, int stopval) {
  int leftcount = 0;
  int rightcount = 0;
    int done = 0;
  int motor_speed = -100;
  int lastchange = 0;
  int oldleftcount = 0;
  int oldrightcount = 0;
  int lastchangedL = 0;
  int lastchangedR = 0;
  float starttime = seconds();
  int loopcount = 0;
  int mid_bumper = 0;

  if (forw) 
     motor_speed = 100;
   

  enable_encoder(7);
  enable_encoder(8);
    
  right_motors(motor_speed);  
  msleep(007l);
  left_motors(motor_speed);
  while (!done) {
    oldleftcount = leftcount;
    oldrightcount = rightcount;
    rightcount = read_encoder(7);
    leftcount = read_encoder(8);
    if (leftcount > oldleftcount)
      lastchangedL = 0;
    else 
      lastchangedL++;
    if (rightcount > oldrightcount)
      lastchangedR = 0;
    else
      lastchangedR++;
    if ((rightcount - leftcount) > 2 ) 
      right_motors(motor_speed - 13);
    else if ((leftcount - rightcount) > 2) 
      left_motors(motor_speed - 13);
    else {
      left_motors(motor_speed);
      right_motors(motor_speed);
    }
    
    if (port == 0) {
      if ((leftcount + rightcount) > (stopval * 2))
	done = 1;
      if ((lastchangedL > 6) || (lastchangedR > 6)) {
	ao();
	disable_servos();
	msleep(200l);
	enable_servos();
	i_forw_servos(servo_sleep);
	go_straight2(0,0,10);
	disable_servos();
	msleep(100l);
	enable_servos();
	i_forw_servos(250l);
	go_straight2(0,0,40);
	msleep(150l);
	i_turn_servos(servo_sleep); 
	r_click(24,25,on_error);
	reset_encoder(7);
	reset_encoder(8);
	if (lastchangedL > 9)
	  stopval = stopval + 45 - leftcount;
	else
	  stopval = stopval + 45 - rightcount;
	msleep(100l);
	i_forw_servos(servo_sleep);
	lastchangedL = 0;
	lastchangedR = 0;
      }
    } else {
      if ((lastchangedL > 6) || (lastchangedR > 6)) {
	ao();
	disable_servos();
	msleep(200l);
	enable_servos();
	i_forw_servos(100l);
	go_straight2(0,0,80);
	msleep(100l);
	i_side_servos(servo_sleep);
	if (side) 
	  go_straight2(1,0,60);
	else
	  go_straight2(0,0,60);
	msleep(100l);
	i_forw_servos(servo_sleep);
	lastchangedL = 0;
	lastchangedR = 0;
      }
      if(!mid_bumper)
	mid_bumper = digital(port+2);
      if ( (digital(port) + digital(port + 1) + mid_bumper) > 1)
	done = 1;
      else if(mid_bumper)
	if(side)
	  right_motors(0);
	else
	  left_motors(0);

    }
    
  }
  /*printf("\n 7: %d 8: %d", rightcount, leftcount);*/
  if(forw){
    ao();
  } else {
    left_motors(0);  
    msleep(007l);
    right_motors(0);
  } 
  return 1;
}

int go_straight2(int forw, int port, int stopval) {
  int leftcount = 0;
  int rightcount = 0;
    int done = 0;
  int motor_speed = -100;
  int lastchange = 0;
  int oldleftcount = 0;
  float starttime = seconds();
  int loopcount = 0;

  if (forw) 
     motor_speed = 100;
   

  enable_encoder(7);
  enable_encoder(8);
    
  left_motors(motor_speed);
  msleep(007l);
  right_motors(motor_speed);  
  while (!done) {
    oldleftcount = leftcount;
    rightcount = read_encoder(7);
    leftcount = read_encoder(8);
    if ((rightcount - leftcount) > 2 ) 
      right_motors(motor_speed - 13);
    else if ((leftcount - rightcount) > 2) 
      left_motors(motor_speed - 13);
    else {
      left_motors(motor_speed);
      right_motors(motor_speed);
    }
    
    if (port == 0) {
      if ((leftcount + rightcount) > (stopval * 2))
	done = 1;
    } else {
      
      if ( (digital(port) + digital(port + 1) + digital(port + 2)) > 1)
	done = 1;
      else if(digital(port+2))
	if(side)
	  right_motors(0);
	else
	  left_motors(0);

    }
    
  }
  /*printf("\n 7: %d 8: %d", rightcount, leftcount);*/
  if(forw){
    ao();
  } else {
    right_motors(0);
    msleep(007l);
    left_motors(0);  
  } 
  return 1;
}


void calibrate_turn() {

  int change = 0;

  printf("\nStart to Calib Clock 90 TURN ON BATTERY");
  wait_start();
  
  while (!stop_button()) {
    clock90L += change;
    clock90R += change;
    r_click(clock90L, clock90R, 1);
    while (!press_start() && !stop_button()) {
      change = (knob() - 123) / 8;
      printf("\nStop/Start       %d", change);
      sleep(.1);
    }
  }
  
  change = 0;
  printf("\nStart to Calib Counter 90");
  wait_start();
  
  while (!stop_button()) {
    counter90L += change;
    counter90R += change;
    r_click(counter90L, counter90R, 0);
    while (!press_start() && !stop_button()) {
      change = (knob() - 123) / 8;
      printf("\nStop/Start     %d", change);
      sleep(.1);
    }
  }

  
  change = 0;
  
  printf("\nStart to Calib Clock 180");
  wait_start();
  
  while (!stop_button()) {
    add180 += change;
    r_click(2 * clock90L + add180, 2 * clock90R + add180, 1);
    while (!press_start() && !stop_button()) {
      change = (knob() - 123)/8;
     printf("\nStop/Start     %d", change);
      sleep(.1);
    }
  }

  /* change = 0;
  printf("\nStart to Calib Counter 180");
  wait_start();
  
  while (!stop_button()) {
    counter180L += change;
    counter180R += change;
    r_click(counter180L, counter180R, 1);
    while (!press_start() && !stop_button()) {
      change = (knob() - 123)/8;
     printf("\nStop/Start     %d", change);
      sleep(.1);
    }
  }
  */


}

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




void cali_45() {

  int serv0val = 1825 ;
  int serv4val = 2250;
  int knobby = 0;
  
  servo(0,serv0val);
  servo(3,serv4val);
  
  wait_start();

  while (!stop_button()){
    
    sleep(.4);
    
    go_straight(1,0,300);
    
    while (!press_start()) {
      if (knobby != (knob() - 123) * 20) {
	knobby = (knob() - 123) * 20;
	printf("\nChange Servo 0:   %d", knobby);
      }
      sleep(.1);
    }
    
    serv0val += knobby;

    while (!press_start()) {
      if (knobby != (knob() - 123) * 20) {
	knobby = (knob() - 123) * 20;
	printf("\nChange Servo 4:   %d", knobby);
      }
      sleep(.1);
    }
    
    serv4val += knobby;
    printf("\n0: %d 4: %d", serv0val, serv4val);

    servo(0,serv0val);
    servo(3,serv4val);

    wait_start();

    
  }
}

void all_back(){
  int k;
  for (k = 0; k< 6; k++)
    bk(k);
}

void all_on(){
  int k;
  for (k = 0; k< 6; k++)
    fd(k);
}

void open_scissors(){
  servo(5, 2350);
  msleep(200l);
  servo(5, 2850);
}

int orient() {
  
  int port1_white = 0;
  int port2_white = 0;
  int port3_white = 0;
  int white_side = 0;

  if (analog(LED_PORT1) < white_cutoff)
    port1_white = 1;
  if (analog(LED_PORT2) < white_cutoff) 
    port2_white = 1;
  if (analog(LED_PORT3) < white_cutoff) 
    port3_white = 1;

  printf("\n%d %d %d", port1_white, port2_white, port3_white);
  printf("  %d %d %d", analog(LED_PORT1), analog(LED_PORT2), analog(LED_PORT3));  
  
  if ((!port1_white) && (!port2_white) && (port3_white)) {
    r_click(clock90L, clock90R, 1);
    orientation = 90;
  } else if ((!port1_white) && (port2_white) && (!port3_white)) {
    r_click(2*clock90L+add180, 2*clock90R+add180, 1);
    orientation = 180;
  } else if ((!port1_white) && (port2_white) && (port3_white)) {
    white_side = 1;
    r_click(2*clock90L+add180, 2*clock90R+add180, 1);
    orientation = 180;
  } else if ((port1_white) && (!port2_white) && (!port3_white)) {
    r_click(counter90L, counter90R, 0);
    orientation = 270;
  } else if ((port1_white) && (!port2_white) && (port3_white)) {
    white_side = 1;
    r_click(clock90L, clock90R, 1);
    orientation = 90;
  } else if ((port1_white) && (port2_white) && (!port3_white)) {
    white_side = 1;
    orientation = 0;
  } else if ((port1_white) && (port2_white) && (port3_white)) {
    white_side = 1;
    r_click(counter90L, counter90R, 0);
    orientation = 270;
  }
  
  side = white_side;
  return white_side;
}

void ir_follow() {
  int i;
  int j;

  int detect = 0;
  int max = 0;
  int which_port;
  int port_counts[3];
  int travel = 0;
    
  int right_port_num = 21;
  int center_port_num = 22;
  int left_port_num = 23;
  int port1;
  int port2;
  int rightval = 0;
  int leftval = 0;
  int oldleftval = 0;
  int oldrightval = 0;
  int lastmoved = 0;

  enable_encoder(7);
  enable_encoder(8);
  
  while(1) {

    oldrightval = rightval;
    oldleftval = leftval;
    rightval = read_encoder(7);
    leftval = read_encoder(8);

    if(!(travel == 0)){
      if((!(rightval == oldrightval)) && (!(leftval == oldleftval)))
	lastmoved = 0;
      else
	lastmoved++;
      if(lastmoved > 4){
	ao();
	msleep(100l);
	if(travel == 1){
	  move_left();
	  msleep(150l);
	} else {
	  move_right();
	  msleep(150l);
	}
	lastmoved = 0;
      }
    }

    for (i = 0; i < 3; i++) 
      port_counts[i] = 0;		   
    detect = 0;
    max = 0;
    for(i = 21; i< 24; i++) {
      digital(i);
      for (j = 0; j < 3; j++) {
	if(digital(i)) {
	  port_counts[i-21] += 1;
	  if (port_counts[i-21] > max)
	    max = port_counts[i-21];
	  detect = 1;
	}
      }
    }
    
    if (detect) {
      if (port_counts[1] == max){ 
	ao();
	travel = 0;
      } else if (port_counts[0] ==  max){
	if(travel > -1) move_right();
	travel = 1;
      } else if (port_counts[2] == max){
	if(travel < 1) move_left();
	travel = -1;
      }
    } else {
      ao();
      travel = 0;
    }
    
    sleep(.04);
  }
}

void move_left(){
  fd(2);
  fd(3);
  fd(5);
  msleep(7l);
  fd(0);
  fd(1);
  fd(4);
}

void move_right(){
  fd(0);
  fd(1);
  fd(4);
  msleep(7l);
  fd(2);
  fd(3);
  fd(5);
}

void calibrate_LED(){
  int port1_white = 0;  
  int port2_white = 0;  
  int port3_white = 0;
  while(!stop_button()){
    port1_white = analog(LED_PORT1); 
    port2_white = analog(LED_PORT2);
    port3_white = analog(LED_PORT3);
    printf("\n%d %d %d", port1_white, port2_white, port3_white);
    sleep(.2);
  }
}