/*
* The MIT License
*
* Copyright (c) 2007 MIT 6.270 Robotics Competition
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*
*/


// Include headers from OS
#include <joyos.h>
#include <math.h>
#include <lib/pid.h>
#include "umain.h"

// Port definitions
#define GYRO_PORT 11
#define IR_LEFT 21
#define IR_MIDDLE 22 //PLEASE?!?!
#define IR_RIGHT 23
#define IR_FRONT 13
#define IR_BACK 16
#define BUMP_PORT 0
#define DISTANCE_PORT 19
#define S_CLAW 0
#define S_LIFT 1

//More constants
#define LSB_MS_PER_DEG	1400 //old version?
#define LSB_US_PER_DEG 1400000 //new version
#define SPEED -200
#define IN_PER_TICK .14 //.1392
#define SIX_INCHES 43 //encoder ticks
#define ONE_FOOT 86 //encoder ticks
#define EIGHTEEN_INCHES 129 //encoder ticks
#define WHEELBASE 7.5
#define LEFT 1 //for IR sensors... allows to choose which side
#define MIDDLE 2 //of robot we want to hit the line with the find method
#define RIGHT 3 //change this method if you want
#define IR_WHITE 150
#define IR_BLACK 300

//States of motion
#define STILL 0
#define DRIVING 1
#define TURNING 2

//Robot intentions
#define START 0
#define TOWARD_SMALL_BALLS 1
#define TOWARD_BALL_FOUR 2
#define TOWARD_BALL_FIVE 3
#define RAISING_CLAW 4
#define STRAIGHTEN 5
#define TOWARD_GOAL 6
#define TOWARD_FLAGBOX 7
#define AT_FLAGBOX 8

// Servo positions
#define ARM_DOWN 443
#define ARM_UP 130
//int arm_up = 130;
//int arm_down = 443;
#define CLAW_OPEN 430
#define CLAW_CLOSED 250

// PID coefficients
#define K_P 0.5
#define K_D 0.25
#define K_I 0

//directions
#define NORTH 1
#define SOUTH 2
#define WEST 3
#define EAST 4


// Global vars
float scale_left = .8;
float scale_right = 1.2;
float difference = 0;
float diff_deriv = 0;
int encoder_offset_l = 0;
int average_current = 500;
int state = 0;
bool crashed = false;
int l_speed = 0;
int r_speed = 0;
int claw_state = 0; //default open
int lift_state = 0; //default up
int initial_direction = 0;
int left_speed=-200,right_speed=-200;
uint8_t team_number[2] = {6,0};

float turn_factor = 2.2;

// usetup is called during the calibration period. It must return before the
// period ends.
int usetup (void) {
    
    startup();
    
	return 0;
}

// Entry point to contestant code.
// Create threads and return 0.
int
umain (void) {
	
	
	//turn_test();
	
	game_play();
	
	stop();
	
	return 0;
}

//Start of new functions

void turn_test(void) {

	while (1) {
	
		while (!go_press()) {
			turn_factor = ((float)frob_read_range(10,30)/10);
			printf("\n%f",turn_factor);
		}
		turn(M_PI/2);
		go_click();
		turn(-M_PI/2);
		go_click();
		turn(M_PI);
		go_click();
		turn(-M_PI);
		go_click();
		turn(M_PI/4);
		go_click();
		turn(-M_PI*3/4);
	}
}

void startup(){
	//create_thread(&music,STACK_DEFAULT,0,"music");
	//create_thread(&are_we_crashed, STACK_DEFAULT, 0, "crashed?_thread");
	
	encoder_reset (24); 
	encoder_reset (25);

   // Setup the gyro.
  	gyro_start();
	//music();
	
	// Setup servo initial positions
	lift_up();
	open_claw();
	
	pause(1000);
}


void game_play(){
	state = START;
	
	//Find which direction we are pointing and then backs into wall
	
	int direction = which_direction();
	
	init_find_wall(direction);
	
	go_after_small_balls();

	//go_click();
	go_after_large_ball4();
	
	//go_click();
	
	go_after_large_ball5();
	
	//go_click(); 
	
	block_flagbox();
}


void stop(){
	motor_brake(0);
	motor_brake(1);
}

void are_we_crashed(){
	//the age current between the two motors
	//its averaged in case only one motor is crashed
	int current_current = max(motor_get_current_MA(0),motor_get_current_MA (1));
	
	//continually undates the crashed value	
	while(true){
		//if we aren't moving do nothing
		if(current_current <= 5){
			pause(10);
		}
		//if the current is 300% of the average we have crashed
		else if(current_current >= average_current * 5){
			if (!crashed) {
				pause(200);
			    //printf("\nFUCK I crashed");
			    //printf("%d %d %d",current_current,motor_get_current_MA(0),motor_get_current_MA (1));
				//The magic stop function!!!!
				stop();
			}
			crashed = true;	
			pause(10);
		}
		else{
			//average_current = (average_current + current_current)/2;
		}
		//get the current again
		current_current = max(motor_get_current_MA(0),motor_get_current_MA (1));
	}
}

//End of new functions

bool hit_wall(){
	//function drives until it hits a wall and then returns true
	int read1, read2, last, average, count = 0;
	
	//average values
	read1 = analog_read(IR_BACK);
	pause(100);
	read2 = analog_read(IR_BACK);
	average = (read1 + read2)/2;
	
	//Just to make sure last is greater than average
	last = 3000;
	
	//keeps calling as long as the value keeps decreasing
	while(average < last){
		
		last = average;
		//average values
		read1 = analog_read(IR_BACK);
		pause(100);
		read2 = analog_read(IR_BACK);
	
		average = (read1 + read2)/2;
		
		//if for some reason we don't hit a wall
		count ++;
		if(count == 30){
			return false;
		}
	}
	ResetEncoders();
	return true;	
}

void turn(float radians) {
  // Wait for stop
  radians = turn_factor * radians;
  pause(100);
  // Reset encoders
  encoder_reset(24); 
  encoder_reset(25);
  // Initialize encoders
  float el = encoder_read(24);
  float er = encoder_read(25); 
  float rad_trav_el = 0; //radians due
  float rad_trav_er = 0; //to encoders


  // Turn clockwise if negative, counterclockwise if positive
  if(radians >= 0) {
  	l_speed = -SPEED/2;
       r_speed = SPEED/2;
  }
  else {
	l_speed = SPEED/2;
       r_speed = -SPEED/2;
	radians = radians * -1;
  }

  float avg = (el/2+er/2);

  drive(l_speed, r_speed);

  //100 is for accuracy **DO NOT REMOVE**
  while ((rad_trav_el*100) < (radians*100) || (rad_trav_er*100) < (radians*100)) {
      
      //stop if motor is done with turn
      if (rad_trav_el >= radians) {
		motor_brake(0);
		l_speed = 0;
	}
      else if (rad_trav_er >= radians) {
      	motor_brake(1);
		r_speed = 0;
      }

      pause(15);
      el = encoder_read(24);
      er = encoder_read(25);
      avg = ((el+er)/2);
      //only account for half the radians
      rad_trav_el = (2*el*IN_PER_TICK)/((float)WHEELBASE);
      rad_trav_er = (2*er*IN_PER_TICK)/((float)WHEELBASE);

      //g_read = gyro_get_radians();
      //rad_trav_gy = -1*(g_read - g_init);

      //Set the motor scaling for travel
      //set_scale(&el, &er);

      //drive
      drive(l_speed, r_speed);

      pause(6);
  }
  motor_brake(0);
  motor_brake(1);

  pause(200);
  //printf("%d %d",el,er);
  printf("\n %.2f, %.2f", el, er);

}

void turn_no_stop(float radians) {
  // Wait for stop
  radians = turn_factor * radians;
  pause(100);
  // Reset encoders
  encoder_reset(24); 
  encoder_reset(25);
  // Initialize encoders
  float el = encoder_read(24);
  float er = encoder_read(25); 
  float rad_trav_el = 0; //radians due
  float rad_trav_er = 0; //to encoders


  // Turn clockwise if negative, counterclockwise if positive
  if(radians >= 0) {
  	l_speed = -SPEED/2;
       r_speed = SPEED/2;
  }
  else {
	l_speed = SPEED/2;
       r_speed = -SPEED/2;
	radians = radians * -1;
  }

  float avg = (el/2+er/2);

  drive(l_speed, r_speed);

  //100 is for accuracy **DO NOT REMOVE**
  while ((rad_trav_el*100) < (radians*100) || (rad_trav_er*100) < (radians*100)) {
      
      //stop if motor is done with turn
      if (rad_trav_el >= radians) {
		motor_brake(0);
		l_speed = 0;
	}
      else if (rad_trav_er >= radians) {
      	motor_brake(1);
		r_speed = 0;
      }

      pause(15);
      el = encoder_read(24);
      er = encoder_read(25);
      avg = ((el+er)/2);
      //only account for half the radians
      rad_trav_el = (2*el*IN_PER_TICK)/((float)WHEELBASE);
      rad_trav_er = (2*er*IN_PER_TICK)/((float)WHEELBASE);

      //g_read = gyro_get_radians();
      //rad_trav_gy = -1*(g_read - g_init);

      //Set the motor scaling for travel
      //set_scale(&el, &er);

      //drive
      drive(l_speed, r_speed);

      pause(6);
  }

  //printf("%d %d",el,er);
  printf("\n %.2f, %.2f", el, er);

}


//find initial position

int which_direction() {
	//read initial values of IR sensors
	int init_left = analog_read(IR_LEFT);
	/*if (init_left < IR_WHITE) {
		printf("White");
	}
	else if (init_left > IR_BLACK){
		printf("Black");
	}*/
	int init_right = analog_read(IR_RIGHT);

	//turn 90 degrees counterclockwise
	turn(M_PI/2);
	stop();
	pause(100);
	
	//read new values of IR sensors
	int ninety_left = analog_read(IR_LEFT);
	int ninety_right = analog_read(IR_RIGHT);

	//now where the fuck are we?
	if ((init_left > IR_BLACK && init_right > IR_BLACK && ninety_left > IR_BLACK && ninety_right > IR_BLACK) || (init_left < 
		IR_WHITE && init_right < IR_WHITE && ninety_left < IR_WHITE && ninety_right < IR_WHITE)) { //if North
		
		initial_direction = NORTH;
		return NORTH;
	}
	else if ((init_left < IR_WHITE && init_right > IR_BLACK && ninety_left > IR_BLACK && ninety_right < IR_WHITE) || (init_left > 
		IR_BLACK && init_right < IR_WHITE && ninety_left < IR_WHITE && ninety_right > IR_BLACK)) { //if South
		
		initial_direction = SOUTH;
		return SOUTH;
	}
	else if ((init_left > IR_BLACK && init_right > IR_BLACK && ninety_left < IR_WHITE && ninety_right > IR_BLACK) || (init_left < 
		IR_WHITE && init_right < IR_WHITE && ninety_left > IR_BLACK && ninety_right < IR_WHITE)) { //if West
		
		initial_direction = WEST;
		return WEST;	
	}
	else if((init_left > IR_BLACK && init_right < IR_WHITE && ninety_left > IR_BLACK && ninety_right > IR_BLACK) || (init_left < 
		IR_WHITE && init_right > IR_BLACK && ninety_left < IR_WHITE && ninety_right < IR_WHITE)) { //if East

		initial_direction = EAST;
		return EAST;	
	}
	else {//shouldn't have to go into this...
		printf("FAIL!!");
		return 0;
	}
	
	return 0;
}




// Setup the gyro
void gyro_start(void) {
   // Make sure robot is stable.
   printf_P (PSTR("\nStabilizing..."));
	pause (500);
	// Calibrate gyro offset.
	printf_P (PSTR("\nCalibrating     offset...\n"));
	gyro_init (GYRO_PORT, LSB_US_PER_DEG, 500L);
}


// Drive a set number of encoder ticks
void drive_dist(int distance) {
	
	if(distance > 50){
		drive_dist_at(50, 100);
		distance -= 50;
	}
	
	left_speed = -200;
	right_speed = -200;
	
   //Reset encoder values
   ResetEncoders();
   // Initiliaze encoder values.
   int encoder_left = encoder_read(24);
   int encoder_right = encoder_read(25);
    
   struct pid_controller ForwardPID;

	init_pid(&ForwardPID,
			6.0,
			9.0,
			-.1,
			&EncoderDifferential,
			&ForwardApplyMV);
	ForwardPID.goal = 0.0;
	ForwardPID.enabled = 1; 
	
   // Loop until we've driven a certain distance
   while (encoder_left/2 + encoder_right/2 < distance) {

       update_pid(&ForwardPID);
		encoder_left = encoder_read(24);
		encoder_left = encoder_read(25);

	   pause(10);

   }
   
   CoastMotors();
   //printf("%d %d", encoder_left, encoder_right);
}

void drive_dist_no_stop(int distance) {

	left_speed = -200;
	right_speed = -200;
  //Reset encoder values
	ResetEncoders();
   // Initiliaze encoder values.
   int encoder_left = encoder_read(24);
   int encoder_right = encoder_read(25);
    
   struct pid_controller ForwardPID;

	init_pid(&ForwardPID,
			6.0,
			9.0,
			-.1,
			&EncoderDifferential,
			&ForwardApplyMV);
	ForwardPID.goal = 0.0;
	ForwardPID.enabled = 1; 
   
   // Loop until we've driven a certain distance
   while (encoder_left/2 + encoder_right/2 < distance) {

       update_pid(&ForwardPID);
		encoder_left = encoder_read(24);
		encoder_left = encoder_read(25);
	   pause(10);

   }
   //CoastMotors();
   //printf("%d %d", encoder_left, encoder_right);
}

//not correct yet
void drive_dist_back(int distance) {
	
	left_speed = left_speed *-1;
	right_speed = right_speed * -1;
	
  //Reset encoder values
   ResetEncoders();
  
 // Initiliaze encoder values.
   int encoder_left = encoder_read(24);
   int encoder_right = encoder_read(25);
   
   float avg_l=0, avg_r=0, i = 0;
    
   struct pid_controller ForwardPID;

	init_pid(&ForwardPID,
			6.0,
			9.0,
			-.1,
			&EncoderDifferential,
			&ForwardApplyMV);
	ForwardPID.goal = 0.0;
	ForwardPID.enabled = 1; 
   
   // Loop until we've driven a certain distance
   while (encoder_left/2 + encoder_right/2 < distance) {

       update_pid(&ForwardPID);
		encoder_left = encoder_read(24);
		encoder_left = encoder_read(25);
	   pause(10);
	   avg_l = (avg_l*i + left_speed)/(i+1);
		avg_r = (avg_r*i + right_speed)/(i+1);
		i++;
		
   }
   CoastMotors();
   printf("\n%f %f", avg_l, avg_l);
}

//not corrent yet
void drive_dist_at(int distance, int speed) {

	left_speed = -speed;
	right_speed = -speed;
	
   //Reset encoder values
   ResetEncoders();
   
   // Initiliaze encoder values.
   int encoder_left = encoder_read(24);
   int encoder_right = encoder_read(25);
    
   struct pid_controller ForwardPID;

	init_pid(&ForwardPID,
			6.0,
			9.0,
			-.1,
			&EncoderDifferential,
			&ForwardApplyMV);
	ForwardPID.goal = 0.0;
	ForwardPID.enabled = 1; 
   
   // Loop until we've driven a certain distance
   while (encoder_left/2 + encoder_right/2 < distance) {
	
       update_pid(&ForwardPID);
		encoder_left = encoder_read(24);
		encoder_left = encoder_read(25);
	   pause(10);

   }
   CoastMotors();
   //printf("%d %d", encoder_left, encoder_right);
}


void drive_dist_no_stop_at_no_reset(int distance, int speed){
	
	left_speed = -speed;
	right_speed = -speed;
	
   //Reset encoder values
   //ResetEncoders();
   
   // Initiliaze encoder values.
   int encoder_left = encoder_read(24);
   int encoder_right = encoder_read(25);
    
   struct pid_controller ForwardPID;

	init_pid(&ForwardPID,
			6.0,
			9.0,
			-.1,
			&EncoderDifferential,
			&ForwardApplyMV);
	ForwardPID.goal = 0.0;
	ForwardPID.enabled = 1; 
   
   // Loop until we've driven a certain distance
   while (encoder_left/2 + encoder_right/2 < distance) {
	
       update_pid(&ForwardPID);
		encoder_left = encoder_read(24);
		encoder_left = encoder_read(25);
	   pause(10);

   }
   //CoastMotors();
   //printf("%d %d", encoder_left, encoder_right);
}


//orient towards long wall
void init_find_wall(int orientation) {
	//make the robot face "east"
	switch(orientation) {
		//North(now west) turn 180 degrees
		case 1 : turn(M_PI); break;
		//South(now east) do nothing
		case 2 : break;
		//West (now south) turn 90 degrees counter-clockwise
		case 3 : turn(M_PI/2); break;
		//East (now north) turn 90 degrees clockwise
		case 4 : turn(-M_PI/2); break;
	}
	
	//back up to find wall 
	straighten_out();
	//printf("Wall found");

	pause(100);
}




float gyro_get_radians() {
	return (gyro_get_degrees() * M_PI/180);
}

// Drive with given left and right speed
void drive(int16_t lspeed, int16_t rspeed) {

   motor_set_vel(0,(scale_left*lspeed));
   motor_set_vel(1,(scale_right*rspeed));

}

void close_claw() {
	if (claw_state == 1) {
		servo_set_pos(S_CLAW, CLAW_CLOSED);
	}
	claw_state = 0;
}
		
void open_claw() {
	if (claw_state == 0) {
		servo_set_pos(S_CLAW, CLAW_OPEN);
	}
	claw_state = 1;
}
void lift_up() {
	if (lift_state == 0) { 
		servo_set_pos(S_LIFT, ARM_UP);
		//arm_up = arm_up - 3;

	}
	lift_state = 1;
}
void lift_down() {
	if (lift_state == 1) {
		servo_set_pos(S_LIFT, ARM_DOWN);
		//arm_down = arm_down - 3;
	}
	lift_state = 0;
}

int where_the_fuck_is_the_line(int sensor) {
	int p_sensor = IR_MIDDLE; //default
	switch(sensor) {
		case LEFT : p_sensor = IR_LEFT;
		case MIDDLE : p_sensor = IR_MIDDLE;
		case RIGHT : p_sensor = IR_RIGHT;
	}
	
	encoder_reset(24);
	encoder_reset(25);
	// Initiliaze encoder values.
   	int encoder_left = encoder_read(24);
   	int encoder_right = encoder_read(25);

   	// Initialize gyro values
   	float g_init = gyro_get_radians();
   	float g_read = gyro_get_radians();
	int ir_reading = analog_read(p_sensor);
	uint32_t start_time = get_time();
	
	//drive (straight) towards the damn line until we find it, constantly checking IR
	while (get_time()-start_time < 2000) {	
		drive_dist(7);
		if (check_ir(p_sensor) == true) {
			motor_brake(0);
			motor_brake(1);
			return 0;
		}
	}
	motor_brake(0);
	motor_brake(1);
	return -1;
}

bool check_ir(int sensor){
	int ir_reading = analog_read(sensor);
	if (ir_reading > IR_BLACK) {
		return true;
	}
	return false;
}


//function assumes the robot is vaguely oriented with a wall behind it
void straighten_out(void) {
	
	drive(-SPEED, -SPEED*2/3);

	if (hit_wall()) {
		pause(500);
		stop();
	}
	else{
		printf("\nOh... Shit.....");
	}
	
}

void straighten_out_forward(void) {
    int start = get_time();
    crashed = false;
	
	while( (!crashed || (get_time() - start < 300)) && (get_time() - start < 2000)) { 
		drive(SPEED, SPEED); 
	}
	pause(100);
	stop();
	pause(100);
	crashed = false;
    
}

//dead recogning for small balls
bool haz_ball () {
    return (analog_read(13) < 400);
  } 

void go_after_small_balls(){
	state = TOWARD_SMALL_BALLS;
	
	//drive out to ~black line and straighten out
	drive_dist(EIGHTEEN_INCHES * 12.5 / 18);
	//stop();
	lift_down();
	turn(-M_PI/2.0);
	straighten_out(); 
	
	//drive straight and collect the two balls
	drive_dist_no_stop(37 * EIGHTEEN_INCHES / 18);
	close_claw();
	drive_dist_no_stop(11 * EIGHTEEN_INCHES / 18);
	
	//head to goal
	turn_no_stop(-M_PI / 8);
	open_claw();
	drive_dist(EIGHTEEN_INCHES * 12/18);
	
	//orient for next function
	drive_dist_back(SIX_INCHES);
	turn( -3 * M_PI / 4);
}

void go_after_large_ball4(){
	state = TOWARD_BALL_FOUR;
	
	//find out where we are with 2 wall straightens
	straighten_out();
	drive_dist(EIGHTEEN_INCHES * 18/ 18);
	turn( - M_PI /2);
	straighten_out();
	
	//head toward the large ball
	drive_dist(EIGHTEEN_INCHES * 19 / 18);
	drive_dist_at(EIGHTEEN_INCHES * 10 / 18, 100);
	
	close_claw();
	pause(400);

	
	lift_up();
	pause(50);
	
	//take large ball to goal
	//turn AWAY from the flag box
	turn(- M_PI);
	go_click();
	drive_dist(EIGHTEEN_INCHES * 14/18);
	go_click();
	turn(M_PI/2);
	go_click();
	drive_dist(EIGHTEEN_INCHES * 14 / 18);
	go_click();
	turn(- M_PI/4);
	go_click();
	drive_dist(EIGHTEEN_INCHES * 6/18);
	
	//drop ball
	open_claw();
	pause(500);
	
	//orient for next function
	go_click();
	drive_dist_back(SIX_INCHES);
	go_click();
	turn(- 3* M_PI /4);
	lift_down();
}

void go_after_large_ball5(){
	state = TOWARD_BALL_FIVE;
	
	//straighten out and find out where the fuck we are
	straighten_out();
	drive_dist(EIGHTEEN_INCHES * 44/18);
	turn(-M_PI/2);
	straighten_out();
	
	//head toward the large ball
	drive_dist(EIGHTEEN_INCHES * 19 / 18);
	drive_dist_at(EIGHTEEN_INCHES * 9 / 18, 100);
	
	close_claw();
	pause(400);
	
	lift_up();
	pause(50);
	
	//take large ball to goal
	//turn AWAY from the flag box
	turn(- M_PI);
	
	drive_dist(EIGHTEEN_INCHES * 11/18);
	turn(M_PI/2);
	drive_dist(EIGHTEEN_INCHES * 38 / 18);
	turn(- M_PI/4);
	drive_dist(EIGHTEEN_INCHES * 6/18);
	
	//drop ball
	open_claw();
	pause(500);
	
	
	//orient for next function
	drive_dist_back(SIX_INCHES);
	turn( M_PI * -3/4);
}

void block_flagbox(){
	state = TOWARD_FLAGBOX;
	
	//straighten out and orient ourselves
	straighten_out();
	drive_dist(EIGHTEEN_INCHES * 32.5/18);
	turn(-M_PI/2);
	straighten_out();
	
	//head toward the flagbox
	drive_dist(EIGHTEEN_INCHES * 26.5 / 18);
	
	turn(M_PI);
	straighten_out();
	
	state = AT_FLAGBOX;

}

float distance_get_inches(){
	//averages distance reading
	int read1 = analog_read(DISTANCE_PORT);
	pause(100);
	int read2 = analog_read(DISTANCE_PORT);
	pause(100);
	int read3 = analog_read(DISTANCE_PORT);
	
	float read = (read1 + read2 + read3) /3.0;

	return (-9.0 * pow(10.0, -7) * pow(read, 3) + 0.0011 * pow(read, 2) - .04742 * read + 86.639);
}

/***CRASH RECOVERY CODE***/
//handle option 2 or three
void we_crashed() {
	//step one: break out of current loop (done by checking for thread)

	//step two: stop motors
	stop();

	//step three: backup a couple inchescd /
	drive_dist_back(SIX_INCHES);
	
	//step four: rotate so back is facing nearest wall (way to detect?)
	//distance sensor code here

	//step five: call straighten out
	straighten_out();
	
	crashed = false;

	//step six: refer to last state... go back to what it was doing
	recover_state();
}

//to be used as a link to recovery functions
void recover_state() {
	//insert respective recovery functions
	switch (state) {
		case START : break;
		case TOWARD_SMALL_BALLS : assume_the_recovery_position();
		case TOWARD_BALL_FOUR : assume_the_recovery_position();
		case TOWARD_BALL_FIVE : assume_the_recovery_position();
		case RAISING_CLAW : break;
		case STRAIGHTEN : break;
		case TOWARD_GOAL : break;
		case TOWARD_FLAGBOX : break;
		case AT_FLAGBOX : break;
	}
}


//returns the robot to the goal corner
void assume_the_recovery_position() {
	//which direction are we facing
	int direct = gyro_reading();
	//do we know if there is a robot in the goal?
	//insert if time
	
	//recover, go for ball 4
	if (direct == EAST) {
		orient_east();
	}
	else if (direct == NORTH) {
		orient_north();
		//drive straight until line
		//line follow to ball position??
	}
	else if (direct == SOUTH) {
		orient_south();
	}
}

//gets the direction of robot while against wall **NOT FINISHED**
int gyro_reading() 
{
	//find gyro reading
	float g_read = gyro_get_radians();
	while(g_read < 0) {
		g_read += 360;
	}
	g_read = fmod(g_read, 360);
	
	//interpret to find direction
	if ((g_read < M_PI/4) || (g_read > 7*M_PI/4)) {
		return initial_direction;
	}
	else if ((g_read > M_PI/4) && (g_read < (3 * M_PI /4))) {
		//90 degrees counterclockwise from start
		switch(initial_direction) {
			case NORTH : return WEST;
			case SOUTH : return EAST;
			case WEST : return SOUTH;
			case EAST : return NORTH;
		}
	}
	else if ((g_read > (3 * M_PI/4)) && (g_read < (5 * M_PI/4))) {
		//180 degrees from start
		switch(initial_direction) {
			case NORTH : return SOUTH;
			case SOUTH : return NORTH;
			case WEST : return EAST;
			case EAST : return WEST;
		}
	}
	else if((g_read > (5 * M_PI/4)) && (g_read < (7 * M_PI/4))){
		//90 degrees clockwise from start
		switch(initial_direction) {
			case NORTH : return EAST;
			case SOUTH : return WEST;
			case WEST : return NORTH;
			case EAST : return SOUTH;
		}
	}
	return 0;
}

void orient_east() {
	//drive out arbitrary short distance
	drive_dist(SIX_INCHES);
	
	//turn 90 degrees counterclockwise
	turn(M_PI/2);
	
	//straight back into wall
	straighten_out();
}

void orient_north() {
	//drive out arbitrary short distance
	drive_dist(SIX_INCHES);
	
	//turn 90 degrees clockwise
	turn(-M_PI/2);	

	//straight back into wall
	straighten_out();
	
	//orient east to corner
	orient_east();
}

void orient_south() {
	//drive out arbitrary short distance
	drive_dist(SIX_INCHES);

	//turn 90 degrees counterclockwise
	turn(M_PI/2);

	//straight back into wall
	straighten_out();
	
	//orient east to corner
	orient_east();
}
