void initialize(){
  enable_servos();
  servo(SERVO_ARM,ARM_HOLD);
  servo(SERVO_WHEEL,FRONT_WHEEL_STRAIGHT);
  enable_encoder(SHAFT_LEFT);
  enable_encoder(SHAFT_RIGHT);
}

void finalize(){
  ao();
  servo(SERVO_ARM,ARM_HOLD);
  servo(SERVO_WHEEL,FRONT_WHEEL_STRAIGHT);
  sleep(FRONT_WHEEL_WAIT);
  disable_servos();
}


void wheelLeft(int speed){
  motor(MOTOR_LEFT_FRONT,speed);
  motor(MOTOR_LEFT_BACK,speed);
  motor(4,speed);
}
void wheelRight(int speed){
  motor(MOTOR_RIGHT_FRONT,speed);
  motor(MOTOR_RIGHT_BACK,speed);
  motor(5,speed);
}


int isMyColor(int lightSensorID, int value){
  return ( ((myColor==BLUE) && (value < LIGHT_TRESHOLD[lightSensorID]))
	   || ((myColor==SAND) && (value > LIGHT_TRESHOLD[lightSensorID])));
}

int isBlue1(int lightSensorID){
  return (colorBeneath[lightSensorID] == BLUE);
}

int isSand1(int lightSensorID){
  return (colorBeneath[lightSensorID] == SAND);
}



int isBlue(int lightSensorID, int value){
  return (value < LIGHT_TRESHOLD[lightSensorID]);
}

int isSand(int lightSensorID, int value){
  return (value > LIGHT_TRESHOLD[lightSensorID]);
}



void readColor(){
  int valOff[4],valOn[4],i,sensor;

  /* Initialize values */
  for(sensor=0;sensor<4;sensor++){
    valOff[sensor]=0;
    valOn[sensor]=0;
  }

  /* Read values for several times */
  for(i=0;i<READ_LIGHT_TIME;i++){    
    /* Read LED-off values */
    for(sensor=0;sensor<4;sensor++){
      valOff[sensor] += analog(COLOR_PORT[sensor]);
    }
    set_digital_out(LED_SWITCH);
    sleep(LED_WAIT);
    
    /* Read LED-on values */
    for(sensor=0;sensor<4;sensor++){
      valOn[sensor] += analog(COLOR_PORT[sensor]);
    }
    
    clear_digital_out(LED_SWITCH);
    sleep(LED_WAIT);
  }

  /* Compute the difference -> store in valOff
     Also determine the colors and store in global colorBeneath */
  for(sensor=0;sensor<4;sensor++){
    valOff[sensor] -= valOn[sensor];
    if(isBlue(sensor,valOff[sensor])){
      colorBeneath[sensor]=BLUE;
    }else{
      colorBeneath[sensor]=SAND;
    }
  }
  
  printf("l=%d m=%d r=%d b=%d\n",
	 valOff[LIGHT_SENSOR_LEFT],
	 valOff[LIGHT_SENSOR_MID],
	 valOff[LIGHT_SENSOR_RIGHT],
	 valOff[LIGHT_SENSOR_BACK]);
	 
}


void straightFront(){
  servo(SERVO_WHEEL,FRONT_WHEEL_STRAIGHT);
  sleep(FRONT_WHEEL_WAIT);
}



void alignWithFrontWall(){
  float time;
  
  printf("Align Front Wall\n");

  reset_encoder(SHAFT_LEFT);
  reset_encoder(SHAFT_RIGHT);
  time = seconds();
  while( (read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT) < ALIGN_DIST)
	 && (!digital(ARM_BUMP_LEFT) || !digital(ARM_BUMP_RIGHT))
	 && (seconds()-time < ALIGN_TIMEOUT)){
    if(digital(ARM_BUMP_RIGHT)){
      servo(SERVO_WHEEL,3000);
      wheelLeft(ALIGN_SPEED);
      wheelRight(0);
    }else if(digital(ARM_BUMP_LEFT)){
      servo(SERVO_WHEEL,1000);
      wheelRight(ALIGN_SPEED);
      wheelLeft(0);
    }else{
      servo(SERVO_WHEEL,2000);
      wheelLeft(ALIGN_SPEED);
      wheelRight(ALIGN_SPEED);
    }   
    sleep(ALIGN_WAIT);
  }
  ao();
}


void alignWithBackWall(){
  float time;
  reset_encoder(SHAFT_LEFT);
  reset_encoder(SHAFT_RIGHT);

  printf("Align BACK Wall\n");

  time = seconds();
  while( (read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT) < ALIGN_DIST)
	 && ((!digital(BACK_BUMP_LEFT)) || (!digital(BACK_BUMP_RIGHT)))
	 && (seconds()-time < ALIGN_TIMEOUT)){
    if(digital(BACK_BUMP_RIGHT)){
      servo(SERVO_WHEEL,3000);
      wheelLeft(-ALIGN_SPEED);
      wheelRight(ALIGN_F_SPEED);
    }else if(digital(BACK_BUMP_LEFT)){
      servo(SERVO_WHEEL,1000);
      wheelRight(-ALIGN_SPEED);
      wheelLeft(ALIGN_F_SPEED);
    }else{
      servo(SERVO_WHEEL,2000);
      wheelLeft(-ALIGN_SPEED);
      wheelRight(-ALIGN_SPEED);
    }   
    sleep(ALIGN_WAIT);
  }
  ao();
}



int opponentFront(){
  return digital(BEACON_NORTH);
}

int opponentLeft(){
   return digital(BEACON_WEST);
}

int opponentBack(){
  return digital(BEACON_SOUTH);
}

int opponentRight(){
  return digital(BEACON_EAST); 
}





void AlignLeftWall(){
  while( !digital(ARM_BUMP_LEFT)){
    turn(CCW,10,TURN_SPEED);}
  turn(CW,30,TURN_SPEED);
}


void testback(){
  int i;
  servo(SERVO_WHEEL,FRONT_WHEEL_STRAIGHT);
  sleep(FRONT_WHEEL_WAIT);
  wheelLeft(-100);
  wheelRight(-100);
  sleep(0.3);
  ao();
  servo(SERVO_WHEEL,1000);
  wheelRight(-100);
  sleep(FRONT_WHEEL_WAIT*2.0);
  ao();
  servo(SERVO_WHEEL,3000);
  wheelLeft(-100);
  sleep(FRONT_WHEEL_WAIT*2.0);
  ao();
}


void testgo(){
  wheelLeft(100);
  wheelRight(100);
  sleep(1.0);
}


void testLight(){
  while(1){
    while(!stop_button()){}
    motor(0,15);motor(1,15);motor(2,15);motor(3,15);
    sleep(0.5);
    readColor();
    printf("l=%d, m=%d, r=%d, b=%d\n",colorBeneath[LIGHT_SENSOR_LEFT],colorBeneath[LIGHT_SENSOR_MID],colorBeneath[LIGHT_SENSOR_RIGHT],colorBeneath[LIGHT_SENSOR_BACK]);
    sleep(0.5);
    ao();
  }

}


void teststop(){
  enable_encoder(SHAFT_LEFT);
  enable_encoder(SHAFT_RIGHT);
  while(1){
    while(!stop_button()){
      printf("%d\n",analog(FRONT_DIST));
      sleep(0.3);
    }
    reset_encoder(SHAFT_LEFT);
    reset_encoder(SHAFT_RIGHT);
    wheelLeft(100);
    wheelRight(100);
    while(read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT)<100){
      printf("%d\n",analog(FRONT_DIST));
      sleep(0.02);
    }
    ao();
    while(!stop_button()){
      printf("%d\n",analog(FRONT_DIST));
      sleep(0.3);
    }
    reset_encoder(SHAFT_LEFT);
    reset_encoder(SHAFT_RIGHT);
    wheelLeft(100);
    wheelRight(100);
    while(read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT)<300){
      printf("%d\n",analog(FRONT_DIST));
      sleep(0.02);
    }
    ao();
  }
  finalize();
  return;
}