#define blink_delay .01
#define first_ball_timeout .2
#define TIMEOUT1 3.
#define TIMEOUT2 3.
#define Depth_right 19 /* was 15 */
#define Depth_left 10 /* was 6 */

void startup() {
  ir_transmit_off();
  start_servos();
  servo_direct=-1;
  calibrate_light(blink_delay);
  /*rightlight_thres=125;
    leftlight_thres=141; 
    backlight_thres=93;
    backleftlight_thres=74;*/ 
  start_press();
  start_machine(startlight_port);
  start_servos();
  ir_transmit_on();
  lighthandler_pid=start_process(light_handler(blink_delay));
  shaft_handler();
}

/* Go get their 1st ball */
void getfirstball(int droparm) {
  float time_out=seconds()+orient()+first_ball_timeout;
  arm(1);
  drive(2);
  while(!((our_color-rightlight)&(our_color-leftlight)&(our_color-backlight)&(our_color-backleftlight))&(seconds()<time_out)) {defer();}
  while((our_color-rightlight)&(our_color-leftlight)&(seconds()<time_out)) {defer();}
  if(droparm)
    arm(0);
  stop();
} 

/* Bring ball back to our side */
void bringballback(int hug_dir,int extra_dist) {
  int attempt=0;
  float swervetime=seconds()+TIMEOUT1;
  drive(hug_dir);
  while(!((our_color-rightlight)*(our_color-leftlight)*(our_color-backlight)*(our_color-backleftlight))) {defer();}
  while((our_color-rightlight)|(our_color-leftlight)|(our_color-backlight)|(our_color-backleftlight)) { 
    if((seconds()>swervetime)||(digital(right_back_bump_port)&&digital(left_back_bump_port))) {
      if(digital(left_back_bump_port)&&digital(right_back_bump_port))
	if(hug_dir==4) {
	  drive_dist(7,2,.5);
	  turn_dist(0,8,1.8);
	  printf("this is a turn");
	} else {
	  drive_dist(7,2,.5);
	  turn_dist(1,8,1.8);
	  printf("this is another turn");
	}
      else if(hug_dir==4) {
	printf("SWERVING forw_left");
	swerve_forw_left(8);
      } else {
	printf("SWERVING forw_right");
	swerve_forw_right(8);
      }
      drive(hug_dir);
      attempt++;
      if(attempt>5) {
	fucked_pid=start_process(we_re_fucked());
	kill_process(go_pid); /* suicide */
      }
      swervetime=seconds()+TIMEOUT2;
    }
    defer();
  }
  printf("\nHome!");
  if(drive_dist(hug_dir,extra_dist,2.)) {
    drive_dist(7,6,.8);
    drive_dist(0,10,2.);
  }
}

void releaseballhome(int dir) {
  arm(2);
  turn_dist(dir,18,2.);
}

/* Go back to their side */
void backtotheirside(int hug_dir) {
  float swervetime=seconds()+TIMEOUT1;
  int custom_dir,attempt=0;  
  if(hug_dir==5)
    custom_dir=2090;
  else
    custom_dir=1560;
  custom_back(custom_dir,7,1.5);
  drive(hug_dir);
  while(!((our_color-rightlight)*(our_color-leftlight)*(our_color-backlight)*(our_color-backleftlight))) {
    if((seconds()>swervetime)||(digital(right_back_bump_port)&&digital(left_back_bump_port))) {
      if(digital(left_back_bump_port)&&digital(right_back_bump_port))
	if(hug_dir==4) {
	  drive_dist(7,2,.5);
	  turn_dist(0,8,1.8);
	  printf("\nThis is a turn");
	  /*swerve_forw_right(8);*/
	} else {
	  drive_dist(7,2,.5);
	  turn_dist(1,10,2.);
	  printf("\nThis is another turn");
	  /*swerve_forw_left(8);*/
	}
      else if(hug_dir==5) {
	printf("\nSwerving forw_right");
	swerve_forw_right(8);
      } else {
	printf("\nSwerving forw_left");
	swerve_forw_left(8);
      }
      drive(hug_dir);
      attempt++;
      if(attempt>5) {
	fucked_pid=start_process(we_re_fucked());
	kill_process(go_pid); /* suicide */
      }
      swervetime=seconds()+TIMEOUT2;
    }  
    defer();
  }
  printf("\nEnemyLand!");
  swervetime=seconds()+TIMEOUT1;
  while(((our_color-backlight)&(our_color-backleftlight))|!((our_color-rightlight)|(our_color-leftlight))) {
    if(seconds()>swervetime) {
      if(hug_dir==5)
	drive_dist(2,7,.5);
      else
	drive_dist(3,7,.5);
      drive(hug_dir);
      swervetime=seconds()+TIMEOUT2;
    }  
    defer();
  }
  printf("\nLine!");
  stop();
}

int alignwall(int dir,int turn_ticks) {
  turn_dist(dir,turn_ticks,2.);
  drive(0);
  return(waitforbackbump(1.5));
}

/* Follow the line and gather more balls */
void getlineballs(int dir,int backbumpfail) {
  arm(1);
  if(backbumpfail) /* So we don't hang off the edge of the wall when backbump fails */
    drive_dist(dir,39,4.5);
  else
    drive_dist(dir,41,4.5);
  arm(0);
  drive(0);
  sleep(.07);
  drive(7);
  sleep(.1);
  stop();
}

void returntowall() {
  swerve_left(14);
  drive(0);
  waitforbackbump(1.5);
  drive_dist(7,2,.5);
  turn_dist(1,10,2.);
}

void returntootherwall() {
  swerve_right(15);
  drive(0);
  waitforbackbump(1.5);
  drive_dist(7,2,.5);
  turn_dist(0,9,2.);
}

int detectRobotStart() {
  /*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); 
    }*/
  printf("\n%d",analog(dist_port));
  return(digital(IR_port_1)*(analog(dist_port)>dist_low_thresh)*(analog(dist_port)<dist_high_thresh));
}

void left_branch(int run) {
  if(run==1) {
    turn_dist(0,16,2.);
    stop();
    drive(0);
    waitforbackbump(1.5);
    drive_dist(7,2,.5);
    turn_dist(0,9,2.);
  } else
    returntootherwall();
  bringballback(4,Depth_left);
  releaseballhome(1);
  backtotheirside(5);
  getlineballs(6,alignwall(0,10));
}

void right_branch(int run) {
  returntowall();
  bringballback(5,Depth_right);
  releaseballhome(0);
  backtotheirside(4);
  getlineballs(1,alignwall(1,8));
}
  
void custom_back(int dir,int ticks,float time_out) {
  int right_final=read_encoder(rightshaft_port)+ticks,left_final=read_encoder(leftshaft_port)+ticks;
  float time=seconds()+time_out;
  move_servo(dir);
  fd(leftmotor1);
  bk(rightmotor1);
  fd(leftmotor2);
  bk(rightmotor2);
  fd(leftmotor3);
  bk(rightmotor3);
  while(((read_encoder(rightshaft_port)<right_final)+(read_encoder(leftshaft_port)<left_final))&&(seconds()<time)) {defer();}
}

void custom_forw(int dir,int ticks,float time_out) {
  int right_final=read_encoder(rightshaft_port)+ticks,left_final=read_encoder(leftshaft_port)+ticks;
  float time=seconds()+time_out;
  move_servo(dir);
  bk(leftmotor1);
  fd(rightmotor1);      
  sleep(slow_delay);
  bk(leftmotor2);
  fd(rightmotor2);
  sleep(slow_delay);
  bk(leftmotor3);
  fd(rightmotor3);
  while(((read_encoder(rightshaft_port)<right_final)+(read_encoder(leftshaft_port)<left_final))&&(seconds()<time)) {defer();}
}