/*Code for The Dude*/

int sensor1 = 0, sensor2 = 0, sensor3 = 0;

void main()
{
  ir_transmit_off();

  enable_servos();
  servo(5,4000);
  servo(1,500);
  servo(3,100);
  
  start_press();

  servo(3,3850);
  msleep(200L);

  start_machine(16);

  ir_transmit_on();
  
  enable_servos();
  servo(5,4000);
  servo(1,500);
  servo(3,0);

  turning_function(orientation());
  
  car_function();

  servo(1,500);
  msleep(200L);

  /*servo delay for other robot*/
  msleep(1800L);

  servo(5,100);

  msleep(800L);

  motor(0,100);
  motor(2,100);

  msleep(500L);

  motor(0,-60);
  motor(2,-60);

  msleep(1000L);

  motor(0,0);
  motor(2,0);
  
  servo(5,4000);
  
  msleep(2000L);

  while(0 == 0)
    {

      servo(5,0);
      msleep(500L);

      motor(0,100);
      motor(2,100);
      msleep(250L);
      motor(0,0);
      motor(2,0);

      motor(0,-60);
      motor(2,-60);
      msleep(700L);
      motor(0,0);
      motor(2,0);
      
      servo(5,4000);

      msleep(4400L);
    }

}

/*WARNING - is 230 right?*/
void turn_cclockwise()
{
  servo(3, 2100);
  enable_encoder(7);
  reset_encoder(7);
  while (read_encoder(7) < 190) 
    {
      motor(1,100);
      motor(3,100);
    }
  motor(1,-100);
  motor(3,-100);
  msleep(50L);
  motor(1,0);
  motor(3,0);

  disable_encoder(7);
}

/*WARNING - is 330 right?*/
void turn_clockwise()
{
  servo(3,100);
  enable_encoder(7);
  reset_encoder(7);
  while (read_encoder(7) < 209) 
    {
      motor(1,-100);
      motor(3,-100);
    }
  motor(1,100);
  motor(3,100);
  msleep(50L);
  motor(1,0);
  motor(3,0);
  disable_encoder(7);
}

/*WARNING - is 500 right?*/
void turn_around()
{
  servo(3, 100);
  enable_encoder(7);
  reset_encoder(7);
  while (read_encoder(7) < 365) 
    {
      motor(1,-100);
      motor(3,-100);
    }
  motor(1,100);
  motor(3,100);
  msleep(50L);
  motor(1,0);
  motor(3,0);
  disable_encoder(7);

}

void turning_function(int orientation)
{
  if(orientation == 0)
    return();
  if(orientation == 1)
    turn_cclockwise();
  if(orientation == 2)
    turn_around();
  if(orientation == 3)
    turn_clockwise();

  msleep(100L);
}

/*there are 4 return values - 0,1,2,3
0 - correct orientation;
1 - pointing right
2 - pointing down
3 - pointing left*/
int orientation()
{
  /*A sensor is true for white, false for blue*/

  printf("%d %d %d", analog(3), analog(4), analog(5));
  
  /*WARNING here the threshold will be 125 - though this may not be true*/
  if (analog(3) < 230)
    sensor1 = 1;
  if (analog(4) < 201)
    sensor2 = 1;
  if (analog(5) < 230)
    sensor3 = 1;
  /*if on the white side*/
  if(((sensor1 + sensor2 + sensor3) == 2) || ((sensor1 + sensor2 + sensor3) == 3)) 
    {
      printf(" white");
      if(sensor1 == 0)
        return 0;
      if(sensor2 == 0)
        return 1;
      if(sensor3 == 0)
        return 3;
      return 2;
    }
  /*if on the blue side*/
  else
    {
      printf(" blue");
      if(sensor1 == 1)
        return 0;
      if(sensor2 == 1)
        return 1;
      if(sensor3 == 1)
        return 3;
      return 2;
    }
}

void car_function()
{
  motor(0,100);
  motor(2,100);
  
  servo(1,2000);
  msleep(300L);
  servo(1,1600);
  msleep(200L);
  arm_swing();
  arm_swing();
  arm_swing();

  motor(0,60);
  motor(2,60);

  msleep(300L);

  motor(0,0);
  motor(2,0);
}

void arm_swing()
{
  enable_servos();
  servo(1,3600);
  msleep(250L);
  servo(1,2200);
  msleep(250L);

}