using namespace std;
#include <sys/time.h>
#include <signal.h>
#include <unistd.h>
#include <iostream>
#include <math.h>
#include <time.h>
#include <stdio.h>
#include "wheels.h"
#include "io.h"
#include "settings.h"

Wheels::Wheels(IO *oIO_ptr) {
	oIO = oIO_ptr;
}


Wheels::~Wheels() {
	oIO->SetMotor(MOTOR_L, 0);
	oIO->SetMotor(MOTOR_R, 0);
	oIO->SetMotor(MOTOR_FAN, 0);
}

void Wheels::GoDesired(int speed,int time, bool &motorTimerOn, int &motorTimerCount, int &curTimerThreshold) {
	printf("go desired start\n");
	SetDesired(speed,0.0);
	motorTimerOn=true;
	curTimerThreshold=time;
	printf("go desired stop %i\n",sleep(500));
}

int Wheels::getAngleTime(int angle) {
	cout << "GAT=" << angle << endl;
   angle = abs(angle);
   if (angle == 0)  return 0;
   if (angle <= 90) return 18;
   if (angle <= 85) return 17;
   if (angle <= 75) return 16;
   if (angle <= 65 && angle > 60) return 15;
   if (angle <= 60 && angle > 55) return 14;
   if (angle <= 55 && angle > 50) return 13;
   if (angle <= 50 && angle > 45) return 12;
   if (angle <= 45 && angle > 40) return 11;
   if (angle <= 40 && angle > 35) return 10;
   if (angle <= 35 && angle > 30) return 9;
   if (angle <= 30 && angle > 25) return 8;
   if (angle <= 25 && angle > 20) return 7;
   if (angle <= 20 && angle > 15) return 6;
   if (angle <= 15 && angle > 10) return 5;
   if (angle <= 10 && angle > 5)  return 4;
   if (angle <= 5  && angle > 0)  return 3;
  
	return -5; 
}

void Wheels::GoDesiredAngle(int angle,bool &motorTimerOn,int &motorTimerCount, int &curTimerThreshold) {
	angle = angle/10;
   printf("set desired angle\n");
	//  motorTimerOn = true;
	int numWholes = abs(angle/90);
	int remainderPart = angle % 90;
	cout << "wholes percent=" << numWholes << " " << remainderPart << endl;
	//for (int i = 0; i <numWholes; i++)
 
	//motorTimerOn = true;
	curTimerThreshold = getAngleTime(angle);
	motorTimerOn = true;
	cout << "CTT=" << curTimerThreshold << endl;
	if ( angle < 0 ) {
		SetDesiredByPercent(MOTOR_R, 100);
		SetDesiredByPercent(MOTOR_L, -100);
	}
	if(angle > 0) {
		SetDesiredByPercent(MOTOR_R, -100);
		SetDesiredByPercent(MOTOR_L, 100);
	}
 
/*
	motorTimerOn=true; 
	curTimerThreshold = getAngleTime(remainderPart);

	if (angle < 0) {
		SetDesiredByPercent(MOTOR_R, 100);
		SetDesiredByPercent(MOTOR_L, -100);
	}
	if (angle > 0) {
		SetDesiredByPercent(MOTOR_R, -100);
		SetDesiredByPercent(MOTOR_L, 100);
	}
*/
}


bool Wheels::SetDesired(int speed, float angle) {
	float l_pct, r_pct;
	int left, right;
	
	if (DEBUG) {
		cout << "SetDesired: speed=" << speed << "; angle=" << angle << endl;
	}

	if (speed < -100 || speed > 100) {
		if (DEBUG) {
			cout << "Wheels::SetDesired: Desired speed out of range" << endl;
		}
		return false;
	} else if (angle < -90 || angle > 90) {
		if (DEBUG) {
			cout << "Wheels::SetDesired: Desired angle out of range" << endl;
		}
		return false;
	} else {
		if (angle == 0) {
			l_pct = 100;
			r_pct = 100;
		} else if (angle < 0) {
			l_pct = 100 - 100 * (angle / 90);
			r_pct = 100;
		} else {
			l_pct = 100;
			r_pct = 100 - 100 * (angle / 90);
		}
		left  = (int)( ((double)speed / (double)l_pct) * 100 );
		right = (int)( ((double)speed / (double)r_pct) * 100 );

//cout << "left=" << left << "; right=" << right << endl;

		if (speed == 0 && angle > 0) {
			SetDesiredByPercent(MOTOR_L, -75);
			SetDesiredByPercent(MOTOR_R, 75);
		} else if (speed == 0 && angle < 0) {
			SetDesiredByPercent(MOTOR_L, 75);
			SetDesiredByPercent(MOTOR_R, -75);
		} else {
			SetDesiredByPercent(MOTOR_L, left);
			SetDesiredByPercent(MOTOR_R, right);
		}
		return true;
	}
}


bool Wheels::SetDesiredByPercent(int num, int percent) {
	// :: sets the desired motor speed ::
	// actual speed changes are handled by Update() which handles speedup/slowdown
	// num = MOTOR_L or MOTOR_R
	// percent = -100 for full backwards, 100 for full forwards, 40 = 40% forward

	if (DEBUG) {
//		cout << "SetDesiredByPercent: #" << num << "=" << percent << endl;
	}

	if (percent < -100 || percent > 100) { // out of range
		if (DEBUG) {
			cout << "Wheels::SetDesiredByPercent: Desired out of range" << endl;
		}
		return false;
	} else {
		switch (num) {
			case MOTOR_L: des_L = percent; break;
			case MOTOR_R: des_R = percent; break;
			default:
				if (DEBUG) {
					cout << "Invalid motor selection" << endl;
				}
				return false;
		}
		return true;
	}
}


int Wheels::GetDesired(int num) {
	// :: gets the current desired motor speed
	// num = MOTOR_L or MOTOR_R
	switch (num) {
		case MOTOR_L: return des_L; break;
		case MOTOR_R: return des_R; break;
		default:
			if (DEBUG) {
				cout << "Invalid motor selection" << endl;
			}
			return false;
	}
}

int Wheels::GetCurrent(int num) {
	// :: gets the current actual motor speed
	// num = MOTOR_L or MOTOR_R
	switch (num) {
		case MOTOR_L: return cur_L; break;
		case MOTOR_R: return cur_R; break;
		default:
			if (DEBUG) {
				cout << "Invalid motor selection" << endl;
			}
			return false;
	}
}


void Wheels::Update() {
	// :: actually changes the wheel speed ::
	// call every 100ms or so
	// we use this so that we don't go from full speed to stop (or full reverse) too quickly
	int diff = 0;
	int new_pct = 0;

	if (cur_L != des_L) {
		diff = des_L - cur_L;
		if (abs(diff) > MAX_CHANGE) {
			if (diff > 0) {
				new_pct = cur_L + MAX_CHANGE; // increase forwards (or decrease backwards)
			} else {
				new_pct = cur_L - MAX_CHANGE; // increase backwards (or decrease forwards)
			}
		} else {
			new_pct = cur_L + diff; // diff is already signed
		}
		if (DEBUG) {
//			cout << "New Motor L % = " << new_pct << endl;
		}
		cur_L = new_pct;
		
		// send to IO port
		oIO->SetMotor(MOTOR_L, new_pct);
	}

	if (cur_R != des_R) {
		diff = des_R - cur_R;
		if (abs(diff) > MAX_CHANGE) {
			if (diff > 0) {
				new_pct = cur_R + MAX_CHANGE; // increase forwards (or decrease backwards)
			} else {
				new_pct = cur_R - MAX_CHANGE; // increase backwards (or decrease forwards)
			}
		} else {
			new_pct = cur_R + diff; // diff is already signed
		}
		if (DEBUG) {
//			cout << "New Motor R % = " << new_pct << endl;
		}
		cur_R = new_pct;
		
		// send to IO port
		oIO->SetMotor(MOTOR_R, new_pct);
	}

}


void Wheels::Demo() {
/*
	// :: runs a short demo to show that the wheels work ::
	char input[5];

	// 1) move forward
	cout << "Press enter to start" << endl;
	
	gets(input);
	cout << "Moving Forward" << endl;
	SetDesiredByPercent(MOTOR_L, 100);
	SetDesiredByPercent(MOTOR_R, 100);

	// 2) turn left 90 degrees
	cout << "Press enter for next stage" << endl;
	getchar();
	gets(input);
	cout << "Turning Left" << endl;
	SetDesiredByPercent(MOTOR_L, -100);
	SetDesiredByPercent(MOTOR_R, 100);

	// 3) arc right
	cout << "Press enter for next stage" << endl;
	gets(input);
	cout << "Turning Right" << endl;
	SetDesiredByPercent(MOTOR_L, 100);
	SetDesiredByPercent(MOTOR_R, 50);

	// 4) arc left
	cout << "Press enter for next stage" << endl;
	gets(input);
	cout << "Turning Left" << endl;
	SetDesiredByPercent(MOTOR_L, 50);
	SetDesiredByPercent(MOTOR_R, 100);

	cout << "Press enter for next stage" << endl;
	gets(input);
	cout << "Backing Up" << endl;
	// 5) back up
	SetDesiredByPercent(MOTOR_L, -100);
	SetDesiredByPercent(MOTOR_R, -100);

	// 6) stop
	cout << "Press enter for next stage" << endl;
	gets(input);
	cout << "Stopping" << endl;
	SetDesiredByPercent(MOTOR_L, 0);
	SetDesiredByPercent(MOTOR_R, 0);

	cout << "Demo Complete" << endl;
*/
}