/*
 * Control the gyroscope/motors. Much of the code
 * is stolen shamelessly from Howard Samuels.
 */

int gyro_type = 300;        /* 150 for ADXRS150, 300 for ADXRS300 */
float lsb_ms_per_deg;       /* nominal scale factor depending on gyro type */
float scale_factor = 1.017; /* adjust this for your particular gyro! */
float theta, theta_deg;     /* current angle, units explained
			       in update_angle() */
float offset;               /* gyro offset */
int gyro_port = 3;          /* gyro analog input port number */
int num_avgs = 800;         /* number of averages for calibration */
long time_ms;               /* system time the last gyro reading was taken */

int MAX_SPEED=80;           /* As fast as we'll let the robot go. */
float last_expected;        /* The expected current direction */

void gyro_config() {
  /*
    Averaging a number of readings only works when the gyro
    noise is greater than the converter resolution.  Since
    the xrs300 noise is far lower than the 20mV resolution
    of the 8-bit converter, a noise source has been added
    to the gyro board.  You should average for about 5
    seconds to minimize the effects of the added noise.
  */

  int samples;
  float sum;
  long start_time_ms;
  long int_time_ms = 5000L;

  if (gyro_type==300) 
    lsb_ms_per_deg = 256.0;
  else if (gyro_type==150) 
    lsb_ms_per_deg = 640.0;

  printf("\nPlace robot,   press start.\n");
  button(START);
  printf("\nStabilizing...\n");
  threadsafe_msleep(2000L); /* Wait for robot to stabilize mechanically */
  printf("\nCalibrating    offset...\n");

  /* average some samples for offset */
  sum = 0.0;
  samples = 0;
  start_time_ms = mseconds();
  while( ( mseconds() - start_time_ms ) < int_time_ms ){
    sum += (float)analog(gyro_port);
    samples++;
  }
  offset = sum / (float)samples;

  theta = 0.0;

  /* Don't really start updating angle until after the
     start of the round (thus the following line is
     commented out here but runs in 6jew70.c). */
  /* start_process(update_angle()); */
}

void update_angle(){
/* The units of theta are converter LSBs * ms
   1 deg/s = 5mV = 0.256 LSB for ADXRS300, 12.5mV = 0.64LSB for ADXRS150
   1 deg = 0.256LSB * 1000ms = 256 (stored in lsb_ms_per_deg)
   To convert theta to degrees, divide by lsb_ms_per_deg. */

  long delta_t_ms, new_time_ms;
  int i;

  time_ms = mseconds();

  for(;;){
    new_time_ms = mseconds();
    delta_t_ms = (new_time_ms - time_ms);

    /* This does the Riemann sum; CCW gyro output polarity is negative
       when the gyro is visible from the top of the robot. */
    theta -= ((float)analog(gyro_port) - offset) * (float)delta_t_ms; 
    theta_deg = theta / lsb_ms_per_deg;

    time_ms = new_time_ms;
    defer();
  }
}

void display_angle(){
  for(;;){
    printf("%d degrees\n",(int)(theta / lsb_ms_per_deg));
    threadsafe_msleep(200L);
    defer();
  }
} /* display_angle() */


int turning = 0, turnspeed = 0;
void go_to_angle(float angle) {
/* To convert degrees to units of theta, multiply by lsb_ms_per_deg.
   Demo robot turns at about 120 deg/s when speed = 100 */

  float angle_target;
  float error, tolerance;
  int i, j, bump;
  long last_check; float last_error;

  turning = 1;

  tolerance = lsb_ms_per_deg / 4.0;
  angle_target = angle * lsb_ms_per_deg;
  error = theta - angle_target;
  if (fabs(error) <= tolerance)
    return;

  for (i = 50; i < MAX_SPEED; i++) {
    last_error = theta - angle_target;
    spin(i * -sign(last_error));
    last_check = mseconds();
    while(mseconds() - last_check < 100L)
      defer();
    error = theta - angle_target;
    if (fabs(error - last_error) > 2.5*lsb_ms_per_deg)
      break;
  }
  turnspeed = i;

  bump = 0; last_check = mseconds();
  while (1) {
    if (mseconds() - last_check > 100L) {
      if (fabs(error - last_error) < lsb_ms_per_deg / 10.0)
	bump += 3;
      else if (bump > 0)
	bump -= 3;
      last_error = error;
      last_check = mseconds();
    }
    error = theta - angle_target;
    if (fabs(error) < tolerance)
      break;
    spin((turnspeed + bump) * -sign(error));
    printf("\n%f\n", error/lsb_ms_per_deg);
  } /* if ( fabs(error)... */
  spin(0);

  turning = 0;
} /* go_to_angle() */


void go_forward_straight(float angle, int speed, long time_ms, int forward){

/* Make sure the robot is pointing in the right direction before calling
  this routine. This routine makes the robot go straight up to the specified
  duration. Note that speed must be greater than tweak, and less than
  100 - tweak.  You can make a similar routine to back up.  Extra credit
  for making one routine that can do either. */

  long start_time;
  float tolerance, f_angle;
  int tweak = 15, dir;

  /* I get extra credit. -- gsivek */
  if (forward) {
    dir = 1;
    lmul = 1.0;
    rmul = 0.8;
  } else {
    dir = -1;
    lmul = 1.0;
    rmul = 1.0;
  }

  tolerance = lsb_ms_per_deg / 2.0; /* +/- 0.5 degree accuracy */
  start_time = mseconds();
  
  f_angle = (float)angle * lsb_ms_per_deg;
  
  while ((mseconds() - start_time) < time_ms){
    if (theta < (f_angle - tolerance)) {
      /* Turn CCW a little */
      set_motor_power(dir*speed - tweak, dir*speed + tweak);
    } else if (theta > ((float)angle + tolerance)){
      /* Turn CW a little */
      set_motor_power(dir*speed + tweak, dir*speed - tweak);
    } else {
      set_motor_power(dir*speed, dir*speed);
    }
  
    /* You can check a sensor here and terminate the while loop if necessary */

  } 
  
  /* Throw it in reverse to stop quickly */
  /*
    set_motor_power(-80*dir, -80*dir);
    msleep(20L);
  */
  set_motor_power(0, 0);  
}


void turn_left() {
  /* 90 degrees left */
  desired_angle += LEFT_TURN;
  /* go_to_angle(theta_deg + LEFT_TURN);*/
  go_to_angle(desired_angle);
}

void turn_right() {
  /* 90 degrees right */
  desired_angle += RIGHT_TURN;
  /* go_to_angle(theta_deg + RIGHT_TURN); */
  go_to_angle(desired_angle);
}

void turn_around() {
  /* 180 degrees */
  desired_angle += 2.0*LEFT_TURN;
  /* go_to_angle(theta_deg + 2.0*LEFT_TURN); */
  go_to_angle(desired_angle);
}

void spin(int speed){
  set_motor_power(-speed, speed);
} /* spin() */