/* Functions:
  activate_robot
  forward
  forwardcurve
  forwardslightleft
  forwardslightright
  backward
  backslow
  backwardcurve
  backwardslightleft
  backwardslightright
  turnleft
  turnright
  turnleft90
  turnright90
  leftservodegrees
  rightservodegrees
  alignstraight
  alignturn
  armcentered
  backarmup
  frontarmup
  leftmotors
  rightmotors
  bothmotors
*/

/*****************
 * Basic driving *
 *****************/

#define leftservoleft90 100
#define leftservoleft45 1100
#define leftservocenter 1940
#define leftservoright45 2900
#define leftservoright90 3800
#define rightservoleft90 200
#define rightservoleft45 1270
#define rightservocenter 2170
#define rightservoright45 3200
#define rightservoright90 4000
#define armservobothup 500
#define armservobackup 600
#define armservofrontup 3000

#define right90time 450
#define left90time 450

void activate_robot() {
  enable_servos();
  enable_encoder(7);
  enable_encoder(8);
  alignstraight();
  frontarmup();
}

void leftmotors(int power) {
  motor(0,power);
  motor(1,power);
}

void rightmotors(int power) {
  motor(2,power);
  motor(3,power);
}

void bothmotors(int power) {
  leftmotors(power);
  rightmotors(power);
}

void forward(int power) {
  alignstraight();
  bothmotors(power);
}

void forwardcurve(int leftpower, int rightpower, int leftservo, int rightservo) {
  leftservodegrees(leftservo);
  rightservodegrees(rightservo);
  msleep(100L);
  leftmotors(leftpower);
  rightmotors(rightpower);
}

void forwardslightleft(int power) {
  if (power > 50) {  /* to make sure doesn't go over limit */
    leftmotors(power);
    rightmotors(power);
  }
  else {
    leftmotors(power);
    rightmotors(power);
  }
  leftservodegrees(-3);
  rightservodegrees(-3);    
  alignstraight();
}

void forwardslightright(int power) {
  if (power > 50) {  /* to make sure doesn't go over limit */
    leftmotors(power);
    rightmotors(power);
  }
  else {
    leftmotors(power);
    rightmotors(power);
  }
  leftservodegrees(3);
  rightservodegrees(3);
  alignstraight();
}


void backward(int power) {
  alignstraight();
  if (power > 60) {
    backslow();
    msleep(200L);
  }
  leftmotors(-1*power);
  rightmotors(-1*power);
}

void backslow() {
  alignstraight();
  bothmotors(-60);
}

void backwardcurve(int leftpower, int rightpower, int leftservo, int rightservo) {
  leftservodegrees(leftservo);
  rightservodegrees(rightservo);
  msleep(100L);
  leftmotors(-1*leftpower);
  rightmotors(-1*rightpower);
}

void backwardslightleft(int power) {
  if (power > 50) {  /* to make sure doesn't go over limit */
    leftmotors(-1*power);
    rightmotors(-1*power);
  }
  else {
    leftmotors(-1*power);
    rightmotors(-1*power);
  }
  leftservodegrees(-3);
  rightservodegrees(-3);
}

void backwardslightright(int power) {
  if (power > 50) {  /* to make sure doesn't go over limit */
    leftmotors(-1 * power);
    rightmotors(-1 * power);
  }
  else {
    leftmotors(-1 * power);
    rightmotors(-1 * power);
  }
  leftservodegrees(3);
  rightservodegrees(3);
}

void turnright(int power) {
  alignturn();
  turnslow(1);
  leftmotors(power);
  rightmotors(-1*power);
}

void turnleft(int power) {
  alignturn();
  turnslow(2);
  leftmotors(-1*power);
  rightmotors(power);
}

void turnslow(int direction) {
  if (direction == 1) {
    leftmotors(40);
    rightmotors(-40);
  }
  else {
    leftmotors(-40);
    rightmotors(40);
  }
  msleep(100L);
}

void turnright90() {
  alignturn();
  turnright(60);
  whileencoders(3,89);
  ao();
}

void turnleft90() {
  alignturn();
  turnleft(60);
  whileencoders(4,102);
  ao();
}

void turnleft85() {
  alignturn();
  turnleft(60);
  whileencoders(4,80);
  ao();
}

void turnleft95() {
  alignturn();
  turnleft(60);
  whileencoders(4,100);
  ao();
}


/* degrees from -45 to 45, with -45 being servo toward left, 45 to right */
void leftservodegrees(int degrees) {
  if (degrees > 0)
    servo(0,leftservocenter + (int) ((float) (degrees) * (float)(leftservoright90-leftservocenter)/90.));
  else
    servo(0,leftservocenter + (int) ((float) (degrees) * (float)(leftservocenter - leftservoleft90)/90.));
  
  /*  if (degrees == 0)
      servo(0,leftservocenter);
      else
      servo(0,(int)((float)(degrees+45)*(float)(leftservoright45 - leftservoleft45)/90.) + leftservoleft45); */
  
}




void rightservodegrees(int degrees) {
  if (degrees > 0)
    servo(1,rightservocenter + (int) ((float) (degrees) * (float)(rightservoright90-rightservocenter)/90.));
  else
    servo(1,rightservocenter + (int) ((float) (degrees) * (float)(rightservocenter - rightservoleft90)/90.));
  /*
    if (degrees == 0)
    servo(1,rightservocenter);
    else
    servo(1,(int)((float)(degrees+45)*(float)(rightservoright45 - rightservoleft45)/90.) + rightservoleft45);
  */
}
 
void alignstraight() {
  servo(0,leftservocenter);
  servo(1,rightservocenter);
  msleep(200L);
}

void alignturn() {
  servo(0,leftservoright45);
  servo(1,rightservoleft45);
  msleep(200L);
}

void armcentered() {
  servo(2,2000);
  msleep(200L);
}

void backarmup() {
  servo(2,armservobackup);
  msleep(200L);
}

void frontarmup() {
  servo(2,armservofrontup);
  msleep(200L);
}

void botharmup() {
  servo(2,armservobothup);
  msleep(200L);
}

void wallturn90(int direction, int power) {
  printf("0");
  alignstraight();
  printf("1");
  if (power > 0)
    forward(40);
  else
    backward(40);
  printf("2");
  whileencoders(5,10);
  printf("3");
  ao();

  reset_encoders();
  if (direction == 1) { /* turn left */
    printf("going left\n");
    forwardcurve(0,power,90,-45);
    while(encoder(2) < 220)
      printf("in loop\n");
    ;
    ao();
    printf("done!\n");
  }
  else {
    forwardcurve(power,0,45,-90);
    while(encoder(1) < 200)
      ;
    ao();
    printf("done!\n");
  }
  ao();
    printf("done! with all\n");
}