/*Library  for basic robot motion - 6.270 Team 11- Alex, Hilary, Kat*/
int navigatespeed = 60; /* Speed at which the drive routines operate */
long navigatestepsize = 500L; /* Number of msecs between legs */
int stuckdist = 25;
int teamsign;

/*ir globals*/
int ir_sensor= 6;
int  black_thresh = 150; /*Set by calibrate_ir, default  */
int  white_thresh = 50;/* Set by calibrate_ir, default */
int ir_thresh = 25; /*added to white, subt. from black during calibration*/

/*Gyro-related globals, thanks to Howard Samuels, 11/16/2004 
  howard.samuels@analog.com */
int gyro_type = 300;        /* 150 for ADXRS150, 300 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 expl. 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 */
/*motor globals*/
int rightmotor = 2;
int leftmotor = 1;
/*servo globals*/
int rightcaster = 1;       /* Port number for right caster */
int rc_center = 2250;
float rc_mult = 1750./90.; 
int leftcaster = 2;        /* Port number for left caster */
int lc_center  = 1880;
float lc_mult  = 1680./90.;
int ballgate = 0;        /* Port number for ball gate servo */
int bg_start = 4000;         /*Servo value to allow balls in */  
float bg_mult = 2400./360.;
int pusher = 5;           /* Port number for pusher */
int pusher_min = 600;
int pusher_max = 2500;
float pusher_ext_time = 1.0; /*Time between when the pusher is 
			       extended and when we pull it back */
/*rf globals */
int teamnumber; /*will be zero or one, set by determine_teamnumber*/
int ourcolor; /*white = 0, black = 1 */
/*other globals*/
float pi = 3.1415926535;  /*mmm, pi */

/*gyro thread functions -------------------------------------------------*/
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(){
  /*don't use if you want to see other things on the screen */
  for(;;){
    printf("%d degrees\n",(int)(theta / lsb_ms_per_deg)); 
    msleep(200L);
    defer();
  }
} /* display_angle() */



/*motion functions--------------------------------------------------------*/ 

void backup(int speed, float dist)
{
  float fdur;
  float speedconvert = 1.60; /*convert from motor units to boardunits/s*/
  /*This is essentially the motor top speed in boardunits/s divided by 100*/
  float speed_bu;
  leftcasterangle(0.0);
  rightcasterangle(0.0);
  /*dist = speed*dur*/
  speed_bu = (float)speed * speedconvert; /*convert to ms*/
  fdur = (dist/speed_bu);
  motor(leftmotor, -speed);
  motor(rightmotor, -speed);
  sleep(fdur);
  ao();
}

void forward(int speed, float dist)
{
 float fdur;
  long dur;
  float speedconvert = 1.3; /*convert from motor units to boardunits/s*/
  /*This is essentially the motor top speed in boardunits/s divided by 100*/
  float speed_in_units_per_msec;
  /*dist = speed*dur*/
  speed_in_units_per_msec= .001* (float)speed * speedconvert; /*convert to ms*/
  fdur = (dist/speed_in_units_per_msec);
  dur = (long)fdur;
  /*printf("fdur = %f\n", fdur);*/
  timedrive(speed, dur);
}

void timedrive(int speed, long dur){
  /* 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;
  int tweak = 10;
  float angle_target = theta; /*We want to keep going where we're pointed,
				but theta will be changing, so take it's current
				value*/
  float tolerance;
  rightcasterangle(0.0);
  leftcasterangle(0.0);
  tolerance = lsb_ms_per_deg * 0.5; /* +/-0.5 degree accuracy */
  
  /* then go forward under gyro control for the specified time */
  start_time = mseconds();
  
  while ((mseconds() - start_time) < dur){
    if (theta < (angle_target - tolerance)) {
      /* Turn CCW a little */
      motor(leftmotor, speed - tweak);
      motor(rightmotor, speed + tweak);
    } else if (theta > (angle_target + tolerance)){
      /* Turn CW a little */
      motor(leftmotor, speed + tweak);
      motor(rightmotor, speed - tweak);
    } else {
      motor(leftmotor, speed);
      motor(rightmotor, speed);
    }  
    /*    if(1 == bump) \/* If we hit something *\/      
	  break;*/
  } /* while */
  motor(leftmotor, 0);
  motor(rightmotor, 0);
} /* timedrive() */

void wiggle()
{
  forward(80, 20.);
  turnangle(15.);
  backup(80, 20.);
  turnangle(-15.);
}

void turnangle(float angle)
{
  float desired;
  if(sign(angle) == 1)
    {
      desired = (theta/lsb_ms_per_deg)+angle;
    }
  else
    {desired = (theta/lsb_ms_per_deg)+angle;}

  /*    printf("turning to %f\n", desired);*/
    turnabs(desired);
}

void turnabs(float angle) {
  /* To convert degrees to units of theta, multiply by lsb_ms_per_deg.*/
  float angle_target;
  float error, tolerance;
  int speed;
  
  tolerance = 3. * lsb_ms_per_deg;
  /*  printf("%f\n", tolerance);*/
  angle_target = angle * lsb_ms_per_deg;
  error = theta - angle_target;
  while ( fabs(error) > tolerance ) 
    {
      spin(30 * -sign(error));
      error = theta - angle_target;
      /*      printf("error = %f\n", (error/lsb_ms_per_deg));*/
    }
  spin(0);  
} /* go_to_angle() */

void leftcasterangle(float angle)
     /* This sets the left caster to an angle relative to the body of the bot*/
{
  int casterpos =  (lc_center + (int)(lc_mult * angle));
  /*  printf("%d ", casterpos);*/
  servo(leftcaster, casterpos);   
}

void rightcasterangle(float angle)
     /* This sets the right caster to an angle relative to the body of the bot*/
{
  int casterpos =  (rc_center + (int)(rc_mult * angle));
  /*  printf("%d ", casterpos);*/
  servo(rightcaster, casterpos);
}


void spin(int speed){
  leftcasterangle(-55.);
  rightcasterangle(55.);
  motor(leftmotor, -speed);
  motor(rightmotor, speed);
} /* spin(), left is positive */


  
void rotate_ballgate(float position_deg)
{
  /* ballgate: Port number for ball gate servo */
  /* 0 = 1-way, 360 = open */
  int gatepos =  (bg_start - (int)(bg_mult * position_deg));
  /*  printf("%d ", gatepos);*/
  servo(ballgate, gatepos);
}

void openballgate()
{
  rotate_ballgate(400.);
}

void closeballgate()
{
  rotate_ballgate(0.0);
}

void lockballgate()
{
  rotate_ballgate(320.0);
}

void retract_pusher()
{
  servo(pusher, pusher_min);
}

void extend_pusher()
{
  servo(pusher, pusher_max);
}
void push_balls()
{
  /* pusher: Port number for pusher */
  openballgate();
  servo(pusher, pusher_max);
  sleep(pusher_ext_time);
  servo(pusher, pusher_min);
  closeballgate();
}


/*music playing functions---------------------------------------------------*/
void upscale()
{
  tone(261.6,.3);
  tone(293.6,.3);
  tone(329.6,.3);
  tone(349.2,.3);
  tone(391.9,.3);
  tone(440.,.3);
  tone(493.8,.3);
  tone(523.2,.3);
}

void downscale()
{
  tone(523.2,.3);
  tone(493.8,.3);
  tone(440.,.3);
  tone(391.9,.3);
  tone(349.2,.3);
  tone(329.6,.3);
  tone(293.6,.3);
  tone(261.6,.3);
}
/*IR related functions-------------------------------------------------------*/
int ir_checkcolor()
{
  /* Send out an IR ping.  
     If the result is more than black_thresh, color = black
     If the result is less than white_thresh, color = white
     If it is more than black_thresh and less than white_thresh,
     throw it out and try again 
  */
  /* 0 = white, 1 = black */
  int white = 0;
  int black = 1;
  int color; 
  
  int ir_result = analog(ir_sensor);
  if(ir_result > black_thresh)
    {color = black;}
  else if(ir_result < white_thresh)
    {color = white;}
  return color;
}

/*Calibration Sequences-----------------------------------------------------*/
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("Press start      for gyro.\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;
  printf("done");
  
} /* calibrate_gyro() */


void calibrate_ir()
{
  /*Sets white_thresh and black_thresh for the ir sensor*/
  int black_reading, white_reading;
  printf("black square   press start\n");
  start_press();
  black_reading = analog(ir_sensor);
  printf("white square   press start\n");
  start_press();
  white_reading = analog(ir_sensor);
  white_thresh = white_reading + ir_thresh;
  black_thresh = black_reading - ir_thresh;
  printf("bthresh = %d  ", black_thresh);
  printf("wthresh = %d\n", white_thresh);
 }



void startup()
{
  rf_team = 11;
  rf_enable();
  calibrate_ir();
  calibrate_gyro();
  enable_servos();
  closeballgate();
  printf("gate to closed, press start\n");
  start_press();
  openballgate();
  printf("ballgate should be open\n");
  disable_servos();
  start_machine(); /*Waits for start signal over RF*/
  start_process(update_angle());
  printf("enabling servos\n");
  enable_servos();
}

void startup_test()
{/*like startup, but without rf*/
  enable_servos();
  calibrate_ir();
  calibrate_gyro();
  start_process(update_angle());
}
  

/*Math functions-----------------------------------------------------------*/
float getangle(int delx, int dely) 
     /*calculates angle between two offsets in radians, then converts to degrees */
{
  float tangle;
  /*  printf("delx is %d", delx);
      printf("dely is %d\n", dely);*/
  tangle = 0.0;
  if(delx == 0) /*deal with divide by zero problem */
    printf("div by 0 bad!\n");
  else  
    tangle = atan((float)dely/(float)delx);
  if(delx < 0)
    tangle += pi;
  if(tangle < 0.0)
    tangle += (2.0 * pi);
  /* convert to degrees */
  tangle *= (180./pi); /*tangle rad * (180/pi deg/rad) = tangle*180/pi deg */
  /*printf("tangle = %f deg\n", tangle);*/
  return tangle;
}

float distancefrom(float x1, float x2, float y1, float y2)
{
  float delx = x1 - x2;
  float dely = y1 - y2;
  float distance;
  /*  printf("delx is %f", delx);
      printf("dely is %f\n", dely);*/
  distance = sqrt(delx*delx + dely*dely);
  /*  printf("distance is %f\n", distance);*/
  return distance;
}

float fabs(float arg)
{
  if(arg < 0.)
    return -arg;
  return arg;
}
int isign(int arg)
{
if(arg < 0)
    return -1;
  return 1;
}
  
int sign(float arg)
{
  if(arg < 0.)
    return -1;
  return 1;
}

/*Functions for orientation----------------------------------------*/

int determine_facing()
{
  int firstcolor, secondcolor, thirdcolor, fourthcolor, facing, colors ;
                    /*0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15 */
  int facing_map[] = {0, 5, 6, 0, 7, 0, 0, 3, 8, 0, 0, 2, 0, 1, 4, 0};
  firstcolor = ir_checkcolor();
 /*turn to 90 from starting position*/
  turnangle(90.0);
  secondcolor = ir_checkcolor();
/*turn to directly away from start position*/
  turnangle(90.0);
  thirdcolor = ir_checkcolor();
  turnangle(90.0);
  fourthcolor = ir_checkcolor();
  turnangle(90.0);
  /*  North Court North Facing: Black Black White Black (1) */
  /*  North Court East Facing: Black White Black Black  (2) */
  /*  North Court South Facing: White Black Black Black (3) */
  /*  North Court West Facing: Black Black Black White  (4) */
  /*  South Court South Facing: White White White Black (5) */
  /*  South Court West Facing: White White Black White  (6) */
  /*  South Court North Facing: White Black White White (7) */
  /*  South Court East Facing:  Black White White White (8) */
  /*  "I have no idea"                                  (0) */
  colors = (firstcolor << 3) | (secondcolor << 2) | (thirdcolor << 1) | (fourthcolor);
  facing = facing_map[colors];
  printf("facing = %d", facing); 
  return facing;
}

void turn_to_face()
{  
  int white = 0;
  int black = 1;
  int wherefacing;
  wherefacing = determine_facing();
  if (0 == wherefacing)
    {
      downscale();
      printf("I'm lost!");
      tone(440.0, .1);
      tone(440.0, .1);
      tone(440.0, .1);
      wherefacing = determine_facing() /*try again*/;
    }

  if(wherefacing <=4) /*north court, black team*/
    {ourcolor = black;}
  else /*south court, white team */
    {ourcolor = white;}
  
  /* Turn to face zero and reset the gyro */
  if((1 == wherefacing)||(5 == wherefacing))
    {}/* We're already facing towards the first 4 balls */
  else 
    {
      if((2 == wherefacing)||(6 == wherefacing))
	{
	  turnabs(-90.0);
	  theta = (theta + (90.0*lsb_ms_per_deg));
	} /*Turn towards the first 4 balls*/
      else
	{if((3 == wherefacing)||(7 ==wherefacing))
	  {turnabs(180.0);
	  theta = theta - (180.*lsb_ms_per_deg);}
	else
	  {
	    if((4== wherefacing)||(8==wherefacing))
	      {turnabs(90.0);
	      theta = theta -  (90.*lsb_ms_per_deg);}
	  }
      }
    }
  printf("%f\n", (theta/lsb_ms_per_deg));
  turnabs(0.0);
}

int determine_teamnumber()
{
  int our_rf_number;

  turn_to_face();
  /* determine what rf number we are */
  if(((rf_y0 <  0) && (ourcolor==1))||((rf_y0 > 0) &&(ourcolor==0)))
    {our_rf_number=1;}
  else
    {
      if(((rf_y0 > 0) && (ourcolor==1))||((rf_y0==-110) && (ourcolor==0)))
	{our_rf_number=0;}
    }
  /*since it's convenient, also set our teamsign*/
  if(teamnumber == 1)
    {teamsign = 1;}
  else
    {teamsign =-1;}
  return our_rf_number;
}

int get_xpos()
{
  int xpos;
  if(0 ==teamnumber)
    {xpos = rf_x0;}
  else
    {xpos = rf_x1;}
}

int get_ypos()
{
  int ypos;
  if(0 ==teamnumber)
    {ypos = rf_y0;}
  else
    {ypos = rf_y1;}
}


/*driveto functions*/


float worldAngleToLocalAngle(float angle)
{
	if(ourcolor == 1) /* black */
	{
		return angle - 90.;
	}
	else if(ourcolor == 0) /* white */
	{
		return angle + 90.;
	}
	else
	{
		printf("ourcolor poorly defined.");
		return 0.;
	}
}

float localAngleToWorldAngle(float angle)
{
	if(ourcolor == 1) /* black */
	{
		return angle + 90.;
	}
	else if(ourcolor == 0) /* white */
	{
		return angle - 90.;
	}
	else
	{
		printf("ourcolor poorly defined.");
		return 0.;
	}
}

float dsquaredfrom(float x1, float x2, float y1, float y2)
{
	float delx = x1 - x2;
	float dely = y1 - y2; 
	float distance;

	distance = delx*delx + dely*dely;
	return distance;
}

float getCloser(int xdest, int ydest)
{
	float pi = 3.1415926535897932384626;
	int initial_x = rf_x0; /* Gets initial position of our robot */
	int initial_y = rf_y0;
	float targetangle, phi, distance, prevdist;
	
	/* Calculate the angle between our desired target angle and the x axis */
	targetangle = getangle(xdest - get_xpos(), ydest - get_ypos());

	turnabs(worldAngleToLocalAngle(targetangle));
	/* Now we think we're on the right heading.  
	   We should drive timeforward and "line follow" that heading
	   while making corrections using the RF beacon. */
	distance = distancefrom((float)xdest, (float)get_xpos(), (float)ydest, (float)get_ypos());
 	prevdist = distance;
 	while (distance <=  prevdist && distance > 0.0)
	{
		timedrive(navigatespeed, navigatestepsize);
		prevdist = distance;
		distance = distancefrom((float)xdest, (float)get_xpos(), (float)ydest, (float)get_ypos());
		if((int)fabs(prevdist-distance) < stuckdist) /* We're stuck! */
		{
			wiggle();
			return distance;
		}
	}
 
	return distance;
}

void driveTo(int xdest, int ydest)
{
	float thresh = 10.0; /* The threshold for "close enough" */
	float distance = 10000.0; /* Initial condition */
	printf("Going into while loop\n");
  	while(distance > thresh) 
    	{
	  distance = getCloser(xdest, ydest); 
	  /* turn until we're on more or less 
	     the right heading, then go until 
	     we start getting further away */
	  printf("In main while   Distance = %f\n", distance);
	}
}

void boardTour()
{

  if(teamnumber == 1)
    {teamsign = 1;}
  else
    {teamsign =-1;}
  
  driveTo(200, 50*teamsign);
  driveTo(200, 350*teamsign);
  driveTo(-200, 350*teamsign);
  driveTo(-200, 50*teamsign);
}


void navigateTo(int xdest, int ydest)
{
  if(isign(get_ypos()) == isign(ydest) || ydest == 0) /* Same half of board, simple line */
    {
      driveTo(xdest, ydest);
    }
  else /* Destination on other half of board */
    {
      driveTo(180 * isign(xdest), 90 * isign(get_ypos())); /* Our side of midfield, buffered */
      driveTo(180 * isign(xdest), 90 * isign(ydest)); /* Other side of midfield, buffered */
      driveTo(xdest, ydest);
    }
}