#define leftmotor1 0
#define leftmotor2 2
#define leftmotor3 5
#define rightmotor1 1
#define rightmotor2 3
#define rightmotor3 4
#define left_back_bump_port 15
#define right_back_bump_port 13
#define left_bump_port 13
#define servo_port 0
#define servo_center 1795 /* Old 1810 */
#define servo_right 1975 /*Less steep was 2030*/
#define servo_left 1605 /*Less steep was 1680*/ 
#define servo_slight_left 1765
#define servo_slight_right 1816
#define servo_right_back 1645 /* was 1680 */
#define servo_left_back 1805 /* this needs to be at least this steep */
#define servo_turn_left 0
#define servo_turn_right 3685
#define slow_delay .025
#define slow_delay_back 0.
persistent int servo_direct=0;
float servo_sleep=.5;
float turntime=.175;
float drivetime=1.0;
int turnticks_right=5;
int turnticks_left=4;
int lastServoPos=-1;

void move_servo(int pos) {
  servo(servo_port, pos);
  sleep((float)(abs(lastServoPos - pos)) / 7500.0);
  lastServoPos = pos;
}

void drive(int direction) {
  if(direction==0) {
    move_servo(servo_center);
    servo_direct=0;
  } else if(direction==1) {
    move_servo(servo_slight_left);
    servo_direct=-1;
  } else if(direction==2) {
    move_servo(servo_right);
    servo_direct=1;
  } else if(direction==3) {
    move_servo(servo_left);
    servo_direct=-1;
  } else if(direction==4) {
    move_servo(servo_right_back);
    servo_direct=-1;
  } else if(direction==5) {
    move_servo(servo_left_back);
    servo_direct=1;
  } else if(direction==6) {
    move_servo(servo_slight_right);
    servo_direct=1;
  } else {
    move_servo(servo_center);
    servo_direct=0;
  }
  if((direction==5)|(direction==4)|(direction==0)) {
    fd(leftmotor1);
    bk(rightmotor1);      
    sleep(slow_delay_back);
    fd(leftmotor2);
    bk(rightmotor2);
    sleep(slow_delay_back);
    fd(leftmotor3);
    bk(rightmotor3);
  } else {
    bk(leftmotor1);
    fd(rightmotor1);
    sleep(slow_delay);
    bk(leftmotor2);
    fd(rightmotor2);
    sleep(slow_delay);
    bk(leftmotor3);
    fd(rightmotor3);      
  }
}

void stop() {
  ao();
  sleep(.05);
}

void turn(int direction) {
  if(servo_direct>0) {
    move_servo(servo_turn_right);
    servo_direct=1;
  } else {
    move_servo(servo_turn_left);
    servo_direct=-1;
  }
  if(direction) {
    bk(leftmotor1);
    bk(leftmotor2);
    bk(leftmotor3);
    bk(rightmotor1);
    bk(rightmotor2);
    bk(rightmotor3);
  } else {
    fd(leftmotor1);
    fd(leftmotor2);
    fd(leftmotor3);
    fd(rightmotor1);
    fd(rightmotor2);
    fd(rightmotor3);
  }
}

int drive_dist(int direction,int ticks,float time) {
  int right_final=read_encoder(rightshaft_port)+ticks,left_final=read_encoder(leftshaft_port)+ticks;
  float time_final=seconds()+time;
  drive(direction);
  if(time<.1)
    while((read_encoder(rightshaft_port)<right_final)+(read_encoder(leftshaft_port)<left_final)) {defer();}
  else {
    while(((read_encoder(rightshaft_port)<right_final)+(read_encoder(leftshaft_port)<left_final))&&(seconds()<time_final)) {defer();}
    stop();
    return(seconds()>time_final);
  }
  stop();
}

void turn_dist(int direction,int ticks,float time) {
  int right_final=read_encoder(rightshaft_port)+ticks,left_final=read_encoder(leftshaft_port)+ticks;
  float time_final=seconds()+time;
  turn(direction);
  if(time<.1) /* Below a threshold now */
    while((read_encoder(rightshaft_port)<right_final)|(read_encoder(leftshaft_port)<left_final)) {defer();}
  else {
    while(((read_encoder(rightshaft_port)<right_final)|(read_encoder(leftshaft_port)<left_final))&(seconds()<time_final)) {defer();}
    if(seconds()>=time_final)
      printf("\nTURN TIME OUT!!!");
  }
  stop();
}

int waitforbackbump(float time_out) {
  int a=0, b=0, attempt=0;
  float tryagaintime=seconds()+time_out;
  while(!(a&&b)&&(attempt<=1)) {
    if(seconds()>tryagaintime) {
      attempt++;
      printf("\nTrying BACKBUMP Again! %d %d %d", attempt, a, b);
      drive_dist(7,4,1.);
      drive(0);
      if(attempt>1) {
	sleep(1.5);
	return 1;
      }
      a=0;
      b=0;
      tryagaintime=seconds()+1.5;
    } else {
      if(a) {
	move_servo(servo_left);
	servo_direct=-1;
      }
      if(b) {
	move_servo(servo_right);
	servo_direct=1;
      }
      if(digital(left_back_bump_port)) {
	off(leftmotor1);
	off(leftmotor2);
	off(leftmotor3);
	a=1;
      }
      if(digital(right_back_bump_port)) {
	off(rightmotor1);
	off(rightmotor2);
	off(rightmotor3);
	b=1;
      }
    }
    defer();
  }
  stop();
  return 0;
}

void swerve_left(int length) {
  custom_back(1325,length,1.5);
  custom_back(2210,length,1.5);
}

void swerve_right(int length) {
  custom_back(2280,length,1.5);
  custom_back(1290,length,1.5);
}

void swerve_forw_left(int length) {
  custom_forw(1240,length,1.5); /* was 1250 */
  custom_forw(2330,length,1.5); /* was 2290 */
}

void swerve_forw_right(int length) {
  custom_forw(2240,length,1.5);
  custom_forw(1300,length,1.5);
}