/* volts addl_drive_time throw_time throw_dist line_dist
    6.38               0          9         20           works like butta
    6.37               0          9         20           great 
    6.21               0          9         20           starting to decline */

#define addl_drive_time .15
#define throw_dist 20
#define throw_time .9
#define throw_timeout 2.
#define startlight_port 16
#define dist_port 5
#define dist_low_thresh 10
#define dist_high_thresh 60
#define IR_port_1 9 
#define IR_port_2 10
#define IR_port_3 11
#define IR_port_4 12
int lighthandler_pid,fucked_pid,middle_detect_pid,go_pid;

void main() { 
  while(!start_button()) {
    printf("\n %d %d %d %d %d  %d",analog(dist_port),digital(9),digital(10),digital(11),digital(12),(digital(IR_port_1)*(analog(dist_port)>dist_low_thresh)*(analog(dist_port)<dist_high_thresh)));
    sleep(.1); 
  }
  startup();
  go_pid=start_process(go());
}

void go() {
  getfirstball(1);
  start_process(lift_arm());
  turn_dist(0,throw_dist,throw_timeout);
  drive_dist(4,6,1.2);
  getlineballs(1,alignwall(1,8));
  branch_point(1);
}

void lift_arm() {
  sleep(throw_time);
  arm(2);
}

void branch_point(int run) {
  if(detectRobotStart())
    while(1) {
      right_branch(run);
      run++;
    }
  else {
    while(1) {
      left_branch(run);
      run++;
    }
  }
}

void we_re_fucked() {
  int current_color=(int)((float)(rightlight+leftlight+backlight)/2.);
  printf("\nFUCKED %d",current_color);
  arm(3);
  middle_detect_pid=start_process(middle_detect(current_color));
  drive(0);
  waitforbackbump(5.);
  drive_dist(7,2,.5);
  turn_dist(1,8,2.3);
  while(1) {
    drive(0);
    waitforbackbump(1.2);
    drive_dist(7,2,.5);
    turn_dist(1,8,2.3);
  }
}

void middle_detect(int color) {
  while(1) {
    if(!(color-leftlight)&(color-rightlight)&(color-backlight)&(color-backleftlight)) {
      stop();
      kill_process(fucked_pid);
      printf("\nUnfucked...now what?");
      getfirstball(0);
      getlineballs(1,alignwall(0,8));
      branch_point(1);
    }
    sleep(.05);
  }
}

void speedtest(int dir) {
  start_servos();
  shaft_handler();
  drive(dir);
  while(!stop_button()) {
    printf("\n%d %d",read_encoder(leftshaft_port),read_encoder(rightshaft_port))
;
  sleep(.1);
  }
  stop();
}