// Basic Left wall following code
void wallFollowingL()
{
	int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
	while (1)
	{
		get_front_ir_dists(&leftFrontIR, &rightFrontIR);
		get_side_ir_dists(&leftSideIR, &rightSideIR);
		distAhead = get_us_dist();

		if (distAhead < 40){		
			if (leftFrontIR - rightFrontIR > 0){
				set_motors(-SPEED, SPEED);
			}

			else {
				set_motors(SPEED, -SPEED);
			}
		}


		if (leftFrontIR > 30 )
			{	set_motors(SPEED-10, SPEED+10); } //turn left

		else if (leftFrontIR < 15 )
			{	set_motors (SPEED+10, SPEED-10); } //turn right

		else
			{set_motors(SPEED, SPEED); } // Straight

	}
}

// Basic Right wall following code
void wallFollowingR()
{
	int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
	while (1)
	{

		get_front_ir_dists(&leftFrontIR, &rightFrontIR);
		get_side_ir_dists(&leftSideIR, &rightSideIR);
		distAhead = get_us_dist();

		if (distAhead < 45){	
			if (leftFrontIR - rightFrontIR > 0){
				set_motors(-SPEED, SPEED);
			}

			else {
				set_motors(20, -20);
			}
		}

		else if (rightFrontIR > 30 && rightSideIR > 25)
			{	set_motors(SPEED+30, SPEED-8); } //turn right

		else if (rightFrontIR < 20 )
			{	set_motors (SPEED-10, SPEED+25); } //turn left

		else
			{set_motors(SPEED+8, SPEED+8); } // Straight

	}


}

	///////////////////////////////////////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////////////////////////////////////
	//
	//					go to stuff   
	//
	///////////////////////////////////////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////////////////////////////////////
	bool goToAngle(int toGoAngle, float* locationData) {
		//spin in place until facing angle. motors will turn at same speed, but at opposite directions

		int motorDiffkp  = 10;
		int motorDiffMax = 200;
		int maxAngle = 35;

		//get psi so it is between 180 and -180, 0 being at one goal

		int psi = (int)(locationData[0] *  RAD_DEG_RATIO) - 90; 
		if (psi<-180) { psi +=360; }
		if (psi>180) { psi -=360; }

		//error in angle, factoring in rotational velocity should be taken care of by kalman filter
		int theta_angle = (psi-toGoAngle);

		int atAngleError = 10;

		if (theta_angle > 180 ) { theta_angle -= 360; } 
		if (theta_angle < -180) { theta_angle += 360; }

		//motorDiff is the difference in PWM between the two motors
		int motorDiff = (int)((float)(motorDiffkp)*(float)(abs(theta_angle))/(float)maxAngle+minPWM-20);

		if (abs(theta_angle) > maxAngle) { motorDiff = motorDiffMax; }

		if (theta_angle >0) {  set_motors( motorDiff, -motorDiff ); }
		else {			set_motors( -motorDiff, motorDiff ); }

		if ( abs(theta_angle) < atAngleError) { atAngle = true; return true;}
		else {atAngle = false; return false;}

	}

static void motor_drive() {
  if (g_motor_state.enabled) {
    set_motors(0, g_motor_state.direction * g_motor_state.speed);
  } else {
    set_motors(0, 0);
  }
}

//Calibrates the sensor
void calibrate(unsigned int *sensors, unsigned int *minv, unsigned int *maxv) {
	//say something to the user
	clear();
	lcd_goto_xy(0, 0);
	print(" Fluffy");
	lcd_goto_xy(0, 1);
	print("A=Go!");
	
	//wait on the calibration button
	wait_for_button_press(BUTTON_A);
	
	//wait for the user to move his hand
	delay_ms(500);

	//activate the motors
	set_motors(40, -40);
	
	//take 165 readings from the sensors...why not?
	int i;
	for (i = 0; i < 165; i++) {
		read_line_sensors(sensors, IR_EMITTERS_ON);
		update_bounds(sensors, minv, maxv);
		delay_ms(10);
	}

	//and turn the motors off, we're done
	set_motors(0, 0);
	
	delay_ms(750);
}

void turnToDirection(int turnDirection) {
	adjustAngle();
	int currentDir;
	currentDir = currentDirection();
	double target = (currentDir + turnDirection) * M_PI / 2;

	while(fabs(currentHeading() - target) > turnThreshold) {
		if(fabs(currentHeading() - target) < slowThreshold) {
			if(currentHeading() > target) {
				set_motors(-5,5);
			}
			if(currentHeading() < target) {
				set_motors(5,-5);
			}
		} else {
			if(currentHeading() > target) {
				set_motors(-15,15);
			}
			if(currentHeading() < target) {
				set_motors(15,-15);
			}
		}
		calcPos();
	}
	set_motors(0,0);
	adjustAngle();
	usleep(10000);
}

int main(void) {
	m_clockdivide(0);
	setup_pins();
	if ( RF_debug) {setupUSB();}
	setup_timer_1();
	setup_timer_3();
	m_bus_init();
	m_rf_open(chan,RX_add,p_length);

	int timer_3_cnt = 0;

	//sei();
	set_motors(0,0);
	while (1){
		if (check(TIFR3,OCF3A)){
			set(TIFR3, OCF3A);
			timer_3_cnt++;
			if ( timer_3_cnt % 10 ==0 ) {
				timer_3_cnt=0;
				m_green(TOGGLE);
			}
			if ( timer_3_cnt == 1000){ timer_3_cnt=0;}
		}
		if(new){
			switch ( receive_buffer[11] ) {
				case Single_Joy: single_joystick(); break;
				case Double_Joy: double_joystick(); break;
				case Tank_Mode: tank_driving(); break;
				case Mario_Kart: Mario_Drive(); break;
				default : set_motors(0,0); m_green(2);
			}
			if(RF_debug){ debug_rf(); }
		}
	}
}

// Turns according to the parameter dir, which should be 'L', 'R', 'S'
// (straight), or 'B' (back).
void turn(char dir, int angle)
{
   int time = (long)((float)(angle*MSPER90/9.0));

	switch(dir)
	{
	case 'L':
		// Turn left.
		set_motors(-40,40);
		delay_ms(time);
		break;
	case 'R':
		// Turn right.
		set_motors(40,-40);
		delay_ms(time);
		break;
	case 'B':
		// Turn around.
		set_motors(40,-40);
		delay_ms(time*2);
		break;
	case 'S':
		// Don't do anything!
		break;
	}
}

void turnLeft(int maxspd, int minspd, int degrees) 
{
	//causes robot to spin left to a certain angle.

	int left1, left2, right1, right2, drotated = 0;
	get_motor_encoders(&left1, &right1);
	int turnLength = RIGHT_ANGLE_TURN_ENC * degrees / 90;
	
	while(drotated < turnLength) 
	{
		get_motor_encoders(&left2, &right2);
		drotated = ((left1 - left2) + (right2 - right1)) / 2;

		if(drotated < (maxspd - minspd) * 2)
		{
			//accelerate phase
			set_motors(-(minspd + drotated / 2), minspd + drotated / 2);
		} 
		else if(drotated < turnLength - (maxspd - minspd) * 2) 
		{
			//max speed phase
			set_motors(-maxspd, maxspd);
		} 
		else
		{
			//decelerate phase
			set_motors(-(minspd + (turnLength - drotated) / 2), minspd + (turnLength - drotated) / 2);
		}
		positioncalc();
	}
	set_motors(0, 0);
}

// Basic Left wall following code
void wallFollowingL()
{
	int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
	while (1)
	//for(i = 0; i < 250; i++)
	{
		get_front_ir_dists(&leftFrontIR, &rightFrontIR);
		get_side_ir_dists(&leftSideIR, &rightSideIR);
		distAhead = get_us_dist(); 
		
		printf("i = %i\n", i);
		
		if ( rightFrontIR < 30 )
		{ turnRight (90.0, 5);} // super turn right
		
		else if (leftFrontIR > 30 && leftSideIR > 25)
		{ set_motors(SPEED-15, SPEED+50);} // super turn left
		
		else if (leftFrontIR > 38)
		{ set_motors(SPEED, 30);} // too far, turn left
		
		else if ( leftFrontIR < 18 )
		{ set_motors(SPEED*1.5, SPEED);}  // turn right
		
		else
		{set_motors(SPEED+5, SPEED+5); } // Straight
	}
}

void update_circle_velocities(float error){
    int pwr_change = format_velocity(fabs(error));
    int dir = field_state.drive_direction;
    if(field_state.substage == DRIVE_SUBSTAGE){
        /*if(fabs(mod_f(field_state.target_angle - mod_f(gyro_get_degrees(), 180), 180)) < ROTATION_THRESHOLD){
         if(current_vel == 0)
         current_vel = target_vel;
         }*/
        if(error*dir > 0){
            set_motors(dir*current_vel, dir*current_vel - dir*pwr_change);
            //printf("Left Vel: %d, Right Vel: %d\n", current_vel, current_vel - pwr_change);
        }
        else{
            set_motors(dir*current_vel - dir*pwr_change, dir*current_vel);
            //printf("Left Vel: %d, Right Vel: %d\n", current_vel - pwr_change, current_vel);
        }
    }
    else if(field_state.substage == TERRITORY_APPROACH_SUBSTAGE || field_state.substage == LEVER_APPROACH_SUBSTAGE || field_state.substage == DUMPING_SUBSTAGE || field_state.substage == PIVOT_SUBSTAGE || field_state.substage == LEVER_RETREAT_SUBSTAGE || field_state.substage == TERRITORY_RETREAT_SUBSTAGE){
        if(fabs(mod_f(field_state.target_angle - mod_f(gyro_get_degrees(), 180), 180)) < ROTATION_THRESHOLD){
            if(current_vel == 0){
                //used to know when we started moving toward the lever/territory
                field_state.stored_time = field_state.curr_time;
                current_vel = target_vel;
            }
        }
        if(error*dir > 0){
            set_motors(dir*current_vel + dir*pwr_change, dir*current_vel - dir*pwr_change);
            //printf("Left Vel: %d, Right Vel: %d\n", current_vel, current_vel - pwr_change);
        }
        else{
            set_motors(dir*current_vel - dir*pwr_change, dir*current_vel + dir*pwr_change);
            //printf("Left Vel: %d, Right Vel: %d\n", current_vel - pwr_change, current_vel);
        }
    }
}

// ### Experimental racing function
double race_to(double curr_coord[2], double x, double y){
    double steering = 1.8;                 // Ratio between the speed of each wheels.
    double error_margin_angle = 3;      // Degrees
    double speed = 70;
    double dx = x - curr_coord[0];
    double dy = y - curr_coord[1];
    
    double targetA = atan2(dx, dy);
    double dangle = targetA - face_angle;
    dangle += (dangle > to_rad(180)) ? -to_rad(360) : (dangle < -to_rad(180)) ? to_rad(360) : 0;

    double distance = fabs(sqrt(dx*dx + dy*dy));
    // If we need to turn, perform steering maneuver
    if(fabs(dangle) >= to_rad(error_margin_angle)){
        if (to_degree(dangle) > 0){
            set_motors( speed*steering, speed/steering);
        }
        else {
            set_motors(speed/steering, speed*steering);
        }
        printf("Steering: %f \n", to_degree(dangle));
    }
    else { // Otherwise just go straight;
        set_motors(speed, speed);
    }
    position_tracker(curr_coord);
    return distance;
}

void turn90()
{
	while( fabs(currentHeading() - (M_PI/2.0)) > turnThreshold)
	{
		if(fabs(currentHeading() - (M_PI/2.0)) < slowThreshold)
		{
			if(currentHeading() > (M_PI/2.0))
			{
				set_motors(-1,1);
			}

			if(currentHeading() < (M_PI/2.0))
			{
				set_motors(1,-1);
			}
		}

		else
		{
			if(currentHeading() > (M_PI/2.0))
			{
				set_motors(-15,15);
			}

			if(currentHeading() < (M_PI/2.0))
			{
				set_motors(15,-15);
			}
		}
		
		calcPos();
	}
	set_motors(0,0);
	printf("Target : %f\n", M_PI/2.0);
}

void loop()
{
    // Set LED green.
    set_color(RGB(0, 1, 0));
    // Spinup motors to overcome friction.
    spinup_motors();
    // Move straight for 2 seconds (2000 ms).
    set_motors(kilo_straight_left, kilo_straight_right);
    delay(2000);
    
    // Set LED red.
    set_color(RGB(1, 0, 0));
    // Spinup motors to overcome friction.
    spinup_motors();
    // Turn left for 2 seconds (2000 ms).
    set_motors(kilo_turn_left, 0);
    delay(2000);
    
    // Set LED blue.
    set_color(RGB(0, 0, 1));
    // Spinup motors to overcome friction.
    spinup_motors();
    // Turn right for 2 seconds (2000 ms).
    set_motors(0, kilo_turn_right);
    delay(2000);
    
    // Set LED off.
    set_color(RGB(0, 0, 0));
    // Stop for half a second (500 ms).
    set_motors(0, 0);
    delay(500);
}

void moveBackwards(int maxspd, int minspd, int dist) 
{
	//speeds up, moves the robot backwards a certain distance, and slows down.

	int left1, left2, right1, right2;
	get_motor_encoders(&left1, &right1);
	
	while(1)
	{
		get_motor_encoders(&left2, &right2);
		int dtravelled = ((left1 - left2) + (right1 - right2)) / 2;

		if(dtravelled < (maxspd - minspd) * 2)
		{
			//accelerate phase
			set_motors(-(minspd + dtravelled / 2), -(minspd + dtravelled / 2));
		} 
		else if(dtravelled < dist - (maxspd - minspd) * 2) 
		{
			//max speed phase
			set_motors(-maxspd, -maxspd);
		} 
		else if(dtravelled < dist)
		{
			//decelerate phase
			set_motors(-(minspd + (dist - dtravelled) / 2), -(minspd + (dist - dtravelled) / 2));
		}
		else
		{
			break;
		}
		positioncalc();
	}
	set_motors(0, 0);
}

void Mario_Drive(){
	int HLeft =  ((*(int*)(&receive_buffer[6])) * 3 )/ 2;
	if (receive_buffer[1]==1) // right
		{ left = -MK_reverse ; right = -MK_reverse;}
	else if (receive_buffer[0]==1 ) // left
		{ left = MK_drive ; right = MK_drive;}
	else {left = 0; right =0; set_motors(0,0); return;}


	if ( left > -60 ){ 	// going forward
		if (HLeft > 0){ 	// joystic is to the right
			right -= HLeft;
		}else {			// joystic is to the left
			left  += HLeft;
		}
	}else {		 	// going backword  ( left < 0)
		if (HLeft > 0){		// joystic is to the right
			right += HLeft;
		}else {			// joystic is to the left
			left  -= HLeft;
		}
	}
	set_motors(left,right);

}

/** driveAhead ************************************************************
 * Drive Ahead to the specified distance given in inches
 * @params distance -- the distance in inches
 * ***********************************************************************/
void driveAhead(double distance) {
	// motors forward for some time
	set_motors(SPEED, SPEED);
	delay_ms(WAIT_PER_INCH * distance);
	set_motors(0,0);
	// pause for a fraction of a second before continuing to smooth out the run
	delay_ms(250);
}

void turn_right(int power, int angle)
{
	int delay = (DELAY_PER_ANGLE_PER_POWER * angle)/power;

	set_motors(power,-power);
	delay_ms(delay);
	set_motors(0,0);
}

void correctToStraight(int motorSpeed) {
	if (get_side_ir_dist(LEFT) < 15) {
		set_motors(motorSpeed + 3, motorSpeed);
	}
	if (get_side_ir_dist(RIGHT) < 15) {
		set_motors(motorSpeed, motorSpeed + 3);
	}
}

// This is the main function, where the code starts.  All C programs
// must have a main() function defined somewhere.
int main()
{
	unsigned int sensors[5]; // an array to hold sensor values

	// set up the 3pi
	initialize();

	// This is the "main loop" - it will run forever.
	while(1)
	{
		// Get the position of the line.  Note that we *must* provide
		// the "sensors" argument to read_line() here, even though we
		// are not interested in the individual sensor readings.
		unsigned int position = read_line(sensors,IR_EMITTERS_ON);

		if(position < 1000)
		{
			// We are far to the right of the line: turn left.

			// Set the right motor to 100 and the left motor to zero,
			// to do a sharp turn to the left.  Note that the maximum
			// value of either motor speed is 255, so we are driving
			// it at just about 40% of the max.
			set_motors(0,100);

			// Just for fun, indicate the direction we are turning on
			// the LEDs.
			left_led(1);
			right_led(0);
		}
		else if(position < 3000)
		{
			// We are somewhat close to being centered on the line:
			// drive straight.
			set_motors(100,100);
			left_led(1);
			right_led(1);
		}
		else
		{
			// We are far to the left of the line: turn right.
			set_motors(100,0);
			left_led(0);
			right_led(1);
		}
	}

	// This part of the code is never reached.  A robot should
	// never reach the end of its program, or unpredictable behavior
	// will result as random code starts getting executed.  If you
	// really want to stop all actions at some point, set your motors
	// to 0,0 and run the following command to loop forever:
	//
	// while(1);
}