/* 640.0 for ADXRS150, 256.0 for ADXRS300 */
float lsb_ms_per_deg = 256.0;   /* nominal scale factor depending on gyro type */
float scale_factor = 1.017;     /* adjust this for your particular gyro! */
float theta;                    /* current angle, units explained in update_angle() */
float offset;                   /* gyro offset */
int gyro_port = 19;              /* 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 left = 3;                   /* motor ports */
int right = 1;
int whiteside;

int flsensor = 6;
int frsensor = 5;
int blsensor = 3;
int brsensor = 2;

int backbumper = 26;
int frontbumper = 29;
int leftbumper = 30;
int rightbumper = 27;
int bradj = -50;

int motorspeedfast = 70;
int motorspeedslow = 35;

/* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
       BEGIN GYRO CODES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

void calibrate_gyro(){
/*
   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;

  printf("Place robot,    press start.\n");
  /*start_press();*/
  printf("Stabilizing...\n");
  msleep(500L); /* Wait for robot to stabilize mechanically */
  printf("Calibrating     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;

} /* calibrate_gyro() */


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; 

    time_ms = new_time_ms;
    defer();
  }
} /* update_angle() */


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


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 speed;

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

  if ( fabs(error) > tolerance ) {

    spin(100 * -sign(error));
    while(fabs(error) > (80.0 * lsb_ms_per_deg)){
      error = theta - angle_target;
    }

    spin(70 * -sign(error));
    while(fabs(error) > (60.0 * lsb_ms_per_deg)){
      error = theta - angle_target;
    }

    spin(50 * -sign(error));
    while(fabs(error) > (10.0 * lsb_ms_per_deg)){
      error = theta - angle_target;
    }

    /* Slam on the brakes */
    spin(100 * sign(error));
    msleep(10L);

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

    spin(0);  

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

} /* go_to_angle() */


void spin(int speed){
  motor(left, -speed);
  motor(right, speed);
} /* spin() */


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

/* 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 100 - tweak. */

  long start_time;
  float angle_target;
  float tolerance;

  tolerance = lsb_ms_per_deg * 0.5; /* +/-0.5 degree accuracy */

  /* first turn the robot */
  go_to_angle(angle) ;

  /* then go forward under gyro control for the specified time */
  start_time = mseconds();
  angle_target = angle * lsb_ms_per_deg; 
  
  while ((mseconds() - start_time) < duration_ms){
    if (theta < (angle_target - tolerance)) {
      /* Turn CCW a little */
      motor(left, speed - tweak);
      motor(right, speed + tweak);
    } else if (theta > (angle_target + tolerance)){
      /* Turn CW a little */
      motor(left, speed + tweak);
      motor(right, speed - tweak);
    } else {
      motor(left, speed);
      motor(right, speed);
    }  
    /* You can check a sensor here and terminate the while loop */
  } /* while */
  
  /* Throw it in reverse briefly to stop quickly */
  motor(left, -100);
  motor(right, -100);
  msleep(20L);  
  motor(left, 0);
  motor(right, 0);
  
} /* void go_straight() */


void go_straight_to_line(float angle, int speed, int tweak){

/* 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 100 - tweak. */

  float angle_target;
  float tolerance;

  tolerance = lsb_ms_per_deg * 0.5; /* +/-0.5 degree accuracy */

  /* first turn the robot */
  go_to_angle(angle) ;

  /* then go forward under gyro control for the specified time */
  angle_target = angle * lsb_ms_per_deg; 
  
  while ( (islight(1,analog(flsensor))) && (islight(1,analog(frsensor))) )
  {
    if (theta < (angle_target - tolerance)) {
      /* Turn CCW a little */
      motor(left, speed - tweak);
      motor(right, speed + tweak);
    } else if (theta > (angle_target + tolerance)){
      /* Turn CW a little */
      motor(left, speed + tweak);
      motor(right, speed - tweak);
    } else {
      motor(left, speed);
      motor(right, speed);
    }  
  } /* while */
  
  /* Throw it in reverse briefly to stop quickly */
  motor(left, -100);
  motor(right, -100);
	  msleep(20L);  
  motor(left, 0);
  motor(right, 0);
  
} /* void go_straight() */


void go_open_loop( int speed, long duration_ms ){

/* Drive both motors at a specified speed and duration. */

  long start_time;

  start_time = mseconds();
  
  while ((mseconds() - start_time) < duration_ms){
    motor(left, speed);
    motor(right, speed); 
    /* You can check a sensor here and terminate the while loop if necessary */
  } /* while */
  
  /* Throw it in reverse briefly to stop quickly */
  motor(left, -100);
  motor(right, -100);
  msleep(20L);  
  motor(left, 0);
  motor(right, 0);
  
} /* void go_open_loop() */


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


float fabs(float a){
  if ( a > 0.0 ) return a;
  else return -a;
} /* fabs() */


/* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        END GYRO CODES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

float switchtofloat(int num)
{

/* also inverses 90 to -90 and vice versa because of orientation issues*/
  if(num == 0)
    return 0.;
  else if(num == 1)
    return 1.;
  else if(num == 90)
    return -90.;
  else if(num == 91)
    return -91.;
  else if(num == -90)
    return 90.;
  else if(num == -91)
    return 91.;
  else if(num == 180)
    return 180.;
  else if(num == 181)
    return 181.;
}



int reversedirection(int deg)
{
    if ( (deg <=  0))
    { return (deg + 180); }
    else
    { return (deg - 180); }
}

int orientit(int light[4])
{

/*front right opp color*/

int fl = 0;
int fr = 1;
int bl = 2;
int br = 3;
int retval;

/* array = [fl, fr, bl, br]

or array = [fl, fr,
            bl, br]

   0 means 3 black side
   1 means 3 white side
*/

    if ( (light[fl] == light[bl]) &&
         (light[bl] == light[br]) &&
         (light[bl] != light[fr]) )
    {
     if ( light[fr] == 1)
     { return  91; }
        else
        { return 0; }
    }

    if ( (light[fr] == light[bl]) &&
         (light[bl] == light[br]) &&
         (light[bl] != light[fl]) )
    {
     if ( light[fl] == 1)
     { return  1; }
        else
        { return -90; }
    }

    if ( (light[fl] == light[fr]) &&
         (light[fr] == light[br]) &&
         (light[bl] != light[fr]) )
    {
     if ( light[bl] == 1)
     { return  -91; }
        else
     { return  180; }

    }

    if ( (light[fl] == light[bl]) &&
         (light[bl] == light[fr]) &&
         (light[bl] != light[br]) )
    {
     if ( light[br] == 1)
     { return  181; }
        else
     { return  90; }
    }
}


int islight(int lite,int val)
{
  if(lite == 1)
  {
    if(val < 155)
    { return 1; }
    else
    { return 0; }
  }
  else
  {
    if(val >= 155)
    { return 1; }
    else
    { return 0; }
  }
}


int bumped(int port)
{
    return digital(port);
}


/* NOT USED

void startwhere()
{
    int orient;
    int aray[4];
    int whiteside;

    aray[0] = islight(1,analog(flsensor));
    aray[1] = islight(1,analog(frsensor));
    aray[2] = islight(1,analog(blsensor));
    aray[3] = islight(1,analog(brsensor)) + bradj;

    orient = orientit(aray);

    whiteside = orient % 90;
    orient = orient - whiteside;

printf("%d", orient);
printf("    %d", whiteside);
  start_press();


    if(whiteside == 1)
    {
     orient = reversedirection(orient);
    }
}
*/

void forward(int speed)
{
motor(right, speed);
motor(left, speed);
}


void backward(int speed)
{
	motor(right, -speed);
	motor(left, -speed);
}


float square(float x)
{
     return (x*x);
}


/*float distance(int x, int y)
{
     return (sqrt (square ((float) x) + square ((float) y)));
}
*/
/* %%%%%%%%%%%%%%%%%%%%%%%%%%%%
         BEGIN STRATEGY
%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

void strategy()
{

/* Pseudo-Code
(if comments are made like "now facing left"...assuming that you started as white team)
1) orient vertical and move downward/upward to always score the 4 balls adjacent to starting zone
2) back up to line (crosshairs)
3) rotate +90 (turn to face left)
4) drive to line
5) rotate 80* [yes 80]
6) drive forward and get to wall
6) ride along wall [use 2 bump sensors?]
	a) while loop... 
		i) if front bump sensor false turn left
		ii) if back one, turn right...
		iii) if both false turn left
	b) basically same thing as "drive straight" code
7) stop when hit wall
8) loop 5, 6, 7, 8

*/

/* 1) orient vertical and move downward/upward to always score the 4 balls adjacent to starting zone */

/* Done in prior code */

/* 2) back up to line (crosshairs) */
while( islight(0,analog(flsensor)) && (islight(0,analog(frsensor)) ))
{
backward(motorspeedfast);
}
ao();
while( (islight(1,analog(flsensor))) && (islight(1,analog(frsensor))) )
{
printf("backward\n");
    backward(motorspeedfast);
}
ao();
start_press();
while(islight(whiteside,analog(flsensor)))
{
 motor(left,-motorspeedslow); 
}
ao();
while(islight(whiteside,analog(frsensor)))
{
 motor(right,-motorspeedslow); 
}
ao();

theta = 0.;

/* 3) rotate +90 (turn to face left) */

go_to_angle(90.0);

/* 4) drive to line */

go_straight_to_line(90., 40, 5);




/* 5) rotate 80* [yes 80] */

go_to_angle(10.0);
ao();
/* 6) drive forward and get to wall */

while( !bumped(leftbumper) )
{
  forward(motorspeedfast);
}
ao();
/* 7) ride along wall [use 2 bump sensors?] */
/* 	a) while loop...  */
/* 		i) if front bump sensor false turn left */
/* 		ii) if back one, turn right... */
/* 		iii) if both false turn left */
/* 	b) basically same thing as "drive straight" code */

go_to_angle(0.);

/* 8) stop when hit line w/ back sensors */


while( (islight(whiteside,analog(blsensor))) && (islight(whiteside,analog(brsensor) + bradj)) )
{
  if(bumped(leftbumper))
  {
    forward(motorspeedfast);
  }
  else
  {
    motor(right, motorspeedfast);
    motor(left, motorspeedfast);
  }
}
ao();
while(islight(whiteside,analog(blsensor)))
{
 motor(left,motorspeedslow); 
}
ao();
while(islight(whiteside,analog(brsensor) + bradj))
{
 motor(right,motorspeedslow); 
}
ao();

/* 9) loop 5, 6, 7, 8 */

} /* end while 5,6,7,8 loop */


/* %%%%%%%%%%%%%%%%%%%%%%%%%%%%
          END STRATEGY
%%%%%%%%%%%%%%%%%%%%%%%%%%%% */






/* %%%%%%%%%%%%%%%%%%%%%%%%%%%%
          BEGIN MAIN
%%%%%%%%%%%%%%%%%%%%%%%%%%%% */

void main()
{

int orient = 0;
int aray[] = {0,0,0,0};
float orient2;

      rf_team = 9;
      rf_enable();
     start_machine();

  
     /* int mycoord[4]; */

/* %%%%%%%%%%%%%%%%%%%  
    BEGIN GYRO CODING
   %%%%%%%%%%%%%%%%%%% */

  lsb_ms_per_deg *= scale_factor;

  calibrate_gyro();
  start_process(update_angle()); 
  /* Don't set theta, just read it! */
  /* start_process(display_angle()); */

printf("clear\n");
printf("passed\n");





/* %%%%%%%%%%%%%%%%%%%  
     END GYRO CODING
   %%%%%%%%%%%%%%%%%%% */

while( (orient != 1) && (orient != 3) ) 
{
    aray[0] = (analog(flsensor));
    aray[1] = (analog(frsensor));
    aray[2] = (analog(blsensor));
    aray[3] = (analog(brsensor)) + bradj;
printf("%d ", aray[0]);
printf("%d ", aray[1]);
printf("%d ", aray[2]);
printf("%d\n", aray[3]);
aray[0] = islight(0,aray[0]);
aray[1] = islight(0,aray[1]);
aray[2] = islight(0,aray[2]);
aray[3] = islight(0,aray[3]);


    orient = (aray[0] + aray[1] + aray[2] + aray[3]);

}

    orient = orientit(aray);

    whiteside = orient % 90;
    orient = orient - whiteside;

printf("%d ", aray[0]);
printf("%d ", aray[1]);
printf("%d ", aray[2]);
printf("%d ", aray[3]);

printf("change= %d", orient);
printf("   white= %d", whiteside);  


orient2 = switchtofloat(orient);
if (orient2 !=0.)
{     go_to_angle(orient2);  }

go_straight(orient2, -motorspeedfast, 1300L, 5);

ao();
sleep(.5);
/* &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
           START FORWARD 90 SEQUENCE  */
theta = 0.;

forward(motorspeedfast);
sleep(.4);
ao();

go_to_angle(-20.);
while(1)
{
    sleep(.5);
    while(bumped(frontbumper) == 0)
    {
        forward(motorspeedfast);
    }
    theta=0.;
    go_straight(0., -motorspeedfast, 500L, 5);
    go_to_angle(90.);
}

/* printf(" restart\n");
   start_press();
*/
/* strategy(); */

/*  go_straight(orient2, 50, 2500L, 5); */

}