using namespace std;
#include <unistd.h>
#include <iostream>
#include <math.h>
#include <time.h>
#include <stdio.h>
#include <string>
//#include <curses.h>
#include "manual.h"
#include "settings.h"
#include "wheels.h"
Manual::Manual(Wheels *oWheels_ptr, Servos *oServos_ptr, IO *oIO_ptr) {
	oWheels = oWheels_ptr;
	oServos = oServos_ptr;
	oIO = oIO_ptr;
}


Manual::~Manual() {
//	endwin();
}

void Manual::Runner(bool &motorTimerOn, int &motorTimerCount, int &curTimerThreshold)
{
	char input[500];
	char *badCmd = "XXX";
	int amount;
	int power;
	int tilt = 34;//from kallahar's section
	oServos->Set(SERVO_ARM, tilt);

	do {
		char second[1300];
		char * pch;
		char * result;
		char delims[]=" " ;

		cout << "Enter Command: "; 
		cin.getline(second, 1300);
		result = strtok(second, delims);
		cout << endl;
		if (result == NULL) {
			strncat(input, badCmd, 1);
		} else {
			memmove(input, result, strlen(result)+1);
//			input[strlen(result)+1]='\0';
		}
		//amount
		result = strtok(NULL, delims);
		if (result == NULL) {
			amount = -1;
		} else {
			amount = atoi(result);
			cout << "\nAmount=" << result << endl;
		}
		//power
		result = strtok(NULL, delims);
		if ( result == NULL) {
			power = 100; //defalut power is 100
		} else {
			power = atoi(result);
		}
//		cout << "Input Command";
//		cin>>command>>amount>>power;
//		const char *input= command.c_str();
		cout << "Commad Entered= " << input;
		cout << " " << amount << " " << power << " " << endl;
		amount = amount * 10;

		switch (toupper(input[0])) {
		case ' ': //stop
			oWheels->SetDesired(0, 0);
			cout << "Stop" << endl;
			break;

		case 'F': // forward
			oWheels->GoDesired(power, amount, motorTimerOn, motorTimerCount, curTimerThreshold); 
			cout << "Forward" << endl;
			break;

		case 'B': //blow
			if (toupper(input[1]) == 'L') { 
				oIO->SetMotor(MOTOR_FAN, 0);
				cout << "Fan Off" << endl;
				break;
			} else { //back
				oWheels->GoDesired(-power, amount, motorTimerOn, motorTimerCount, curTimerThreshold);
				cout << "Backward" << endl;
				break;
			}

		case 'R': //right
			oWheels->GoDesiredAngle(-amount, motorTimerOn, motorTimerCount, curTimerThreshold); 
			cout << "Right" << endl;
			break;

		case 'L': //left 
			oWheels->GoDesiredAngle(amount, motorTimerOn, motorTimerCount, curTimerThreshold);
			cout << "Left" << endl;
			break;

		case 'U': //arm up
			tilt += 1; 
			oServos->Set(SERVO_ARM, tilt);
			cout << "Arm Up" << endl;
			break;

		case 'D': //arm down
			tilt -= 1; 
			oServos->Set(SERVO_ARM, tilt); 
			cout << "Arm Down" << endl;
			break;

		case 'S': //suction on
			oIO->SetMotor(MOTOR_FAN, 100);
			cout << "Fan On" << endl; 
			break;

		case 'O': //fan off
			oIO->SetMotor(MOTOR_FAN, 0); 
			cout << "Fan Off" << endl;
			break;

		case 'Q':
			cout << "Quit" << endl;
			break;

		default:
			cout << "Unrecognized Command:" << input[0] << endl;
			break;
		} 
	} while(toupper(input[0]) != 'Q');
}


void Manual::Start() {
	char input[5];
//	char in2[5];
//	initscr(); cbreak(); keypad(stdscr, true);

	tilt = 34; // starting for arm, fudge
	cout << "Starting Manual Control, q=quit, ?=menu" << endl;
	do {
		cout << "Speed=" << speed << "; angle=" << angle << ": " << "; tilt=" << tilt << "; fan=" << fan << endl;

		gets(input);
		if (input[0] == 'w') { speed += 100; }
		if (input[0] == 's') { speed -= 100; }
		if (input[0] == 'a') { angle -= 90; }
		if (input[0] == 'd') { angle += 90; }
		if (input[0] == 'r') { tilt += 1; }
		if (input[0] == 'f') { tilt -= 1; }
		if (input[0] == 't') { fan = 100; }
		if (input[0] == 'g') { fan = 0; }
		if (input[0] == 'b') {
			cout << "Port Read: " << oIO->GetParallel(PORT_A) << endl;
		}
		if (input[0] == ' ') { speed = 0; angle = 0; }
		if (input[0] == '?') { cout << endl << "wsad, space=stop, r=up, f=down, t=fanon, g=fanoff, b=read, q=quit" << endl; }
		if (speed > 100) { speed = 100; } else if (speed < -100) { speed = -100; }
		if (angle > 90) { angle = 90; } else if (angle < -90) { angle = -90; }

		oWheels->SetDesired(speed, angle);
		oIO->SetMotor(MOTOR_FAN, fan);
		oServos->Set(SERVO_ARM, tilt);
	} while (input[0] != 'q');

	oWheels->SetDesired(speed, angle);

	cout << "Stopping" << endl;
}