void main(){
  long timeout;
  servo(L_SERVO, L_UP);
  servo(R_SERVO,R_UP);
  
  selfCalibrate();
  printf("put in position\n");
  wait_start();
  start_machine(6);
  enable_servos();
  if(getColor(MBL_SENSOR) == WHITE)
    {
      setup(FORWARD);
    }

  if(getColor(MFL_SENSOR) == WHITE)
    {
      setup(BACKWARD);
    }
  printf("orientation %d\n",ORIENTATION);
  timeout = mseconds();
  while((mseconds()-timeout)< 1000L);
  servo(L_SERVO, L_UP);
  servo(R_SERVO,R_UP);

  
  /*launch small robot depending on side drive opposing motors*/
  fd(4);
  fd(5);
  timeout = mseconds();
  while((mseconds() - timeout) <3000L);
  motor(4,0);
  motor(5,0);
  while(CURRENT_STATE != SEARCHING){
    lineFollow();
  }
  printf("searching\n");
  timeout = mseconds();
  goMotor(0,-25);
  goMotor(1,-25);
  goMotor(2,-25);
  goMotor(3,-25);
  while(mseconds()-timeout<200L);
  sortBalls();
  if(ORIENTATION == FORWARD){
    setup(BACKWARD);
    printf("reversed %d\n", ORIENTATION);
  } 
  else if(ORIENTATION == BACKWARD){
    setup(FORWARD);
    printf("backside\n");
  }
  CURRENT_STATE = GOING_HOME;
  while(CURRENT_STATE != AT_HOME)
    {
      lineFollow(); 
    }
  ao();
}

void setup(int direction){
  long pause = mseconds();
  printf("entered setup with direction %d\n", direction);
  while((mseconds() - pause) < 1000L);
  if(direction == FORWARD){
    ORIENTATION = FORWARD;
    LL_SENSOR = LFL_SENSOR;
    ML_SENSOR = MFL_SENSOR;
    RL_SENSOR = RFL_SENSOR;  
    RMOTOR_1 = RMOTOR_1_FD;
    RMOTOR_2 = RMOTOR_2_FD; 
    LMOTOR_1 = LMOTOR_1_FD;
    LMOTOR_2 = LMOTOR_2_FD;
    F_BUMPER= 12;
    B_BUMPER = 11;
    OURBALL = GREEN;
  }
  else  
    if(direction == BACKWARD){
      ORIENTATION = BACKWARD;
      LL_SENSOR = LBL_SENSOR;
      ML_SENSOR = MBL_SENSOR;
      RL_SENSOR = RBL_SENSOR;  
      RMOTOR_1 = RMOTOR_1_BK;
      RMOTOR_2 = RMOTOR_2_BK; 
      LMOTOR_1 = LMOTOR_1_BK;
      LMOTOR_2 = LMOTOR_2_BK;
      F_BUMPER= 11;
      B_BUMPER = 12;
      OURBALL = WHITE;
    }
  printf("orientation is now %d\n", ORIENTATION);
  while((mseconds() - pause) < 1000L);

}

void goMotor(int number, int power){
  if(ORIENTATION == FORWARD){
    motor(number, power);
  }
  if(ORIENTATION == BACKWARD){
    motor(number, -power);
  }
}

/*compensate for expansion board"*/
int readAnalog(int port)
{
  analog(port);
  return analog(port);
}

void wait_stop(){
  while(!press_stop());
}

void selfCalibrate(){
  /*add a loop in case the first value is unreasonable*/
  long begin;
  printf("put robot on black, press start(start=ok, stop =redo)\n");
  wait_start();

  while(1){
    begin = mseconds();
    while((mseconds() - begin)< 200L);
    LFB_VALUE = averageSensor(LFL_SENSOR);
    MFB_VALUE = averageSensor(MFL_SENSOR);
    RFB_VALUE = averageSensor(RFL_SENSOR);
    LBB_VALUE = averageSensor(LBL_SENSOR);
    MBB_VALUE = averageSensor(MBL_SENSOR);
    RBB_VALUE = averageSensor(RBL_SENSOR);
    printf("black %d, %d, %d, %d, %d,%d\n", LFB_VALUE,MFB_VALUE, RFB_VALUE, LBB_VALUE,MBB_VALUE,RBB_VALUE); 
    if(press_start()){
      break;
    }
    if(press_stop()){
      printf("redo");
    }
	
  }
  printf("put robot on white, press start(start=ok, stop =redo)\n");
  wait_start();

  while(1){
    begin = mseconds();
    while((mseconds() - begin)< 200L);
    LFW_VALUE = averageSensor(LFL_SENSOR);
    MFW_VALUE = averageSensor(MFL_SENSOR);
    RFW_VALUE = averageSensor(RFL_SENSOR);
    LBW_VALUE = averageSensor(LBL_SENSOR);
    MBW_VALUE = averageSensor(MBL_SENSOR);
    RBW_VALUE = averageSensor(RBL_SENSOR);
    printf("white %d, %d, %d, %d, %d,%d\n", LFW_VALUE,MFW_VALUE, RFW_VALUE, LBW_VALUE,MBW_VALUE,RBW_VALUE); 
    if(press_start()){
      break;
    }
    if(press_stop()){
      printf("redo");
    }
	
  }

  printf("put green ball on left, press start\n");
  wait_start();

  while(1){
    begin = mseconds();
    while((mseconds() - begin)< 200L);
    LGREEN_BALL_VALUE = averageSensor(LBALL_SENSOR);
    printf("left green is %d\n", LGREEN_BALL_VALUE);
    if(press_start()){
      break;
    }
    if(press_stop()){
      printf("redo");
    }
  }

  printf("put white ball on left, press start\n");
  wait_start();

  while(1){
    begin = mseconds();
    while((mseconds() - begin)< 200L);
    LW_BALL_VALUE = averageSensor(LBALL_SENSOR);
    printf("left white is %d\n", LW_BALL_VALUE);
    if(press_start()){
      break;
    }
    if(press_stop()){
      printf("redo");
    }
  }
  
  printf("put green ball on right, press start\n");
  wait_start();
  while(1){
    begin = mseconds();
    while((mseconds() - begin)< 200L);
    RGREEN_BALL_VALUE = averageSensor(RBALL_SENSOR);
    printf("right green is %d\n", RGREEN_BALL_VALUE);
    if(press_start()){
      break;
    }
    if(press_stop()){
      printf("redo");
    }
  }


  printf("put ball on right, press start\n");
  wait_start();
  while(1){
    begin = mseconds();
    while((mseconds() - begin)< 200L);
    RW_BALL_VALUE = averageSensor(RBALL_SENSOR);
    printf("right white is %d\n", RW_BALL_VALUE);
    if(press_start()){
      break;
    }
    if(press_stop()){
      printf("redo");
    }
  }
    LFB_THRESHOLD = ((LFB_VALUE + LFW_VALUE)/2);
    MFB_THRESHOLD = ((MFB_VALUE + MFW_VALUE)/2);
    RFB_THRESHOLD = ((RFB_VALUE + RFW_VALUE)/2);

    LBB_THRESHOLD = ((LBB_VALUE + LBW_VALUE)/2);
    MBB_THRESHOLD = ((MBB_VALUE + MBW_VALUE)/2);
    RBB_THRESHOLD = ((RBB_VALUE + RBW_VALUE)/2);

}

int getThreshold(int port){
 
  if(port == LFL_SENSOR){
    return LFB_THRESHOLD;
   } 
  if(port == MFL_SENSOR){
    return MFB_THRESHOLD;
  }

  if(port == RFL_SENSOR){
    return RFB_THRESHOLD;
  }

  if(port == LBL_SENSOR){
    return LBB_THRESHOLD;
  }
  if(port == MBL_SENSOR){
    return MBB_THRESHOLD;
  }
  if(port == RBL_SENSOR){
    return RBB_THRESHOLD;
  }

  if(port == LBALL_SENSOR){
    return LGREEN_THRESHOLD;
  }

  if(port == RBALL_SENSOR){
    return RGREEN_THRESHOLD;
  } 
 

}
int averageSensor(int port)
{
  int sum = 0;
  int i; 
  for(i =0; i<10; i++)
    {
      sum +=analog(port);
    }
  return (sum/10);
}

int getColor(int input){
  if(averageSensor(input) >= (getThreshold(input))){
    return BLACK; 
  }
  return WHITE;
}

void turnRight(){
  printf("turning Right %d %d %d\n", getColor(LL_SENSOR),getColor(ML_SENSOR),getColor(RL_SENSOR));
  CURRENT_STATE = GOING_RIGHT;
      goMotor(RMOTOR_2,25);
      goMotor(RMOTOR_1,25);
      goMotor(LMOTOR_1,50);
      goMotor(LMOTOR_2,50);
}

void hardRight(){
  printf("HARD RIGHT %d %d %d\n", getColor(LL_SENSOR),getColor(ML_SENSOR),getColor(RL_SENSOR));
  CURRENT_STATE = GOING_RIGHT;
      goMotor(RMOTOR_2,25);
      goMotor(RMOTOR_1,25);
      goMotor(LMOTOR_1,75);
      goMotor(LMOTOR_2,75);
}

void hardLeft(){
  printf("HARD LEFT %d %d %d\n",  getColor(LL_SENSOR),getColor(ML_SENSOR),getColor(RL_SENSOR));
  CURRENT_STATE = GOING_LEFT;
    goMotor(RMOTOR_2,75);
    goMotor(RMOTOR_1,75);
    goMotor(LMOTOR_1,25); 
    goMotor(LMOTOR_2,25);
}

void turnLeft(){
  printf("turning Left %d %d %d\n",  getColor(LL_SENSOR),getColor(ML_SENSOR),getColor(RL_SENSOR));
  CURRENT_STATE = GOING_LEFT;
    goMotor(RMOTOR_2,50);
    goMotor(RMOTOR_1,50);
    goMotor(LMOTOR_1,25); 
    goMotor(LMOTOR_2,25);
}

void goStraight(){
  printf("Straight\n");
  CURRENT_STATE = GOING_STRAIGHT;  
    goMotor(RMOTOR_2,25);
    goMotor(RMOTOR_1,25);
    goMotor(LMOTOR_1,25); 
    goMotor(LMOTOR_2,25);
 }

int hitSomething(){
  /*touch sensor logic appears to be inverted*/
  if(!digital(F_BUMPER)){
    return F_BUMPER;
  }
  
  if(!digital(B_BUMPER)){
    return B_BUMPER;
  }
  else{
    return 0;
  }
}

void lineFollow(){
  long lf_timeout;
  int impact = hitSomething();
  long delay;
  lf_timeout = mseconds();

      printf("impact %d\n",impact);
      while (!hitSomething()){
      printf("%d , %d, %d \n", analog(LL_SENSOR), analog(ML_SENSOR), analog(RL_SENSOR));
      if((getColor(LL_SENSOR) != BLACK) && 
	 (getColor(RL_SENSOR) != BLACK) &&
	 (getColor(ML_SENSOR) == BLACK))
      { 
	goStraight();
      }
      else if((getColor(LL_SENSOR) != BLACK) && 
	      (getColor(RL_SENSOR) == BLACK) && 
	      (getColor(ML_SENSOR) == BLACK))
	{ 
	  turnRight();
	}
      else if((getColor(LL_SENSOR) == BLACK) && 
	      (getColor(RL_SENSOR) != BLACK) && 
	      (getColor(ML_SENSOR) == BLACK))
	{ 
	  turnLeft();
	}
      else if((getColor(ML_SENSOR) != BLACK) &&
	      (getColor(LL_SENSOR) != BLACK) &&
	      (getColor(RL_SENSOR) == BLACK))
	{/*  \  | \|
	     \ | |\   right side is on line, center and left are off, this should be hard right*/
	  printf("HARD RIGHT%d %d %d\n",  getColor(LL_SENSOR),getColor(ML_SENSOR),getColor(RL_SENSOR));
	  delay = mseconds();
	  while(mseconds()-delay<3000L);
	  hardRight();
	  CURRENT_STATE = HARD_RIGHT;
	}
      else if((getColor(ML_SENSOR) != BLACK) &&
	      (getColor(RL_SENSOR) !=BLACK) && 
	      (getColor(LL_SENSOR) == BLACK))
	{/*  |/|  /
	     /| | /   left side is on line, center and right are off, this should be hard left*/
	  printf("HARD LEFT%d %d %d\n",  getColor(LL_SENSOR),getColor(ML_SENSOR),getColor(RL_SENSOR));
	  hardLeft();
	  CURRENT_STATE = HARD_LEFT;
	}
      else{
	printf("lost %d %d %d", getColor(LL_SENSOR), getColor(ML_SENSOR), getColor(RL_SENSOR));
	if(CURRENT_STATE == GOING_LEFT)
	  hardLeft();
	if(CURRENT_STATE == GOING_RIGHT)
	  hardRight();
	if(CURRENT_STATE == GOING_STRAIGHT)
	  goStraight();
	if(CURRENT_STATE == HARD_LEFT)
	  hardLeft();
	if(CURRENT_STATE == HARD_RIGHT)
	  hardRight();
      }

     
	if((CURRENT_STATE = DONE) && 
	   (getColor(LBL_SENSOR) == BLACK) && 
	   (getColor(RBL_SENSOR) == BLACK) &&
	   (getColor(LFL_SENSOR) == BLACK) && 
	   (getColor(RFL_SENSOR) == BLACK)
	   )
	  {
	    ao();
	    CURRENT_STATE = AT_HOME;
	    break;
	  }
      /*if you hit the top wall and you are on the line, start collecting balls*/
      
      }  
      if(CURRENT_STATE != GOING_HOME)
	{
	  CURRENT_STATE = SEARCHING;
	}
}

 int readBall(int sensor){
   int total =0;
   int i;
   total = averageSensor(sensor);
   if(OURBALL == GREEN){
     if(total> getThreshold(sensor))
       return 1;
     return 0;
   }
     if(OURBALL == WHITE){
       if((total/10)<getThreshold(sensor))
	 return 1;
       return 0;       
     }
 }
 void sortBalls(){
   long pause = 0L;
   goMotor(0,0);
   goMotor(1,0);
   goMotor(2,0);
   goMotor(3,0);
    
    servo(R_SERVO, R_DOWN);
    servo(L_SERVO, L_DOWN);
      printf("leftball = %d\n", readBall(LBALL_SENSOR));      
      printf("rightball= %d\n", readBall(RBALL_SENSOR));
      pause=      mseconds();
      while(mseconds() - pause < 3000L);
      if(!readBall(LBALL_SENSOR)){
	servo(L_SERVO, L_UP);
      }
      
      if(!readBall(RBALL_SENSOR)){
	servo(R_SERVO, R_UP);
      }
      pause = mseconds();
      goMotor(0, -25);
      goMotor(1, -25);
      goMotor(2, -25);
      goMotor(3, -25);
      while(((getColor(LL_SENSOR) != BLACK) || (getColor(RL_SENSOR) != BLACK)) && 
	    (getColor(ML_SENSOR) == BLACK));
      printf("entered black line");  
      while(((getColor(LL_SENSOR) != WHITE) || (getColor(RL_SENSOR) != WHITE)) && 
	    (getColor(ML_SENSOR) == BLACK));
      printf("past black line");  
      goMotor(0, 0);
      goMotor(1, 0);
      goMotor(2, 0);
      goMotor(3, 0);
      CURRENT_STATE = DONE;
 }

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

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