#include "OMG_Pirates.h"


//#define MOTOR_LEFT1 0
#define MOTOR_LEFT2 0
#define MOTOR_LEFT3 1
//#define MOTOR_RIGHT1 1
#define MOTOR_RIGHT2 2
#define MOTOR_RIGHT3 3

#define LED_LF 8
#define LED_RF 10
#define LED_LR 12
#define LED_RR 14

#define ENCODER_LEFT 27
#define ENCODER_RIGHT 24

#define BUMP_SENSOR_LEFT 1
#define BUMP_SENSOR_RIGHT 0
#define BUMP_SENSOR_BACK 2

int min_motor_speed = 40;       // min motor speed8
int max_motor_speed = 100;       // max motor speed

//int start_time;

int left_drive_vel = 80;		// Left motor driving velocity
int right_drive_vel = 80;		// Right motor driving velocity
int default_drive_vel = 245;

int init_left_encoder = 0;
int init_right_encoder = 0;

void holdGround(){
//	motorSetVel(MOTOR_LEFT1, 2);
	motorSetVel(MOTOR_LEFT2, -1);
	motorSetVel(MOTOR_LEFT3, -1);
//	motorSetVel(MOTOR_RIGHT1, 2);
	motorSetVel(MOTOR_RIGHT2, -1);
	motorSetVel(MOTOR_RIGHT3, -1);
}

void setLeftMotorVel(int speed){
//	motorSetVel(MOTOR_LEFT1, speed);
	motorSetVel(MOTOR_LEFT2, speed);
	motorSetVel(MOTOR_LEFT3, speed);
}

void setRightMotorVel(int speed){
//	motorSetVel(MOTOR_RIGHT1, speed);
	motorSetVel(MOTOR_RIGHT2, speed);
	motorSetVel(MOTOR_RIGHT3, speed);
}

void hitBrakes(){
//	motorBrake(MOTOR_LEFT1);
	motorBrake(MOTOR_LEFT2);
	motorBrake(MOTOR_LEFT3);
//	motorBrake(MOTOR_RIGHT1);
	motorBrake(MOTOR_RIGHT2);
	motorBrake(MOTOR_RIGHT3);
}

void backUp(int speed, long shaftcount) {
  uint8_t tweak = 3;
//  uint8_t endzone = 30;
  uint16_t left_count=0;
  uint16_t right_count=0;
  uint8_t bumpBack;
//  uint8_t bumpRight;
  uint16_t original_left;
  uint16_t original_right;

  original_left = encoderRead(ENCODER_LEFT);
  original_right = encoderRead(ENCODER_RIGHT);
  
  while (left_count+right_count <= shaftcount*2) {
	bumpBack = digitalGet(BUMP_SENSOR_BACK);
//	bumpRight = digitalGet(BUMP_SENSOR_RIGHT);
	if(bumpBack==1){
		return;
	}
	left_count = encoderRead(ENCODER_LEFT)-original_left;
	right_count = encoderRead(ENCODER_RIGHT)-original_right;
	printf("\nleftcount=%6d", left_count);
	printf("rightcount=%5d", right_count);
	
		setRightMotorVel(.95*(-speed + tweak*(right_count - left_count)));
		setLeftMotorVel(-speed - tweak*(right_count - left_count));

  }  
	hitBrakes();
}

void spin(int speed, int shaftcount){
	uint16_t left_count=0;
	uint16_t right_count=0;
	uint16_t original_left;
	uint16_t original_right; 

	original_left = encoderRead(ENCODER_LEFT);
	left_count = encoderRead(ENCODER_LEFT);
	original_right = encoderRead(ENCODER_RIGHT);
	right_count = encoderRead(ENCODER_RIGHT);
	
	while((left_count-original_left<=shaftcount)||(right_count-original_right<=shaftcount)){
		left_count = encoderRead(ENCODER_LEFT);
		right_count = encoderRead(ENCODER_RIGHT);
		setRightMotorVel(-speed);
		setLeftMotorVel(speed);
	}
	hitBrakes();
}
	

void charge(int shaftcount){
  uint8_t tweak = 3;
  uint16_t left_count=0;
  uint16_t right_count=0;
  uint16_t original_left;
  uint16_t original_right;

  original_left = encoderRead(ENCODER_LEFT);
  original_right = encoderRead(ENCODER_RIGHT);
  
  while (left_count+right_count <= shaftcount*2) {
	left_count = encoderRead(ENCODER_LEFT)-original_left;
//	if(left_count<0){
//		left_count+=256;
//	}
//	right_count = encoderRead(ENCODER_RIGHT)-original_right;
//	if(right_count<0){
//		right_count+=256;
//	}
	printf("\nleftcount=%6d", left_count);
	printf("rightcount=%5d", right_count);
	
	setLeftMotorVel(240 + tweak*(right_count - left_count));
	setRightMotorVel(240 - tweak*(right_count - left_count));
	}

  
	hitBrakes();
}

void turnRight(int speed, int angle){
	uint16_t left_count;
	uint16_t original_left;
	uint16_t right_count;
	uint16_t original_right;
    uint16_t nintyfactor=20;
	uint16_t turns;
	uint16_t lefttrue=0;
	uint16_t righttrue=0;
    uint16_t speeda;
	
    if(angle > 180){
        turns=(360-angle)*nintyfactor/90;
        speeda=-speed;
	}else{
        turns=angle*nintyfactor/90;
        speeda=speed;
    }

/*    if(angle==90)
        turns=19;
    if(angle==270){
        turns=17;
    }
	*/

	original_left = encoderRead(ENCODER_LEFT);
	left_count = encoderRead(ENCODER_LEFT);
	original_right = encoderRead(ENCODER_RIGHT);
	right_count = encoderRead(ENCODER_RIGHT);
	while((left_count-original_left<=turns)||(right_count-original_right<=turns)){
		left_count = encoderRead(ENCODER_LEFT);
		if(left_count-original_left<=turns){
			lefttrue=1;
		}
		else{
			lefttrue=0;
		}
		right_count = encoderRead(ENCODER_RIGHT);
		if(right_count-original_right<=turns){
			righttrue=1;
		}
		else{
			righttrue=0;
		}
			setLeftMotorVel(speeda*lefttrue);
			setRightMotorVel(-speeda*righttrue);

		//printf("\nleft: %d right: %d", left_count-original_left, right_count-original_right);
		battery();
	}
	hitBrakes();
}

void drive(int speed, long shaftcount) {
  uint8_t tweak = 3;
  uint8_t endzone = 30;
  uint16_t left_count=0;
  uint16_t right_count=0;
  uint8_t bumpLeft;
  uint8_t bumpRight;
  uint16_t original_left;
  uint16_t original_right;

  original_left = encoderRead(ENCODER_LEFT);
  original_right = encoderRead(ENCODER_RIGHT);
  
  while (left_count+right_count <= shaftcount*2) {
	bumpLeft = digitalGet(BUMP_SENSOR_LEFT);
	bumpRight = digitalGet(BUMP_SENSOR_RIGHT);
	left_count = encoderRead(ENCODER_LEFT)-original_left;
	right_count = encoderRead(ENCODER_RIGHT)-original_right;
	printf("\nleftcount=%6d", left_count);
	printf("rightcount=%5d", right_count);
	
	// Check bump sensors. If both are on, we hit the wall straight on.
	if ((bumpLeft == 1) && (bumpRight == 1)) {
        beep(3,1000);
		return;
	}

	else if (bumpLeft == 1){
		if(left_count+right_count <= shaftcount*2-endzone){
			original_left+=2;			
		}
		else{
			setLeftMotorVel(speed/6);
			setRightMotorVel(speed);
		}
	}
	
	else if (bumpRight == 1){
		if(left_count+right_count <= shaftcount*2-endzone){
			original_right+=2;			
		}
		else{
			setLeftMotorVel(speed);
			setRightMotorVel(speed/6);
		}
	}
	
	else {
		setLeftMotorVel(speed + tweak*(right_count - left_count));
		setRightMotorVel(.82*(speed - tweak*(right_count - left_count)));

	}
  }  
	hitBrakes();
}

int ohelp(){
uint16_t LFvalue;
uint16_t LRvalue;
uint16_t RFvalue;
uint16_t RRvalue;

	if(analogRead(LED_LF)>2000){
		LFvalue=1;
	}
	else{
		LFvalue=0;
	}
	
	if(analogRead(LED_LR)>1400){
		LRvalue=1;
	}
	else{
		LRvalue=0;
	}
	
	if(analogRead(LED_RF)>2000){
		RFvalue=1;
	}
	else{
		RFvalue=0;
	}
	
	if(analogRead(LED_RR)>1400){
		RRvalue=1;
	}
	else{
		RRvalue=0;
	}
	

	//printf("\nLF: %d      RF: %dLR: %d      RR: %d", LFvalue, RFvalue, LRvalue, RRvalue);
	printf("\n%2d %5d %2d %4d %d %5d %2d %5d", LFvalue, analogRead(LED_LF), RFvalue, analogRead(LED_RF), LRvalue, analogRead(LED_LR), RRvalue, analogRead(LED_RR));
	if((LFvalue+RFvalue+LRvalue+RRvalue)%2==1){
		return 0;
	}
	else{
		return 1;
	}
}

int orient(){

//uint16_t threshold=2000;
uint16_t LFvalue;
uint16_t LRvalue;
uint16_t RFvalue;
uint16_t RRvalue;

	if(analogRead(LED_LF)>2000){
		LFvalue=1;
	}
	else{
		LFvalue=0;
	}
	
	if(analogRead(LED_LR)>1400){
		LRvalue=1;
	}
	else{
		LRvalue=0;
	}
	
	if(analogRead(LED_RF)>2000){
		RFvalue=1;;
	}
	else{
		RFvalue=0;
	}
	
	if(analogRead(LED_RR)>1400){
		RRvalue=1;
	}
	else{
		RRvalue=0;
	}
	
	switch(LFvalue+LRvalue+RFvalue+RRvalue){
		case 0:
			return(1);
			break;
		case 1:
			if(LFvalue==1){
				turnRight(default_drive_vel, 270);
				return(0);
			}
			if(LRvalue==1){
				turnRight(default_drive_vel, 180);
				return(0);
			}
			if(RFvalue==1){
				return(0);
			}
			if(RRvalue==1){
				turnRight(default_drive_vel, 90);
				return(0);
			}
			break;
		case 2:
			return(1);
			break;
		case 3:
			if(LFvalue==0){
				turnRight(default_drive_vel, 270);
				return(0);
			}
			if(LRvalue==0){
				turnRight(default_drive_vel, 180);
				return(0);
			}
			if(RFvalue==0){
				return(0);
			}
			if(RRvalue==0){
				turnRight(default_drive_vel, 90);
				return(0);
			}
			break;
		case 4:
			return(1);
			break;
	}
	
	return 1;
}

void battery(){
	uint16_t voltage;

	voltage=readBattery();
	printf("\nBattery Voltage: %d", voltage);               
}

// usetup is called during the calibration period. It must return before the
// period ends.
int usetup (void) {
	goClick();
	rf_set_team(21)	;
	while(ohelp()!=0);
	pause(1500L);
	return 0;	
}

int umain (void) {
//uint32_t start_time;
uint16_t voltage;

//start_time=get_time();

battery();
//printf("\nPlace robot,    press go.\n");

voltage=readBattery();
if(voltage<75){
	beep(3, 1000);
}

while (1) {
	voltage=readBattery();
	if(voltage<75){
		beep(3, 1000);
	}
    if((orient()!=0)&&(orient()!=0)){
		drive(default_drive_vel, 300);
		hitBrakes();
		backUp(240,300);
	}
	else{
		drive(default_drive_vel, 170);
		hitBrakes();
		// 10000L pause is equal to 20 seconds.
		pause(5000L);
		backUp(240,190);
		drive(150,4);                                                                                                                                                                                                                                                                                                                                                                             
		turnRight(default_drive_vel, 105); 
		backUp(240,80);
		drive(default_drive_vel, 270);
		hitBrakes();
		backUp(150, 15); 
		// From here to the end of program, takes 10 seconds total at motor speed = 240.
		turnRight(default_drive_vel, 210);
		pause(1000L);
		drive(default_drive_vel, 140);
		spin(default_drive_vel, 800);
	}
	hitBrakes();
}

	return 0;
}