// 6.270 Team 20
// Amy J Wooten (amy_jo)
// Linda Ye (lye)

#include <global.h>
#include <board.h>
#include <thread.h>
#include <math.h>

#define GYRO_PORT 11
#define BUMP_PORT 0
#define BACK_BUMP_PORT 2

#define MOTOR_LEFT 1
#define MOTOR_RIGHT 3

#define ARM_PORT 1
#define ARMS_OPEN 420
#define ARMS_CLOSED 150

#define GATE_PORT 4
#define GATE_IN 511
#define GATE_OUT 115

#define NE_IR 13
#define SE_IR 15
#define SW_IR 14

#define MVEL 180
#define TWEAK 23
 
uint16_t COLOR[4];                  /* color threshold (black is higher) */
int gyro_type = 300;				/* 150 for ADXRS150, 300 for ADXRS300 */
float lsb_ms_per_deg = 2025.0;		/* nominal scale factor depending on gyro type */
float theta;                		/* current angle, units explained in update_angle() */
float offset;               		/* gyro offset */
uint32_t time_ms;                   /* system time the last gyro reading was taken */
int dir;                            // heading towards 2 or 4 right now?

void calibrate_gyro(void) {
/*
   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;
  uint32_t start_time_ms;
  uint32_t int_time_ms = 5000L;

  printf("\nPlace robot,    press go.\n");
  goClick();
  printf("Stabilizing...\n");
  pause(500); /* Wait for robot to stabilize mechanically */
  printf("Calibrating     offset...\n");

  /* average some samples for offset */
  sum = 0.0;
  samples = 0;

  start_time_ms = get_time();
  while( ( get_time() - start_time_ms ) < int_time_ms ){
    sum += (float)analogRead(GYRO_PORT);
    samples++;
  }
  offset = sum / (float)samples;

  theta = 0.0;
  lcdClear();
  printf("Done calibration\n");
} /* calibrate_gyro() */

int update_angle(void) {
/* 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. */

  uint32_t delta_t_ms, new_time_ms, analog_value;

  time_ms = get_time();

  for(;;){
    analog_value = analogRead(GYRO_PORT);
    new_time_ms = get_time();
    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_value - offset) * (float)delta_t_ms; 

    time_ms = new_time_ms;
	yield();
  }
  return 0;
} /* update_angle() */

int display_angle(void) {
  for(;;){
    printf("%d degrees\n",(int)(theta / lsb_ms_per_deg));
    pause(500);
  }
  return 0;
} /* display_angle() */

void spin(int speed){
  motorSetVel(MOTOR_LEFT, -speed);
  motorSetVel(MOTOR_RIGHT, speed);
} /* spin() */

int sign(float a){
  if ( a > 0.0 ) return 1;
  else return -1;
} /* sign() */

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;

  tolerance = 1.0 * lsb_ms_per_deg;
  angle_target = angle * lsb_ms_per_deg;
  error = theta - angle_target;

  if ( fabs(error) > tolerance ) {

    spin(240 * -sign(error));
    while(fabs(error) > (80.0 * lsb_ms_per_deg)){
      error = theta - angle_target;
	  yield();
    }
    spin(200 * -sign(error));
    while(fabs(error) > (60.0 * lsb_ms_per_deg)){
      error = theta - angle_target;
	  yield();
    }
    spin(180 * -sign(error));
    while(fabs(error) > (10.0 * lsb_ms_per_deg)){
      error = theta - angle_target;
	  yield();
    }

    /* Fine adjust either direction as necessary */
    spin(160 * -sign(error));
    while(fabs(error) > tolerance){
      error = (theta - angle_target);
      spin(160 * -sign(error));
      yield();
    }

    spin(0);  

  } /* if ( fabs(error)... */
} /* go_to_angle() */

int isBlack(int port) {
  return analogRead(port) >= COLOR[port - 13];
}

int isWhite(int port) {
  return analogRead(port) < COLOR[port - 13];
}

void go_straight(float angle, int speed, long duration_ms){

/* The robot turns to the specified angle, then goes straight for 
   the specified duration. Note that speed must be greater than tweak, 
   and less than 255 - tweak. */

  uint32_t start_time;
  uint8_t tweak = 20;
  float angle_target;
  float tolerance;

  tolerance = lsb_ms_per_deg * 2.0; /* +/-0.5 degree accuracy */
  /* first turn the robot */
  go_to_angle(angle) ;

  /* then go forward under gyro control for the specified time */
  start_time = get_time();
  angle_target = angle * lsb_ms_per_deg; 
  
  while ((get_time() - start_time) < duration_ms){
    if (theta < (angle_target - tolerance)) {
      /* Turn CCW a little */
      motorSetVel(MOTOR_LEFT, speed - tweak);
      motorSetVel(MOTOR_RIGHT, speed + tweak);
    } else if (theta > (angle_target + tolerance)){
      /* Turn CW a little */
      motorSetVel(MOTOR_LEFT, speed + tweak);
      motorSetVel(MOTOR_RIGHT, speed - tweak);
    } else if (isBlack(SE_IR) && isBlack(SW_IR)) {
      // if we are going for the 2, and happen to be
      // going straight, we should cross the middle
      // line with both sensors at the same time
      
      // turn to middle now -- deposit balls into goal
      if (dir == 2) go_to_angle(angle - 90.0);
      dir = 0;
      go_straight(angle - 90.0, 200, 1000L);
      break;
    } else {
      motorSetVel(MOTOR_LEFT, speed);
      motorSetVel(MOTOR_RIGHT, speed);
    } 
    if (digitalGet(BUMP_PORT)) break;
    
    yield();
    
  } /* while */
  
  /* Throw it in reverse briefly to stop quickly */
  motorSetVel(MOTOR_LEFT, -250);
  motorSetVel(MOTOR_RIGHT, -250);
  pause(200L);  
  motorSetVel(MOTOR_LEFT, 0);
  motorSetVel(MOTOR_RIGHT, 0);  
} /* void go_straight() */

/**
 * turns itself towards closest ball set
 */
float orient() {
  // angles are 70 instead of 90 because gyro gets
  // confused after spinning 
  int b = 0, w = 0;
  if (isBlack(NE_IR)) b++; else w++;
  if (isBlack(SE_IR)) b++; else w++;
  if (isBlack(SW_IR)) b++; else w++;

  if (b > w) {
    if (isWhite(NE_IR)) 
    { dir = 4;
      return 70;  // backwards
    }
    else if (isWhite(SE_IR)) 
    { dir = 4; return 0; } // straight
    else if (isWhite(SW_IR)) 
    { dir = 2;  return 0; }
    else // turn towards 2 balls -- I don't like 180's
    { dir = 2; return -70; }
  } else {
    if (isBlack(NE_IR)) 
      { dir = 4; return 70; }
    else if (isBlack(SE_IR)) 
      { dir = 4; return 0; }
    else if (isBlack(SW_IR)) 
      { dir = 2; return 0; }
    else // turn towards 2 balls -- prevent 180's
      { dir = 2; return -70; }
  }
}

int kill(void){
    pause(29000L); //tweak this
    for(int i=0; i<6; i++){
        motorSetVel(i, 0);
    }
    halt();

    return 0;
}

int run_demo(void) {
  uint32_t start_time;
  
  servoSetPos(GATE_PORT, GATE_IN);
  int angle = orient();
  
  /* theta/lsb_ms_per_deg is the current angle in degrees */
  go_to_angle(angle);
  yield();
  go_straight( angle, 200, 3000L );
  
  // release balls
  servoSetPos(GATE_PORT, GATE_OUT);
  pause(1000L);
      
  if (dir == 4 || dir == 2) {
      // back up
      motorSetVel(MOTOR_LEFT, -210);
      motorSetVel(MOTOR_RIGHT, -210);
      start_time = get_time();
      while (!digitalGet(BACK_BUMP_PORT)) {
        if (get_time() - start_time > 3000L) {
          // we screwed up, stop
          break;
        }
      }
      
      motorSetVel(MOTOR_LEFT, 250);
      motorSetVel(MOTOR_RIGHT, 250);
      pause(200L);
      
      // ... and stop
      motorSetVel(MOTOR_LEFT, 0);
      motorSetVel(MOTOR_RIGHT, 0);
  } else {
    // miraculously, we saw the midline
    // turn towards 4 and continue
    motorSetVel(MOTOR_LEFT, -240);
    motorSetVel(MOTOR_RIGHT, -240);
    pause(600L);
    angle -= 90.0;
    servoSetPos(GATE_PORT, GATE_IN);
    go_to_angle(angle);
    yield();
    go_straight(angle, 200, 3000L);
    angle += 90.0;
    go_to_angle(angle);
    yield();
    go_straight(angle, 200, 3000L);
  }
  
  while(1);

  return 0;
}
void calibrateColor() {
  int i;
  
  // find the thresholds for black and white
  printf("\nBlack?");
  goClick();
  for (i = 0; i < 4; i++)
      COLOR[i] = analogRead(13 + i);
  printf("\nWhite?");
  goClick();
  for (i = 0; i < 4; i++)
      COLOR[i] = (COLOR[i] + analogRead(13 + i)) / 2;
  printf("\nThreshold: %d %d %d %d", COLOR[0], COLOR[1], COLOR[2], COLOR[3]);
  goClick();
}

// this is called during calibration
int usetup(void) {
    rf_set_team(20);

    calibrateColor();
	calibrate_gyro();
    return 0;
}

int umain(void) {
	create_thread(&update_angle, 64, 0, "update angle");
//	create_thread(&display_angle, 64, 0, "display angle");
	create_thread(&run_demo, 64, 0, "demo");
    create_thread(&kill,64,0,"kill");
	return 0;
}