#define DRIVE_LEFT_SERVO 0
#define DRIVE_RIGHT_SERVO 1
#define LEFT_MOTOR_1 2
#define LEFT_MOTOR_2 3
#define RIGHT_MOTOR_1 0
#define RIGHT_MOTOR_2 1

persistent int DRIVE_LEFT_SERVO_CENTER;
persistent int DRIVE_LEFT_SERVO_PERP ;
persistent int DRIVE_RIGHT_SERVO_CENTER;
persistent int DRIVE_RIGHT_SERVO_PERP;

void calibrate_drive() {
  printf("Set Servo:      Left, Perp\n");
  start_press();
  DRIVE_LEFT_SERVO_PERP = servo_getvalue(DRIVE_LEFT_SERVO);
  printf("Set Servo:      Left, Center\n");
  start_press();
  DRIVE_LEFT_SERVO_CENTER = servo_getvalue(DRIVE_LEFT_SERVO);
  printf("Set Servo:      Right, Perp\n");
  start_press();
  DRIVE_RIGHT_SERVO_PERP = -servo_getvalue(DRIVE_RIGHT_SERVO);
  printf("Set Servo:      Right, Center\n");
  start_press();
  DRIVE_RIGHT_SERVO_CENTER = servo_getvalue(DRIVE_RIGHT_SERVO);

}

void init_drive() {
  DRIVE_LEFT_SERVO_CENTER=1950;
  DRIVE_LEFT_SERVO_PERP=15;
  DRIVE_RIGHT_SERVO_CENTER=1740;
  DRIVE_RIGHT_SERVO_PERP=3540;
  servo(DRIVE_LEFT_SERVO, DRIVE_LEFT_SERVO_CENTER);
  servo(DRIVE_RIGHT_SERVO, DRIVE_RIGHT_SERVO_CENTER);
}

void steer_straight() {
  servo(DRIVE_LEFT_SERVO, DRIVE_LEFT_SERVO_CENTER);
  servo(DRIVE_RIGHT_SERVO, DRIVE_RIGHT_SERVO_CENTER);
}

void steer_sideways() {
  servo(DRIVE_LEFT_SERVO, DRIVE_LEFT_SERVO_PERP);
  servo(DRIVE_RIGHT_SERVO, DRIVE_RIGHT_SERVO_PERP);
}

void steer_halfway() {
  servo(DRIVE_LEFT_SERVO, (DRIVE_LEFT_SERVO_PERP + DRIVE_LEFT_SERVO_CENTER) / 2);
  servo(DRIVE_RIGHT_SERVO, (DRIVE_RIGHT_SERVO_PERP + DRIVE_RIGHT_SERVO_CENTER) / 2);
}

void drive_left(int speed) {
  motor(LEFT_MOTOR_1, speed);
  motor(LEFT_MOTOR_2, speed);
}
void drive_right(int speed) {
  motor(RIGHT_MOTOR_1, speed);
  motor(RIGHT_MOTOR_2, speed);
}
void drive_forward(int speed) {
  drive_left(speed);
  drive_right(speed);
}

void drive_sideways(int speed) {
  drive_left(speed);
  drive_right(-speed);
}

void drive_spin(int speed) {
  drive_left(speed);
  drive_right(-speed);
}

void drive_forward_encoded(int speed, float duration) {

  int count = 0;
  int tolerance = 2;

  enable_encoder(7);
  enable_encoder(8);
  reset_encoder(7);
  reset_encoder(8);

  drive_left(speed);
  drive_right(speed);
  
  if(speed != 0)
    {
      while(count++ < (int)(duration*10.0))
	{
	  if(read_encoder(7) > read_encoder(8) + tolerance) {
	    /* Left is moving too fast */
	    drive_left(speed - 10);
	    drive_right(speed + 10);
	  }
	  if(read_encoder(8) > read_encoder(7) + tolerance) {
	    drive_left(speed + 10);
	    drive_right(speed - 10);
	  }	    
	  sleep(.1);
	}
    }
  drive_forward(0);
}

void drive_backwall() {
  drive_forward(-50);
  sleep(3.0);
  drive_forward(0);
}


void drive_alignforward() {
  
  int max_dist_left = 10;
  int max_dist_right = 10;
  int cont = 2;
  int timeout = 0;
  int left = 1, right = 1;

  enable_encoder(7);
  enable_encoder(8);
  reset_encoder(7);
  reset_encoder(8);	       

  drive_forward(30);

  while(cont && timeout++ < 500)
    {
      if(read_encoder(7) >= max_dist_left && left)
	{
	  drive_left(0);
	  cont--;
	  left = 0;
	}
      if(read_encoder(8) >= max_dist_right && right)
	{
	  drive_right(0);
	  cont--;
	  right = 0;
	}
      sleep(.01);
    }

  drive_forward(0);
}

int drive_sideways_first_ball(int side) {

  steer_halfway();
  sleep(.3);
  steer_sideways();
  sleep(0.5);

  enable_encoder(7);
  enable_encoder(8);
  reset_encoder(7);
  reset_encoder(8);

  if(side == SIDE_WHITE)
    drive_sideways(40);
  else
    drive_sideways(-40);

  sleep(3.0);
  drive_sideways(0);

  return (read_encoder(7) + read_encoder(8));
}

void drive_back_sideways(int clicks, int side) {
  if(side == SIDE_WHITE)
    drive_sideways_clicks(-30, clicks * 7 / 10, 200);
  else
    drive_sideways_clicks(30, clicks * 7 / 10, 200);
}

void drive_sideways_clicks(int speed, int clicks, int timeout)
{
  int count = 0;

  enable_encoder(7);
  enable_encoder(8);
  reset_encoder(7);
  reset_encoder(8);

  drive_sideways(speed);

  while((read_encoder(7) + read_encoder(8)) < clicks && count++ < timeout)
    {
      sleep(.01);
    }


  drive_sideways(0);
}

void drive_straight_clicks(int speed, int clicks, int timeout)
{
  int count = 0;
  int threshold = 1;

  enable_encoder(7);
  enable_encoder(8);
  reset_encoder(7);
  reset_encoder(8);

  drive_forward(speed);

  while((read_encoder(7) + read_encoder(8)) < clicks && count++ < timeout)
    {
      if(read_encoder(7) > read_encoder(8) + threshold)
	{
	  drive_left(speed - 10);
	  drive_right(speed + 10);
	}
      else if(read_encoder(8) > read_encoder(7) + threshold)
	{
	  drive_left(speed + 10);
	  drive_right(speed - 10);
	}
      else
	{
	  drive_left(speed);
	  drive_right(speed);
	}

      sleep(.01);
    }


  drive_forward(0);
}


int drive_find_barrier(int side) {
  
  int timeout = 0;
  int plan = PLAN_C;
  int threshold = 1;
  int total_clicks = 25;

  reset_encoder(7);
  reset_encoder(8);

  drive_forward(25);

  while(!digital(BUMP_LEFT_WHEEL) && !digital(BUMP_RIGHT_WHEEL) && timeout++ < 200
	&& (read_encoder(7) + read_encoder(8) < total_clicks)) {
    if(read_encoder(7) > read_encoder(8) + threshold)
      {
	drive_left(20);
	drive_right(30);
      }
    else if(read_encoder(8) > read_encoder(7) + threshold)
      {
	drive_right(30);
	drive_left(20);
      }
    else
      {
	drive_left(25);
	drive_right(25);
      }

    sleep(.01);
  }

  drive_forward(0);

  if(digital(BUMP_RIGHT_WHEEL))
    {
      /* Found barrier in left near position
	 White: Plan B
	 Black: Plan A
      */
      if(side == SIDE_WHITE)
	plan = PLAN_B;
      else
	plan = PLAN_A;
    }
  else if(digital(BUMP_LEFT_WHEEL))
    {
      /* Found barrier in right near position
	 White: Plan A
	 Black: Plan B
      */
      if(side == SIDE_WHITE)
	plan = PLAN_A;
      else
	plan = PLAN_B;
    }
  else
    {
      /* No barrier-- in far position
	 Either way: Plan C
      */
      plan = PLAN_C;
    }

  /* Back to wall */
  drive_forward(-30);
  sleep(3.5);
  drive_forward(30);
  sleep(0.5);
  drive_forward(0);

  return plan;
}

/* Anchors left wheel and runs right */
int drive_around_left(int speed, int clicks, int timeout)
{
  int count = 0;

  enable_encoder(7);
  enable_encoder(8);
  reset_encoder(7);
  reset_encoder(8);

  drive_left(0);
  drive_right(speed);

  while((read_encoder(7) + read_encoder(8)) < clicks && count++ < timeout)
    {
      sleep(.01);
    }


  drive_forward(0);
}

/* Anchors right wheel and runs left */
int drive_around_right(int speed, int clicks, int timeout)
{
  int count = 0;

  enable_encoder(7);
  enable_encoder(8);
  reset_encoder(7);
  reset_encoder(8);

  drive_right(0);
  drive_left(speed);

  while((read_encoder(7) + read_encoder(8)) < clicks && count++ < timeout)
    {
      sleep(.01);
    }


  drive_forward(0);
}