/*
 * 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 <board.h>
#include <kern/thread.h>
#include <stdlib.h>
#include <hal/delay.h>

#define WHITE 0
#define BLACK 1

#define SHOOTER_SERVO 1
#define ROT_SERVO 0

#define LEFT_MOTOR 2
#define RIGHT_MOTOR 3

#define LEFT_BUMPER 6
#define RIGHT_BUMPER 7

#define LEFT_ENCODER 24
#define RIGHT_ENCODER 25

#define RIGHT_DISTANCE 9
#define LEFT_DISTANCE 10
#define RIGHT_LED 12
#define LEFT_LED 13
#define GYRO_PORT 18
#define LEFT_IR 20
#define RIGHT_IR 22
#define BACK_LEFT_LED 23

#define LSB_MS_PER_DEG	1062.0

#define SHORT_LONG_THRESHOLD 55

#ifndef max
#define max( a, b ) ( ((a) > (b)) ? (a) : (b) )
#endif

// usetup is called during the calibration period. It must return before the
// period ends.

uint8_t init_scoring_target();
//uses frob knob to set scoring target for the match

uint8_t starting_position(int average);
//determines starting position and orients robot correctly

void gyro_correction(uint8_t speed);

bool detect_hole(uint8_t start_pos, int average);
//returns TRUE if a hole is detected

int calibrate_leds();
//returns average of black and white

int get_sensor_input(uint8_t port);
//averages 25 inputs on an analog port

void turn_n_degrees(char* dir, float degrees);
//turns n degrees in dir

int am_i_bumped();
//returns which bumper was bumped

void drive_straight(uint8_t speed, uint8_t start_pos);
//drives straight with a preference to bump the wall

void shoot(uint8_t start_pos, uint8_t num);

void encoder_correction(uint16_t left, uint16_t right, uint8_t speed);
//corrects for encoder deficiencies by quickly changing motor speeds (for one cycle)

uint8_t target;
int average;


int usetup (void) {

	servo_set_pos(SHOOTER_SERVO,360);
	pause(300);
	servo_set_pos(ROT_SERVO,250);
	pause(300);
	target = init_scoring_target();

	average = calibrate_leds();
	printf("\nPosition bot:");
	go_click();
	printf("\nCalibrating gyro:");
	gyro_init (GYRO_PORT, LSB_MS_PER_DEG, 5000L);
	
	return 0;
}

// Entry point to contestant code.
// Create threads and return 0.
int
umain (void) {
uint8_t start_pos = 0;
uint8_t bumper = 0;
uint8_t current_speed = 100;
uint8_t hole_count = 0;

	uint8_t num_fours, num_twos, num_ones = 0;
	num_fours = target/4;
	num_twos = (target%4)/2;
	num_ones = (target%4)%2;
	start_pos = starting_position(average);
	pause(250);
	
	
	drive_straight(current_speed, start_pos);
	int init_time = get_time();
	
	// Loop forever
	while (1) {
		/*while(1) {
			turn_n_degrees("right",90.0);
			pause(500);
			turn_n_degrees("right",90.0);
			pause(500);
			turn_n_degrees("left",90.0);
			pause(500);
			turn_n_degrees("left",90.0);
			pause(500);
		}*/
		
		printf("\ndegrees = %f", gyro_get_degrees());
		//left_count = encoder_read(LEFT_ENCODER);
		//right_count = encoder_read(RIGHT_ENCODER);
		if(abs(gyro_get_degrees()) > 10.0) {
			gyro_correction(current_speed);
		}
		/*if((left_count + right_count)/2 > 100) { 
			encoder_correction(left_count, right_count, current_speed); 
			encoder_reset(LEFT_ENCODER);
			encoder_reset(RIGHT_ENCODER);
			//shoot();
		}*/
			
		bumper = am_i_bumped();
		pause(50);
		switch(bumper){
			case(RIGHT_BUMPER): {
				motor_set_vel(LEFT_MOTOR, -80);
				motor_set_vel(RIGHT_MOTOR, -120);
				beep(200, 1000);
				pause(250);
				drive_straight(current_speed, start_pos);
				break;
			}
			
			case(LEFT_BUMPER): {
				motor_set_vel(LEFT_MOTOR, -80);
				motor_set_vel(RIGHT_MOTOR, -120);
				beep(200, 1000);
				pause(250);
				drive_straight(current_speed, start_pos);
				break;
			}
		}
		
		if(detect_hole(start_pos, average) && (get_time() - init_time) > 3000) { //make sure detect_hole doesn't trigger in starting area
			hole_count++;
			drive_straight(80, start_pos);
			printf("\nhole #%d",hole_count);
			pause(300);
			switch (hole_count) {
				case(2): {
					//if (num_twos) 
					shoot(start_pos, 6);
					drive_straight(-current_speed,start_pos);
					pause(7000);
					gyro_set_degrees(0.0);
					motor_brake(LEFT_MOTOR);
					motor_brake(RIGHT_MOTOR);
					break;
				}
				/*case(3): {
					if(start_pos == WHITE) {
						motor_set_vel(LEFT_MOTOR,195);
						motor_set_vel(RIGHT_MOTOR,80);
						pause(250);
						turn_n_degrees("right",80.0);
					}
					else if(start_pos == BLACK) {
						motor_set_vel(LEFT_MOTOR,80);
						motor_set_vel(RIGHT_MOTOR,195);
						pause(250);
						turn_n_degrees("left",80.0);	
					}
					drive_straight(current_speed, start_pos);
					break;
				}
				case(4): {
					if (num_ones) shoot(start_pos, num_ones);
					break;
				}
				case(6): {
					shoot(start_pos, num_fours);
					}
					break;
				}*/
				
			servo_set_pos(ROT_SERVO,250);
			//while(detect_hole(start_pos, average)){
			//	drive_straight(current_speed, start_pos);
			//}
			//drive_straight(current_speed,start_pos);
		}
	}
		
  		/*
		if(right_count >= 250) {
			motor_brake(RIGHT_MOTOR);
			motor_brake(LEFT_MOTOR);
			diff = right_count - left_count;
			encoder_reset(LEFT_ENCODER);
			encoder_reset(RIGHT_ENCODER);
			
			if(diff != 0) {
				int enc_count = 0;
				if(diff > 0) motor_set_vel(LEFT_MOTOR, 30); 
				else motor_set_vel(RIGHT_MOTOR, 30);
				diff = abs(diff);
				while(enc_count < diff){
					printf("\nIn nested while");
					
					left_count = encoder_read(LEFT_ENCODER);
					right_count = encoder_read(RIGHT_ENCODER);
					enc_count = max(left_count, right_count);
					pause(200);
				}
				motor_brake(RIGHT_MOTOR);
				motor_brake(LEFT_MOTOR);
			}
		}*/
		pause(50); 
	}
	

	// Will never return, but the compiler complains without a return
	// statement.
	return 0;
}

uint8_t starting_position(int average) {
	uint8_t start;
	if(analog_read(LEFT_LED) > average) {
		if(analog_read(RIGHT_LED) > average) {
			if(analog_read(BACK_LEFT_LED) > average) { //BBB
				turn_n_degrees("left", 180.0);
				start = BLACK;
			}
			else { //BBW
				turn_n_degrees("left", 90.0);
				start = BLACK;
			}
		}
		else {
			if(analog_read(BACK_LEFT_LED) > average) { //BWB
				turn_n_degrees("right", 105.0);
				start = BLACK;
			}
			else { //BWW
				turn_n_degrees("left", 105.0);
				start = WHITE;
			}
		}
	}
	else {
		if(analog_read(RIGHT_LED) > average) {
			if(analog_read(BACK_LEFT_LED) > average) { //WBB
				turn_n_degrees("nothing", 0.0);
				start = BLACK;
			}
			else { //WBW
				turn_n_degrees("nothing", 0.0);
				start = WHITE;
			}
		}
		else {
			if(analog_read(BACK_LEFT_LED) > average) { //WWB
				turn_n_degrees("right", 180.0);
				start = WHITE;
			}
			else { //WWW
				turn_n_degrees("right", 90.0);
				start = WHITE;
			}
		}
	}
	drive_straight(110, start);
	return start;
}

/*uint8_t starting_position(int average) {
	uint8_t start;
	//int left_sensor = get_sensor_input(LEFT_DISTANCE);
	//int right_sensor = get_sensor_input(RIGHT_DISTANCE);
	if (left_sensor > right_sensor) { //left sensor against a wall
		if (right_sensor > SHORT_LONG_THRESHOLD) { //right sensor points across the table
			if(analog_read(LEFT_LED) > average) {  //black
				//turn_n_degrees("right", 180.0);
				start = BLACK;
				printf("\nls%i rs%i, ll%i, rl%i, s%i", left_sensor, right_sensor, analog_read(LEFT_LED), analog_read(RIGHT_LED), start);
			}
			else { //white
				//turn_n_degrees("nothing",0);
				start = WHITE;
				printf("\nls%i rs%i, ll%i, rl%i, s%i", left_sensor, right_sensor, analog_read(LEFT_LED), analog_read(RIGHT_LED), start);			}
		}
		else { //right sensor points down the table
			if(analog_read(LEFT_LED) > average) {  //black
				//turn_n_degrees("right",90.0);
				start = BLACK;
				printf("\nls%i rs%i, ll%i, rl%i, s%i", left_sensor, right_sensor, analog_read(LEFT_LED), analog_read(RIGHT_LED), start);
			}
			else { //white
				//turn_n_degrees("right",90.0);
				start = WHITE;
				printf("\nls%i rs%i, ll%i, rl%i, s%i", left_sensor, right_sensor, analog_read(LEFT_LED), analog_read(RIGHT_LED), start);
			}
		}
	}
	else { //right sensor against a wall
		if(left_sensor > SHORT_LONG_THRESHOLD) { //left sensor points across table
			if(analog_read(RIGHT_LED) > average) {  //black
				//turn_n_degrees("nothing",0);
				start = BLACK;
				printf("\nls%i rs%i, ll%i, rl%i, s%i", left_sensor, right_sensor, analog_read(LEFT_LED), analog_read(RIGHT_LED), start);
			}
			else { //white
				//turn_n_degrees("left", 180.0);
				start = WHITE;
				printf("\nls%i rs%i, ll%i, rl%i, s%i", left_sensor, right_sensor, analog_read(LEFT_LED), analog_read(RIGHT_LED), start);
			}
		}
		else { //left sensor points down the table
			if(analog_read(RIGHT_LED) > average) {  //black
				//turn_n_degrees("left",90.0);
				start = BLACK;
				printf("\nls%i rs%i, ll%i, rl%i, s%i", left_sensor, right_sensor, analog_read(LEFT_LED), analog_read(RIGHT_LED), start);
			}
			else { //white
				//turn_n_degrees("left",90.0);
				start = WHITE;
				printf("\nls%i rs%i, ll%i, rl%i, s%i", left_sensor, right_sensor, analog_read(LEFT_LED), analog_read(RIGHT_LED), start);
			}
		}
	}
	//drive_straight(100, start);
	//pause(700);
	return start;
}*/

void gyro_correction(uint8_t speed) {
	motor_brake(LEFT_MOTOR);
	motor_brake(RIGHT_MOTOR);
	pause(400);
	if (gyro_get_degrees() > 0) {
		motor_set_vel(LEFT_MOTOR,speed+30);
		motor_set_vel(RIGHT_MOTOR,speed-30);
	}
	else if (gyro_get_degrees() < 0) {
		motor_set_vel(RIGHT_MOTOR,speed+30);
		motor_set_vel(LEFT_MOTOR,speed-30);
	}
	pause(1000);
}

uint8_t init_scoring_target() {
	uint8_t frob_reading = 0;
	while(!go_press()){
		frob_reading = frob_read_range(15,23);
		printf("\nScore = %d", frob_reading);
		pause(100);
	}
	return frob_reading;
}

int calibrate_leds() {
	printf("\nCalibrate black:");
	go_click();
	int black = analog_read(RIGHT_LED);
	printf("\nCalibrate white:");
	go_click();
	int white = analog_read(RIGHT_LED);
	return (black + white)/2;
}

int get_sensor_input(uint8_t port) {
	int output, total=0;
	for(int i=0;i<25;i++) {
		output = analog_read(port);
		total+=output;
	}	
	return total/25;
}


int am_i_bumped() {
	if(digital_read(RIGHT_BUMPER)) return RIGHT_BUMPER;
	if(digital_read(LEFT_BUMPER)) return LEFT_BUMPER;
	return 0;
}

void drive_straight(uint8_t speed, uint8_t start_pos) {
	printf("\ndriving straight");
	if(start_pos == WHITE){
		motor_set_vel(LEFT_MOTOR, speed-10); 
		motor_set_vel(RIGHT_MOTOR, speed+40);
	}
	else if(start_pos == BLACK){
		motor_set_vel(LEFT_MOTOR, speed+50); 
		motor_set_vel(RIGHT_MOTOR, speed-30);
	}
	return;
}

void encoder_correction(uint16_t left, uint16_t right, uint8_t speed) {
	printf("\nencoder correction");
	if(left > (right+20)) {
		motor_set_vel(RIGHT_MOTOR, (speed + 20));
	}
	else if(right > (left+20)) {
		motor_set_vel(LEFT_MOTOR, (speed + 20));
	}
	pause(100);
}

void turn_n_degrees(char* dir, float degrees) {
	//uint16_t enc_count = 0, enc_target = 26;
	//if(dir=="180") enc_target = 54;
	if(dir=="nothing") return;
	float diff = 0;
	printf("\nTurning %f degrees %s", degrees, dir);
	motor_brake(LEFT_MOTOR);
	motor_brake(RIGHT_MOTOR);
	encoder_reset(LEFT_ENCODER);
	encoder_reset(RIGHT_ENCODER);
	gyro_set_degrees(0.0);
	pause(500);
	while(abs(gyro_get_degrees()) < degrees){
		diff = 60*(degrees - abs(gyro_get_degrees()))/180;
		if(dir == "right"){
			motor_set_vel(LEFT_MOTOR, (100 + diff));
			motor_set_vel(RIGHT_MOTOR, (-80 - diff));	
		}
		else if(dir == "left"){
			motor_set_vel(LEFT_MOTOR, (-80 - diff));
			motor_set_vel(RIGHT_MOTOR, (100 + diff));
		}
		else break;
		
		//enc_count = (encoder_read(LEFT_ENCODER) + encoder_read(RIGHT_ENCODER))/2;
		pause(100);
	}
	motor_brake(LEFT_MOTOR);
	motor_brake(RIGHT_MOTOR);
	pause(500);
	float error = abs(gyro_get_degrees()) - degrees;
	gyro_set_degrees(0.0);
	while(abs(gyro_get_degrees()) < error){
		diff = 60*(degrees - abs(gyro_get_degrees()))/180;
		if(dir == "right"){
			motor_set_vel(LEFT_MOTOR, (-60 - diff));
			motor_set_vel(RIGHT_MOTOR, (80 + diff));
		}
		else if(dir == "left"){
			motor_set_vel(LEFT_MOTOR, (80 + diff));
			motor_set_vel(RIGHT_MOTOR, (-60 - diff));
		}
		else break;
		
		//enc_count = (encoder_read(LEFT_ENCODER) + encoder_read(RIGHT_ENCODER))/2;
		pause(100);
	}
	gyro_set_degrees(0.0);
	encoder_reset(LEFT_ENCODER);
	encoder_reset(RIGHT_ENCODER);
	motor_brake(LEFT_MOTOR);
	motor_brake(RIGHT_MOTOR);
	pause(100);
}

bool detect_hole(uint8_t start_pos, int average) {
	if(start_pos == WHITE) {
		if(analog_read(LEFT_IR)>average && analog_read(RIGHT_LED)>average) return 1;
		else return 0;
	}
	else if(start_pos == BLACK){
		if(analog_read(RIGHT_IR)>average && analog_read(LEFT_LED)>average ) return 1;
		else return 0;
	}
	return 0;
}

void shoot(uint8_t start_pos, uint8_t num) {
	motor_brake(LEFT_MOTOR);
	motor_brake(RIGHT_MOTOR);
	if(start_pos == WHITE) servo_set_pos(ROT_SERVO,495);
	else if(start_pos == BLACK) servo_set_pos(ROT_SERVO,10);
	pause(300);
	drive_straight(110,start_pos); //drive a little more
	pause(500);
	motor_brake(LEFT_MOTOR);
	motor_brake(RIGHT_MOTOR);
	for(int i=0;i<num;i++){
		servo_set_pos(SHOOTER_SERVO, 360);
		pause(500);
		servo_set_pos(SHOOTER_SERVO, 60);
		pause(700);
	}
}