// Include headers from OS
#include <joyos.h>

//Math Stuff
#include <stdio.h>
#include <math.h>
#include <happylib.h>

#define PI 3.14159265358979323846
#define RADTODEG(RAD) ((RAD)*(180.0/PI))
#define THETATODEG(THETA) (THETA*180.0)/2047.0

#define SHARP_M 14150 
#define SHARP_C 10


//Include custom headers 
//#include <sensorsTeam6.h>
//#include <motorsTeam6.h>
//#include <collisionAvoidanceTeam6.h>

#define gyroPort 23
#define LSB_US_PER_DEG 1048830

//Positioning thing
#define VPS_PER_FOOT 682.6666
extern volatile uint8_t robot_id;


#define LRDistancePort 22
#define SRDistancePort 21 //8-23 pull-up ON
#define sideSRPort 20 //8-23 pull-up ON
#define leverBreakbeamPort 10 //8-23 pull-up ON

#define headServoPort 0
uint16_t headServoPos = 250;

#define leverServoPort 1
#define gateServoPort 2

#define leftMotorPort 0
#define rightMotorPort 1
#define sideMotorPort 2

int frobPos = 0;
int bodyPos = 0;
int captureDir = 0;
int8_t gyroInitPosition = 0;
uint8_t minedResources = 0;
uint32_t motorStartingTime = 0;

int16_t points[][2] = {{2047,0},{1791,443},{1280,1330},{1024,1773},{512,1773},{-512,1773},{-1024,1773},{-1280,1330},{-1791,443},{-2047,0},{-1791,-443},{-1280,-1330},{-1024,-1773},{-512,-1773},{512,-1773},{1024,-1773},{1280,-1330},{1791,-443},{443,256},{0,512},{-443,256},{-443,-256},{0,-512},{443,-256}};
char alphabet[] = {'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P','Q','R','S','T','U','V','W','X','Y','Z'};

// usetup is called during the calibration period. 
int usetup (void) {
	robot_id = 6;
	printf("Stabilizing \n");
	pause(1000);
	gyro_init(gyroPort, LSB_US_PER_DEG, 500);
	irdist_set_calibration (SHARP_M, SHARP_C);
	//Initialize some variables
	gyroInitPosition = gyro_get_degrees();
	bodyPos = gyro_get_degrees() - gyroInitPosition;
	//**Use wireless data to set captureDir as 1 or -1**
	captureDir = 1;
	
    return 0;
}


//General Functions
int MIN(int x, int y){
	if(x >= y){
		return x;
	}
	else{
		return y;
	}
}
uint16_t limit_vel(int vel){
	int MAX_VEL = 255;
	if (vel > MAX_VEL){
		return MAX_VEL;
	}
	else if (vel < -MAX_VEL){
		return -MAX_VEL;
	}
	else{
		return (int16_t)vel;
	}
}
int16_t limit_theta(int16_t theta){
	while ((theta > 360) || (theta < -360)){
		if(theta > 360){
			theta -= 360;
		}
		else if(theta < -360){
			theta += 360;
		}
	}
	return theta;
}
int16_t fixAngle(int16_t theta){
	if(theta < 0){
		return 360 + theta;
	}
	else{
		return theta;
	}
}
//Digital and Analog Reads
//Note: if you use digital_read() on an analog port -> returns 0/1 for < or > 2.5 V
void printDigitalReads(){
	for (int i = 0; i<=7; i++){
			printf("Digital Reads: %d",digital_read(i));
		}
	printf("\n");
}	
void printAnalogReads(){
	for (int i = 8; i<=23; i++){
		printf("Analog Reads: %d",analog_read(i));
	}
}
int16_t getPoint(char letter, uint8_t pos){
	int len=sizeof(alphabet)/sizeof(int);
	for(uint8_t i = 0; i<len; i++){
		if (letter == alphabet[i]){
			return points[i][pos];
		}
	}
	return 0;
}


//Sensors

int16_t gyroDegrees(){
	return gyro_get_degrees() - gyroInitPosition;
}
//**Needs to reset better**
void setBodyPos(){
	bodyPos = gyroDegrees();
	while (bodyPos > 360){
		bodyPos -= 360;
	}
	while (bodyPos < -360){
		bodyPos += 360;
	}
}
void resetGyroWithTheta(){
	copy_objects();
	bodyPos = (int)THETATODEG(game.coords[0].theta);
	gyroInitPosition = gyro_get_degrees() - bodyPos;
	
	
}
void sensors_turnTo(int8_t degrees){
	if(degrees < -90){
		degrees = -90;
	}
	else if(degrees > 90){
		degrees = 90;
	}

	servo_set_pos(headServoPort,(int)(300 + degrees*(200.0/90.0)));
	
}
float sensors_LRDistance(){
	return irdist_read(LRDistancePort)/2.54; //Converts to inches
}
float sensors_SRDistance(uint8_t port){
	return analog_read(port); //**convert this to inches**
}
uint8_t sensors_SRIsWithin(uint8_t port, float inches){
	return (sensors_SRDistance(port) < inches); //returns 1 or 0
}
uint8_t sensors_breakbeamIsBroken(uint8_t port){
 return (analog_read(port) > 500); //**500 is arbitrary, find actual value**
}

void sensors_signal(){
	sensors_turnTo(45);
	pause(200);
	sensors_turnTo(0);
	pause(200);
}


//Motors
uint8_t motorPortList[] = {0,1};
int leftMotorVel = 0;
int rightMotorVel = 0;

uint8_t isRobotStopped(){
	return ((leftMotorVel == 0) && (rightMotorVel == 0));
}
uint8_t isRobotStarting(){
	return ((get_time() - motorStartingTime) < 200);
}
void setDriveMotorVels(){
	motor_set_vel(leftMotorPort,leftMotorVel);
	motor_set_vel(rightMotorPort,rightMotorVel);
}
void motors_stopDriveMotors(){
	rightMotorVel = 0;
	leftMotorVel = 0;
	setDriveMotorVels();
}
void motors_brakeDriveMotors(){
	leftMotorVel = 0;
	rightMotorVel = 0;
	int len = sizeof(motorPortList)/sizeof(int);
 	for (int i = 0; i<len; i++){
		motor_brake(motorPortList[i]);
	}
}
void motors_turnLeft(uint8_t speed){
	leftMotorVel = -limit_vel(speed);
	rightMotorVel = limit_vel(speed);
	setDriveMotorVels(leftMotorVel, rightMotorVel);
}
void motors_turnRight(uint8_t speed){
	leftMotorVel = limit_vel(speed);
	rightMotorVel = -limit_vel(speed);
	setDriveMotorVels(leftMotorVel, rightMotorVel);
}

void motors_adjustLeft(uint8_t speed, uint8_t correction){
	leftMotorVel = limit_vel(speed - correction);
	rightMotorVel = limit_vel(speed + correction);
	setDriveMotorVels(leftMotorVel, rightMotorVel);
	pause(250);
}
void motors_adjustRight(uint8_t speed, uint8_t correction){
	leftMotorVel = limit_vel(speed + correction);
	rightMotorVel = limit_vel(speed - correction);
	setDriveMotorVels(leftMotorVel, rightMotorVel);
}

//This sets a given heading but doesn't drive along it
void motors_setHeading(uint8_t speed, int16_t heading){
	heading = fixAngle(heading);
	uint8_t stopped = isRobotStopped();
	uint32_t timer = get_time();
	while ((abs(fixAngle(bodyPos)-heading)>2) && (get_time()-timer < 5000)){
		if (heading < bodyPos){
			if(stopped){
				motors_turnRight(speed);
			}
			else{
				motors_adjustRight(speed,abs(bodyPos-heading)*2);
			}
		}
		else{
			if(stopped){
				motors_turnLeft(speed);
			}
			else{
				motors_adjustLeft(speed,abs(bodyPos-heading)*2);
			}
		}
		setBodyPos();
	}
	motors_stopDriveMotors();
}

//This will move along a certain heading
//If the robot is already moving this will be a slower adjustment
//To go straight - put heading in as bodyPos
void motors_followHeading(int16_t speed, int16_t heading){
	setBodyPos();
	float error = MIN((fixAngle(heading)-fixAngle(bodyPos)), (heading-bodyPos));
	float gain = 1.5; //**adjust this**
	int16_t correction = (int16_t)(error * gain);
	if (speed<0){
		correction = -correction;
	}
	leftMotorVel = limit_vel(speed -= correction);
	rightMotorVel = limit_vel(speed += correction);
	//printf("Heading: %d bodyPos: %d Error: %f Speed: %d LMV: %d RMV %d \n",heading, bodyPos, error, speed, leftMotorVel, rightMotorVel);
	setDriveMotorVels(leftMotorVel, rightMotorVel);

}
void motors_thetaSetHeading(int16_t speed, int16_t heading){
	copy_objects();
	int16_t theta = THETATODEG(game.coords[0].theta);
	int timer = get_time();
	while(abs(heading-theta)>2){
		copy_objects();
		theta = THETATODEG(game.coords[0].theta);
		motors_turnLeft(speed);
		if((get_time() - timer) > 2000){
			break;
		}
	}
	motors_stopDriveMotors();
}
void motors_thetaFollowHeading(int16_t speed, int16_t heading){
	if(isRobotStopped()){
		motors_setHeading(200,heading);
		leftMotorVel = limit_vel(speed);
		rightMotorVel = limit_vel(speed);
		setDriveMotorVels(leftMotorVel, rightMotorVel);
	}
	copy_objects();
	int16_t theta = THETATODEG(game.coords[0].theta);
	uint32_t startTime = get_time();
	float error = heading-theta;
	//printf("Heading: %d bodyPos: %d Error: %f LMV: %d RMV %d \n",heading, bodyPos, error, leftMotorVel, rightMotorVel);
	while ((error>2) || (error<-2)){
		copy_objects();
		theta = THETATODEG(game.coords[0].theta);
		error = heading - theta;
		float gain = 1.5; //**adjust this**
		int16_t correction = (int16_t)(error * gain);
		if (speed<0){
			correction = -correction;
		}
		leftMotorVel = limit_vel(speed -= correction);
		rightMotorVel = limit_vel(speed += correction);
		//printf("Heading: %d bodyPos: %d Error: %f Speed: %d LMV: %d RMV %d \n",heading, bodyPos, error, speed, leftMotorVel, rightMotorVel);
		setDriveMotorVels(leftMotorVel, rightMotorVel);
		
		if ((get_time() - startTime) > 50000){
			break;
		}
		
	}
}
//setBodyPos() must be called before calling this
void motors_gyroDriveStraight(int16_t speed, uint16_t seconds){
	uint32_t timer = get_time();
	setBodyPos();
	int16_t curBodyPos = bodyPos;
 	while(get_time() - timer < seconds){
		motors_followHeading(speed,curBodyPos);
	}
}
uint8_t motor_isStalled(uint8_t motor){
 	uint16_t current = motor_get_current(motor);
 	if ((current > frobPos) && (!isRobotStarting())){ //**Need to check value - this isn't right**
 		return 1;
 	}
 	else{
 		return 0;
 	}
 }
void motors_stopOnStall(){
 	int len = sizeof(motorPortList)/sizeof(int);
 	for (int8_t i = 0; i<len; i++){
 		if (motor_isStalled(motorPortList[i])){
 			motor_brake(motorPortList[i]);
 		}
 	}
}
void motors_stopOnLift(){
 	for (int8_t i = 0; i<2; i++){
 		uint16_t current = motor_get_current(i);
 		if (current < 3){ //**Need to get actual value**
 			motor_brake(motorPortList[i]);
 		}
 	}
}


//Collision avoidance
void avoidStaticObject(){
	//avoid non-moving things
}
void avoidMovingObject(){
	//keep track of object movement
	//different strategy to avoid moving objects - keeps track of their position
}
void simpleAvoidCollisions(int16_t speed){
	if(sensors_LRDistance()<12.0){
		motors_adjustLeft(speed,(12-sensors_LRDistance())*25);
	}
}
void avoidCollisions(int16_t speed){
	copy_objects();
	int16_t curPos[] = {game.coords[0].x,game.coords[0].y,THETATODEG(game.coords[0].theta)};
	
	//check if anything in range of SRDistance -> stop immediately, back up
	//check if anything in range of the LRDistance -> change path to avoid
//	printf("%.2f in\n",sensors_LRDistance());
	float dist = 10.0;
	if(sensors_LRDistance() < dist){
		motors_stopDriveMotors();
		for(int8_t i = -90; i<=90; i+=5){
			sensors_turnTo(i);
//			printf("%.2f in\n",sensors_LRDistance());
			pause(50);
			if(sensors_LRDistance() > dist){
				//motors_setHeading(150,i);
				sensors_turnTo(0);
				pause(50);
				if(i<0){
					motors_turnRight(speed);
					pause(500);
				}
				else{
//					printf("Turning Left");
					motors_turnLeft(speed);
					pause(500);
				}
				
				break;
			}
		}
		
	}
	dist = 24.0;
	if(sensors_LRDistance() < dist){
		for(int8_t i = -90; i<=90; i+=5){
			sensors_turnTo(i);
//			printf("%.2f in\n",sensors_LRDistance());
			pause(50);
			if(sensors_LRDistance() > dist){
				//motors_setHeading(150,i);
				sensors_turnTo(0);
				pause(50);
				if(i<0){
					motors_adjustRight(speed,50);
				}
				else{
//					printf("Turning Left");
					motors_adjustLeft(speed,50);
				}
				
				break;
			}
		}
		
	}

	

	//Turn the robot back straight 
// 	leftMotorVel = speed;
// 	rightMotorVel = speed;
// 	setDriveMotorVels(leftMotorVel, rightMotorVel);
	
	//use LR sensor when going straight -> when it finds something, scan 
	//a few degrees to see if it ends (if so, it's probably the other robot)
	//check if in map, if is 
	//identify type of object (static,moving) in path -> use correct object
}
void avoidWalls(int16_t speed){
	copy_objects();
	int16_t curPos[] = {game.coords[0].x,game.coords[0].y,THETATODEG(game.coords[0].theta)};
	int8_t dist = 100;
	
	if((curPos[0]<getPoint('S',0) + dist) && (curPos[0]>getPoint('U',0) - dist) && (curPos[1]<getPoint('T',1)+dist) && (curPos[1]>getPoint('W',1) - dist)){
		if(curPos[0]>getPoint('S',0)){
			//check the angle
		}
		else if(curPos[0]<getPoint('U',0)){
		
		}
		else if(curPos[1]>getPoint('T',1)){
		
		}
		else if(curPos[1]<getPoint('W',1)){
			
		}
	}
	if((abs(curPos[0] - getPoint('U',0))<dist) && (curPos[1]<getPoint('T',1)) && (curPos[1]>getPoint('W',1))){
		if((curPos[2]<180) && (curPos[2]>0)){
			motors_adjustLeft(speed,abs(curPos[2]-90)*2);
			avoidCollisions(speed); 
		}
		else{
			motors_adjustRight(speed,abs(curPos[2]-270)*2);
			avoidCollisions(speed);
		}
	}
}
//Interaction

void captureTerritory(){
	//**SR Navigation to line up the motor**
	
	//**Until the territory is captured - feedback from wireless chip**
	motor_set_vel(sideMotorPort, 100*captureDir);
}
//returns 0 on failure to mine, 1 on success
uint8_t mineResource(){
	uint32_t time = get_time();
	while(!sensors_breakbeamIsBroken(leverBreakbeamPort)){
		//**SR Navigation (drive in an arc)**
		if((get_time() - time) > 5000){
			return 0;
		}
	}
	servo_set_pos(leverServoPort,400); //**Arbitrary value - fix**
	pause(500); //**Arbitrary value - fix**
	servo_set_pos(leverServoPort,200); //**Arbitrary value - fix**
	minedResources++;
	return 1;
}
void releaseResources(){
	//**SR Navigation
	servo_set_pos(leverServoPort,400); //**Arbitrary value - fix**
	pause(500*minedResources); //**Arbitrary value - fix**
	servo_set_pos(leverServoPort,200); //**Arbitrary value - fix**
	minedResources = 0;
}
void maximizePoints(){
	//**Not void - should return point to navigate to and action to take**
}



//Navigation

// void resetGyro(){
// 	//**MOVE HEAD SERVO TO START POSITION
// 	copy_objects();
// 	float theta = THETATODEG(game.coords[0].theta);
// 	while((theta>1.5) || (theta < -1.5)){
// 		copy_objects();
// 		float theta = THETATODEG(game.coords[0].theta);
// 		if(theta>1.5){
// 			motors_turnRight();
// 		}
// 		else if(theta < -1.5){
// 			motors_turnLeft();
// 		}
// 	}
// 	motors_stopDriveMotors();
// 	pause(100);
// 	gyroInitPosition = gyro_get_degrees();
// 	
// }

void navigateToPoint(int16_t speed, int16_t goalX, int16_t goalY){ 
	
	//Get current position
	copy_objects();
	int16_t curX = game.coords[0].x;
	int16_t curY = game.coords[0].y;
	int16_t dy = goalY-curY;
	int16_t dx = goalX-curX;
	int16_t curTheta = THETATODEG(game.coords[0].theta);
	int16_t goalTheta = (int16_t)RADTODEG(atan2(dy,dx));	

	int16_t err = 300; //**CHANGE**
	//While the difference between goal and current position is greater than allowed err
	while ((abs(dx)>err) ||(abs(dy)>err)){
//		simpleAvoidCollisions(speed);
		copy_objects();
		curX = game.coords[0].x;
		curY = game.coords[0].y;
		dy = goalY-curY;
		dx = goalX-curX;
		curTheta = THETATODEG(game.coords[0].theta);
		goalTheta = (int16_t)RADTODEG(atan2(dy,dx));	
		
//		printf("Cur Theta: %d Goal Theta: %d\n", curTheta, goalTheta);
		float error = goalTheta - curTheta;
		float gain = 2; //**adjust this**
		int16_t correction = (int16_t)(error * gain);
		leftMotorVel = limit_vel(speed - correction);
		rightMotorVel = limit_vel(speed + correction);
		setDriveMotorVels(leftMotorVel, rightMotorVel);
		

		//motors_thetaFollowHeading(200,goalTheta);

	}
//motors_stopDriveMotors();
//	motors_stopOnStall();
}

void navigateToPoints(){
	copy_objects();
	int16_t goalX = game.coords[2].x;
	int16_t goalY = game.coords[2].y;
	int16_t curX = game.coords[0].x;
	int16_t curY = game.coords[0].y;
	int16_t dy = goalY-curY;
	int16_t dx = goalX-curX;
	int16_t curTheta = THETATODEG(game.coords[0].theta);
	int16_t goalTheta = (int16_t)RADTODEG(atan2(dy,dx));	
	int16_t speed = 200;
	float error = goalTheta - curTheta;
	float gain = 2; //**adjust this**
	int16_t correction = (int16_t)(error * gain);
	leftMotorVel = limit_vel(speed - correction);
	rightMotorVel = limit_vel(speed + correction);
	setDriveMotorVels(leftMotorVel, rightMotorVel);
	avoidCollisions(speed);
}

void driveAroundAvoidingThings(){
	leftMotorVel = 100;
	rightMotorVel = 100;
	setDriveMotorVels(leftMotorVel, rightMotorVel);
	avoidCollisions(100);
}

//Exploration period

void explore(int16_t speed){
//	motors_thetaSetHeading(speed,0);
	navigateToPoint(speed,getPoint('H',0),getPoint('J',1));
	navigateToPoint(speed,getPoint('F',0), getPoint('L',1));
	navigateToPoint(speed,getPoint('E',0), getPoint('L',1));
	navigateToPoint(speed,getPoint('C',0), getPoint('A',1));
	navigateToPoint(speed,getPoint('E',0), getPoint('C',1));
	navigateToPoint(speed,getPoint('F',0), getPoint('H',1));
 	navigateToPoint(speed,getPoint('H',0), getPoint('J',1));


	

	//use differential to detect where the gearboxes are 
}

//Tests

void testGyro(){
	printf("Gyro Init Pos: %d Body Pos: %d ",gyroInitPosition, bodyPos);
	setBodyPos();
	printf("Gyro degrees: %d\n",bodyPos);
}
void testFrob(uint8_t range){
	if(frob_read_range(0,range) != frobPos){
			frobPos = frob_read_range(0,range);
			printf("frob pos: %d\n",frobPos);
	}
}
void testLRDistanceSensor(){
	printf("LR Distance: %d\n",sensors_LRDistance()); //converted to inches
}
void testSRDistanceSensors(){
	printf("SR Distance: %d ", sensors_SRDistance(SRDistancePort));
	printf("Side: %d\n", sensors_SRDistance(sideSRPort));
	pause(1000);
}
void testBreakbeams(){

}
void testNavigation(){
	motors_setHeading(100,90);
	uint32_t startTime = get_time();
	motorStartingTime = get_time();
	while ((get_time() - startTime) < 5000){
		motors_gyroDriveStraight(200,bodyPos);
		motors_stopOnStall();
	}
	motorStartingTime = get_time();
	while ((get_time() - startTime) < 10000){
		motors_followHeading(200, 0);
		motors_stopOnStall();
	}
	motors_stopDriveMotors();
}

//Main routine
int umain (void) {
	//For first 10 seconds, run explore

	//Check Inputs, Do while robot running

// 	copy_objects();	
//  	explore(175);
//  	motors_stopDriveMotors();
//  	sensors_signal();
//  	navigateToPoint(150,getPoint('M',0), getPoint('L',1)-200);
//  	motors_stopDriveMotors();
//  	sensors_signal();
//  	motors_thetaSetHeading(100,0);
//  	captureTerritory();
//  	sensors_signal();
//  	pause(1000);
//  	copy_objects();
//  	navigateToPoint(100,getPoint('N',0),game.coords[0].y);
//  	motors_stopDriveMotors();

//  	motors_gyroDriveStraight(150,5000);
//  	motors_stopDriveMotors();
//  	motors_setHeading(100,180);
//  	motors_gyroDriveStraight(150,5000);
//  	motors_stopDriveMotors();
	
// 		motors_setHeading(100,90);
// 		motors_setHeading(100,0);
// 		motors_setHeading(100,-90);
// 		motors_setHeading(100,-180);
// 		motors_setHeading(100,180);
// 		motors_setHeading(100,0);
 	
 	
	while(1){
// 		testGyro();
 		testSRDistanceSensors();		
//		printf("LR Distance: %f\n SR Distance: %f\n",sensors_LRDistance(),sensors_SRDistance());

// 		frobPos = frob_read_range(100,400);
		

//		printf("%.2f in\n",sensors_LRDistance());

// 
// 		servo_set_pos(0,frobPos);

		// **Ask about threading**
// 		Should thread the navigation and keep object avoidance in the while loop?
// 		For threading -> function must be of type 'int'
// 		
// 		create_thread(&navigateToPoint(game.coords[2].x,game.coords[2].y),STACK_DEFAULT,1,"toPoint");


		
	}


    return 0;
}