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

//defines

#define ENCODER_TO_WHEEL_RATIO 45
#define WHEEL_CIRCUMFERENCE 25.1
#define WHEEL_TRACK 18.3

#define SMOOTHNESS  10

//higher CORRECTiON value = longer to correct itself (encoder drive method)
#define CORRECTION  250

//Various ports. left and right are viewed from the front (side w/ claw)
#define L_ENC_PORT  26
#define R_ENC_PORT  24
#define L_MOT_PORT  0
#define R_MOT_PORT  1
#define FLAG_MOT_PORT 2
#define FRONT_DISTANCE_PORT 22
#define LEFT_DISTANCE_PORT 23
#define SERVO_PORT 0
#define BACK_LEFT_BUMP_PORT 0
#define BACK_RIGHT_BUMP_PORT 1
#define CLAW_BUMP_PORT 2



#define SERVO_UP_DIFF 423
#define SERVO_MID_DIFF1 113
#define SERVO_MID_DIFF2 173


// Included libs
#include <lib/geartrain.h>
#include <lib/irdist.h>
//math.h included in geartrain.h

//Global variables
uint8_t team_number[2] = {19, 0};
float direction=0, lastdir;
float x=22.9, y=22.9, lastx, lasty;
int servo_down_pos;

int left_negfactor=1, right_negfactor=1;
//directions: 0 east (towards own goal)
//           90 south (away opp goal) and cw from there
//REMEMBER online image is mirrored


int abs(int a) {
	if (a<0) return -a; 
	else     return a;
}

int principal(float angle) {
	while(angle>180.) {
		angle-=360;
	}
	while(angle<-180.1) {
		angle+=360;
	}
	return angle;
}

int bump_check() {
	if (left_negfactor==-1 && right_negfactor==-1) {
		if (digital_read(BACK_LEFT_BUMP_PORT)==1 || digital_read(BACK_RIGHT_BUMP_PORT)==1) {
			return 0;
		}
	}
	return 1;
}


int drive_motors(int16_t speed, int left_ticks, int right_ticks, uint32_t timeout) {
	left_negfactor = 1;
	right_negfactor = 1;
	lastx = x;
	lasty = y;
	lastdir = direction;
	
	int straight;
	if (left_ticks == right_ticks) {
		straight=1;
	}
	else if (left_ticks == -right_ticks) {
		straight=-1;
	}
	else straight=0;
	
	if (left_ticks<0) {
		left_ticks = -left_ticks;
		left_negfactor = -1;
	}
	if (right_ticks<0) {
		right_ticks = -right_ticks;
		right_negfactor = -1;
	}

	uint32_t start_time = get_time();
	int left_start = encoder_read(L_ENC_PORT);
	int right_start = encoder_read(R_ENC_PORT);
	int r_enc_value, l_enc_value, right_wiggle, left_wiggle, difference, expected_difference;
	
	int right_speed = right_negfactor*speed;
	int  left_speed =  (int)left_negfactor*speed*(1.0*left_ticks/right_ticks);
	int flag1=1, flag2=1;

	
	motor_set_vel(R_MOT_PORT, right_speed);
	motor_set_vel(L_MOT_PORT, left_speed);
	r_enc_value = encoder_read(R_ENC_PORT);
	l_enc_value = encoder_read(L_ENC_PORT);
	pause(30);
	right_wiggle = (encoder_read(R_ENC_PORT) - r_enc_value)/2;
	left_wiggle  = (encoder_read(L_ENC_PORT) - l_enc_value)/2;
	
	
	while((flag1 || flag2) && bump_check()	 ) {
		r_enc_value = encoder_read(R_ENC_PORT);
		l_enc_value = encoder_read(L_ENC_PORT);
		difference = (r_enc_value - right_start) - (l_enc_value - left_start);
		expected_difference = (int)(r_enc_value - right_start)*(1-1.0*left_ticks/right_ticks);
		if (flag1 && encoder_read(R_ENC_PORT) < right_ticks + right_start - right_wiggle) {
			motor_set_vel(R_MOT_PORT, (int)(right_speed* ((1.0*CORRECTION+(expected_difference-difference))/CORRECTION))  );
		}
		else {
			motor_brake(R_MOT_PORT);
			flag1 = 0;
		}
		if (flag2 && encoder_read(L_ENC_PORT) < left_ticks + left_start - left_wiggle) {
			motor_set_vel(L_MOT_PORT, (int)(left_speed* ((1.0*CORRECTION-(expected_difference-difference))/CORRECTION))  );
		}
		else {
			motor_brake(L_MOT_PORT);
			flag2 = 0;
		}
		
		if (straight == 1) { //position tracking, sans circles
			x = lastx + left_negfactor*((r_enc_value - right_start + l_enc_value - left_start)*1./2*cos((360.-direction)*M_PI/180.))/TICKS_PER_CM;
			y = lasty - left_negfactor*((r_enc_value - right_start + l_enc_value - left_start)*1./2*sin((360.-direction)*M_PI/180.))/TICKS_PER_CM;
		}
		
		if (get_time() > start_time+timeout) { //Failed to reach destination before timeout, return error
			motor_brake(R_MOT_PORT);
			motor_brake(L_MOT_PORT);
			return 1;
		}
		
		
		pause(30);
	}
	motor_brake(R_MOT_PORT);
	motor_brake(L_MOT_PORT);
	return 0;
}

int drive_straight(int16_t speed, float distance, uint32_t timeout) {
	//Drives straight for distance cm or until timeout ms elapses
	//negative distance will cause it to drive backwards
	//returns 0 on success or 1 if timeout
	int negfactor = 1;
	if (distance == 0) return 0;
	if (distance < 0) {
		distance = -distance;
		negfactor = -1;
	}
	int ticks_required = (int)CM_TO_TICKS(distance);
	return drive_motors(speed, negfactor*ticks_required, negfactor*ticks_required, timeout);
}



int back_to_wall(int32_t timeout) {
	int value = drive_straight(108, -999, timeout);
    uint32_t start = get_time();
	pause(20);
	if (value == 0) {
		while ( (digital_read(BACK_LEFT_BUMP_PORT)==0 || digital_read(BACK_RIGHT_BUMP_PORT)==0) && start+timeout>get_time()) {
			motor_set_vel(L_MOT_PORT, -108);
			motor_set_vel(R_MOT_PORT, -108);
			pause(50);
			printf("%d %d", digital_read(BACK_LEFT_BUMP_PORT), digital_read(BACK_RIGHT_BUMP_PORT));
		}
	}
	
	direction = (float) (((int)(direction+45))/90*90.);
	
	motor_brake(L_MOT_PORT);
	motor_brake(R_MOT_PORT);
	return 0;
}



int turn(int16_t speed, float angle, uint32_t timeout) {
	//Turns in place for angle degrees cw or until timeout ms elapses
	//negative angles will cause it to turn ccw
	//returns 0 on success or 1 if timeout. Also updates the global direction variable.
	int negfactor = 1;
	if (angle == 0) return 0;
	if (angle < 0) {
		angle = -angle;
		negfactor = -1;
	}
	int ticks_required = (int)DEG_TO_TICKS_IN_PLACE(angle);
	direction+=negfactor*angle;
	return drive_motors(speed, ticks_required*negfactor, -1*ticks_required*negfactor, timeout);
}

int turn_flag(uint32_t timeout) {
	motor_set_vel(L_MOT_PORT, -70);
	motor_set_vel(R_MOT_PORT, -70);
	motor_set_vel(FLAG_MOT_PORT, 250);
	uint32_t start = get_time();
	while(get_time() < start + timeout) {
		pause(100);
	}
	motor_brake(L_MOT_PORT);
	motor_brake(R_MOT_PORT);
	motor_set_vel(FLAG_MOT_PORT, 0);
	return 0;
}


void find_start_direction(){
	int front1, left1, front2, left2;
	do {
		front1 = analog_read(FRONT_DISTANCE_PORT);
		left1  = analog_read(LEFT_DISTANCE_PORT);
		pause(150);
		front2 = analog_read(FRONT_DISTANCE_PORT);
		left2  = analog_read(LEFT_DISTANCE_PORT);
	}
	while (abs(front1-front2)>30 || abs(left1-left2)>30);
	printf("\n%d %d ", front1, left1);
	if (front1<300) {
		if (left1<300) 
			direction = 0;
		else 
			direction = 270;
	}
	else {
		if (left1<300) 
			direction = 90;
		else
			direction = 180;
	}
}

void pick_up_ball() {
	servo_set_pos(SERVO_PORT, servo_down_pos);
	pause(300);
	drive_straight(72, 28, 4000);
	servo_set_pos(SERVO_PORT, servo_down_pos - SERVO_MID_DIFF1);
	pause(1500);
	servo_set_pos(SERVO_PORT, servo_down_pos - SERVO_MID_DIFF2);
	drive_straight(108, -28, 4000);
}

void dump_ball() {
	servo_set_pos(SERVO_PORT, servo_down_pos - SERVO_UP_DIFF);
}


void turn_cw() {
	turn(72, 90, 3000);
}

void turn_ccw() {
	turn(72, -90, 3000);
}

void go_to_point(float speed, float xx, float yy) {
	float req_dir = (float)(360. - 180./M_PI*atan2((double)(yy-y), (double)(xx-x)));
	printf("\n%d", (int)principal(req_dir-direction));
	turn(72, principal(req_dir-direction), 4000);
	float req_dist = (float)sqrt((double)(xx-x)*(xx-x)+(yy-y)*(yy-y));
	printf(" %f", (double)req_dist);
	drive_straight(speed, req_dist, 5000);
}

void go_to_point_backwards(float speed, float xx, float yy) {
	float req_dir = (float)(180 - 180./M_PI*atan2((double)(yy-y), (double)(xx-x)));
	printf("\n%d", (int)principal(req_dir-direction));
	turn(72, principal(req_dir-direction), 4000);
	float req_dist = (float)sqrt((double)(xx-x)*(xx-x)+(yy-y)*(yy-y));
	drive_straight(speed, -req_dist, 5000);
}

void turn_to_point(float xx, float yy) {
	float req_dir = (float)(360. - 180./M_PI*atan2((double)(yy-y), (double)(xx-x)));
	printf("\n%d %d %d", (int)direction, (int)req_dir, (int)principal(req_dir-direction));
	pause(2000);
	turn(72, principal(req_dir-direction), 4000);
}


void competition_routine_dead() {
	int val;
	find_start_direction();
	turn(70, principal(270-direction), 9999);
	back_to_wall(1000);
	drive_straight(96, 50, 4000);
	turn(80, 51.3, 4000);
	pick_up_ball();
	
	turn(80, -51.3, 4000);
	back_to_wall(3000);
	drive_straight(80, 8, 3000);
	turn(80, 90, 4000);
	back_to_wall(1000);
	drive_straight(80, 22.9, 3000);
	turn(80, -176, 4000);
	drive_straight(110, -110, 5000);

	
	//turn(80, 55.0-180, 4000);
	//pause(500);
	//drive_straight(110, -132.2, 5000);
	//pause(500);
	//turn(80, -20.6, 4000);
	back_to_wall(1000);
	dump_ball();
	
	pause(800);
	
	drive_straight(90, 50, 4000);
	val = turn(80, 90, 2000);
	
	if (val == 1) {
		drive_motors(90, 1000, 20, 300);
	}
	
	back_to_wall(1500);
	
	
	drive_straight(90, 36.1, 5000);
	pick_up_ball();
	back_to_wall(3000);
	drive_straight(80, 4, 1000);
	turn(80, -86, 2000);
	drive_straight(80, -53.3, 3000);
	back_to_wall(1000);
	dump_ball();
	
	pause(800);
	
	drive_straight(90, 45.7, 3000);
	val = turn(80, 90, 2000);
	
	if (val == 1) {
		drive_motors(90, 1000, 20, 300);
	}
	back_to_wall(1000);
	
	drive_straight(80, 140.6, 10000);
	turn(80, 90, 2000);
	drive_straight(80, -28.7, 3000);
	turn(80, -90, 2000);
	drive_straight(80, -10, 3000);
	turn_flag(90000);




}
////////////////////////////////////////////////
///////  usetup  and  umain  live   here  //////
////////////////////////////////////////////////

int usetup (void) {
	//set_auto_halt(90);
	servo_down_pos = 450;
	int pos = 0;
	printf("Use frob to set arm to down pos.");
	while(go_press()) ;
	while(!go_press() || pos<SERVO_UP_DIFF) {
		pos = frob_read()/2;
		servo_set_pos(SERVO_PORT, pos);
	}
	servo_down_pos = pos;
	
	dump_ball();
	pause(1000);
	return 0;
}

int umain (void) {
	competition_routine_dead();
	
	return 0;
}