#define IRPORT 6
#define THR2 60
#define THR3 160
#define THR4 40
#define THR5 30
#define THR16 25
#define THR17 35
#define SHAFT90 1300
#define SHAFT180 2325
#define SHAFT270 750
#define SHAFT0 275
#define SHAFTSWING 400
#define TIMETOEXTEND 3.2
#define TIMETOSWINGFORWARD 6.0
#define TIMETOSWINGBACK 2.2
#define BASEROT 50

int ana2, ana3, ana4, ana5, ana16, ana17, blue_or_white;

void main(){
     int position;
     printf("\nReady to calibrate");
     while((!start_button()) && (!stop_button()));
     if(start_button()){
       while(start_button());
       ana2 = analog(2);
       ana3 = analog(3);
       ana4 = analog(4);
       ana5 = analog(5);
       ana16 = analog(16);
       ana17 = analog(17);
       printf("\n%d:%d:%d:%d:%d:%d", analog(2), analog(3), analog(4), analog(5), analog(16), analog(17));
       while((!start_button()) && (!stop_button()));
       if(start_button()){
	 while(start_button());
	 ana2 = (analog(2) + ana2) / 2;
	 ana3 = (analog(3) + ana3) / 2;
	 ana4 = (analog(4) + ana4) / 2;
	 ana5 = (analog(5) + ana5) / 2;
	 ana16 = (analog(16) + ana16) / 2;
	 ana17 = (analog(17) + ana17) / 2;
	 printf("\n%d:%d:%d:%d:%d:%d", analog(2), analog(3), analog(4), analog(5), analog(16), analog(17));
	 while((!start_button()) && (!stop_button()));
       }
     }
     if(stop_button()){
       printf("\nRejected.  Using defaults.");
       ana2 = THR2;
       ana3 = THR3;
       ana4 = THR4;
       ana5 = THR5;
       ana16 = THR16;
       ana17 = THR17;
     }
     else {
       printf("\nUsing these values.");
       while(!start_button());
       while(start_button());
     }

     start_machine(IRPORT);
     position = orient();
     printf("\n%d", position);
     turn_and_extend(position);
     if(analog(17)<THR17){
       blue_or_white = 0;}
     else{
       blue_or_white = 1;}
     swing();
     while(seconds() < 54.0){
       compensate();
     }
     motor(0, BASEROT);
     motor(1, BASEROT);
     motor(2, BASEROT);
     fd(3);
}

int orient(){
    if((analog(2) < ana2) + (analog(3) < ana3) == 1){
             if((analog(2) < ana2) == (analog(4) < ana4)){
                     return 90;}
             else return 180;}
    else if((analog(2) < ana2) == (analog(4) < ana4)){
         return 270;}
    else return 0;
}

void turn_and_extend(int pos){
  float curtime;
  enable_encoder(8);
  reset_encoder(8);
     if(pos == 270){
            bk(0);
            bk(1);
	    bk(2);
            while(read_encoder(8)<SHAFT270);
     }
     else{
            fd(0);
            fd(1);
	    fd(2);
            if(pos == 0){
                   while(read_encoder(8)<SHAFT0);}
            if(pos == 90){
                   while(read_encoder(8)<SHAFT90);}
	    if(pos == 180){
	      while(read_encoder(8)<SHAFT180);}
      }
     disable_encoder(8);
      ao();
      fd(4);
      fd(5);
      curtime = seconds();
      while((!digital(15)) && (seconds() - curtime < 5.0));
      while(!digital(15)){
	go_crazy();
	fd(4);
	fd(5);
	sleep(3.0);
      }
      sleep(1.0);
      while(!digital(14)){
        go_crazy();
	}
   off(4);
   off(5);
}

void go_crazy(){
  printf("\nGoing crazy");
	ao();
	fd(0);
	fd(1);
	fd(2);
	fd(3);
	sleep(2.0);
	bk(0);
	bk(1);
	bk(2);
	bk(3);
	sleep(4.0);
	fd(0);
	fd(1);
	fd(2);
	fd(3);
	sleep(2.0);
	ao();
}

void swing(){
  float curtime;
  curtime = seconds();
  enable_encoder(8);
  reset_encoder(8);
  printf("\nSwinging:");
     motor(0, BASEROT);
     motor(1, BASEROT);
     motor(2, BASEROT);
     fd(3);
     while((read_encoder(8) < SHAFTSWING) && (seconds() - curtime < TIMETOSWINGFORWARD));
     ao();
     if(read_encoder(8) >= SHAFTSWING){
       printf("\nShafted out");}
     else {
       printf("\nTimed out");}
     extendyarm();
}

void extendyarm(){
  enable_servos();
   servo(0, 200);
}

void compensate(){
       if(blue_or_white == 0){
	 if((analog(16)<THR16) && (analog(17)<THR17)){
	   motor(0, BASEROT);
	   motor(1, BASEROT);
	   motor(2, BASEROT);
	   fd(3);}
	 else if((analog(16)>THR16) && (analog(17)>THR17)){
	   motor(0, -BASEROT);
	   motor(1, -BASEROT);
	   motor(2, -BASEROT);
	   bk(3);}
	 else {ao();}
       }
       else if((analog(16)>THR16) && (analog(17)>THR17)){
           motor(0, BASEROT);
           motor(1, BASEROT);
           motor(2, BASEROT);
           fd(3);}
         else if((analog(16)<THR16) && (analog(17)<THR17)){
           motor(0, -BASEROT);
           motor(1, -BASEROT);
           motor(2, -BASEROT);
           bk(3);}
         else {ao();}
}