/*
 *motion.c
 *Andrew McDonnell and Patrick Vinograd
 */

int LEFT_BUMP = 7;
int RIGHT_BUMP = 8;

int LEFT_MOTOR = 1;
int RIGHT_MOTOR = 2;

/*
LEFT_ENCODER = lego0 (expansion port 28)
RIGHT_ENCODER = lego1 (expansion port 29)
*/
int LEFT_ENCODER_FACTOR = 1;
int RIGHT_ENCODER_FACTOR = -1;


int left_encoder() {
  return LEFT_ENCODER_FACTOR * lego0_counts;
}

int right_encoder() {
  return RIGHT_ENCODER_FACTOR * lego1_counts;
}


/*
 *turn a certain angle.  Negative values are counter-clockwise, positive
 *values are clockwise
 */
void turn (int degrees) {
  int orig;
  if (degrees > 0) {      /*right turn*/
    orig = left_encoder();
    motor(1, CRUISE_SPEED);
    motor(2, (-1 * CRUISE_SPEED));
    while (left_encoder() < orig + (degrees/2)) {
      /*printf("\nLturn%d->%d", left_encoder(), (orig + (degrees/2)));
       */
      motor(1, CRUISE_SPEED);
      motor(2, (-1 * CRUISE_SPEED));
      printf(" ");
    }
    alloff();
  }
  else {                  /*left turn*/
    orig = right_encoder();
    motor(1, (-1 * CRUISE_SPEED));
    motor(2, CRUISE_SPEED);
    while (right_encoder() < orig - (degrees/2)) {
      /*printf("\nRturn:%d->%d", right_encoder(), (orig - (degrees/2)));
       */
      motor(1, (-1 * CRUISE_SPEED));
      motor(2, CRUISE_SPEED);
      printf(" ");
    }
    alloff();
  }
  /*sleep(0.6);*/
}

void move(int clicks) {
  int left_orig, right_orig;
  left_orig = left_encoder();
  right_orig = right_encoder();
  while ((left_encoder() < left_orig + clicks)
	 && (right_encoder() < right_orig + clicks)) {
    if (left_encoder() < left_orig + clicks)
      motor(1, LOW_SPEED);
    else
      alloff();
    if (right_encoder() < right_orig + clicks)
      motor(2, LOW_SPEED);
    else
      alloff();
  }
  alloff();
  
}


void show_counts() {
  while (1) {
    printf("\nL: %d, R: %d", left_encoder(), right_encoder());
    sleep(1.0);


  }
}

void print_counts() {
  printf("\nL: %d, R: %d", lego0_counts, lego1_counts);
}

void clear_counts() {
  lego0_counts = 0;
  lego1_counts = 0;
}

void test1() {
  start_press();
  clear_counts();
  start_press();
  print_counts();
}

/*
 *Return a scaling factor to adjust left/right motor speeds for straight
 *travel.  A negative factor will up the speed of the left motor. A positive
 *factor will up the speed of the right motor.  
 */
int compensate() {

}
/*
void main() {
  start_press();
  calibrate();
  motortest(65);
  
  test2();
  
}
*/

int diff;
int i;
void motortest(int basespeed) {
  int speed1 = basespeed;
  int speed2 = basespeed;
  motor(1, speed1);
  motor(2, speed2);
  for (i = 0; i < 30; i++) {
    clear_counts();
    sleep(0.5);
    diff = left_encoder() - right_encoder();
    speed1 -= diff;
    motor(1, speed1);
    motor(2, speed2);
    printf("\nL:%d/%d, R:%d/%d", speed1, left_encoder(), 
	   speed2, right_encoder());
  }
  alloff();
  printf("\nfinal:%d:%d", speed1, speed2);
}

void test2() {
  while (1) {
    start_press();
    clear_counts();
    start_process(waitForPress());
    motor(1, -58);
    motor(2, 58);
  }
}


void waitForPress() {
  stop_press();
  alloff();
  print_counts();
}

void calibrate() {
  int base = 60;
  
  int speed1;
  int speed2;
  int sum, j;
  float average;
  printf("\n");
  for (i = 0; i < 5; i++) {
    sum = 0;
    speed1 = base;
    speed2 = base;
    motor(1, speed1);
    motor(2, speed2);
    for (j = 0; j < 10; j++) {
      clear_counts();
      sleep(0.8);
      diff = left_encoder() - right_encoder();
      speed1 -= diff;
      motor(1, speed1);
      motor(2, speed2);
      sum += (speed1 - speed2);
    }
    alloff();
    base += 2;
    printf("%d,", sum); 
  }
  alloff();
}