#include "mcu_api.h"
#include "mcu_errno.h"

#define INPUT 0
#define OUTPUT 1

int echoPin = 128; //2
int trigPin = 13;  //5
int blinkPin = 40;  //13
int leftMotor = 0; //pwm3
int rightMotor = 2; //pwm6
unsigned long startT, endT, elapTime, stopTime, tmpTime1;
unsigned long counter = 1;


void pulseIn2(unsigned int state, int timeout) {
	int count = timeout;
	while (gpio_read(echoPin) != state && count > 0)
		count--;
}

void turnLeft(int leftMotor, int rightMotor, int duration) {
	pwm_configure(leftMotor,  2000000, 20000000);
	pwm_configure(rightMotor, 2000000, 20000000);
	pwm_enable(leftMotor);
	pwm_enable(rightMotor);
	mcu_sleep(duration);
}

void turnRight(int leftMotor, int rightMotor, int duration) {

	pwm_configure(leftMotor,  700000, 20000000);
	pwm_configure(rightMotor, 700000, 20000000);
	pwm_enable(leftMotor);
	pwm_enable(rightMotor);
	mcu_sleep(duration);
}

void go(int leftMotor, int rightMotor) {

	gpio_write(blinkPin, 0);

	pwm_configure(leftMotor,  700000, 20000000);
	pwm_configure(rightMotor, 2000000, 20000000);
	pwm_enable(leftMotor);
	pwm_enable(rightMotor);

}

void reverse(int leftMotor, int rightMotor, int duration) {
	gpio_write(blinkPin, 1);

	pwm_configure(leftMotor,  2000000, 20000000);
	pwm_configure(rightMotor, 700000, 20000000);
	pwm_enable(leftMotor);
	pwm_enable(rightMotor);
	mcu_sleep(duration);
}

void stopMotor(int leftMotor, int rightMotor) {
//	host_send((unsigned char*)"Stopped\n",8);

	pwm_disable(leftMotor);
	pwm_disable(rightMotor);
}

void incrStopTime(int reset) {
	if (tmpTime1 > 0) {
		stopTime += (time_ms() - tmpTime1);
		if (reset) tmpTime1 = 0;
		else tmpTime1=time_ms();
	};
}

void mcu_main() {
	unsigned int runMotor = 1;
	unsigned char btInput = 0;
	char buf[64];
	int len;
	unsigned long distTraveled;
	/* your configuration code starts here */
	startT = time_ms();
	elapTime = time_ms();
	stopTime = 0;

	gpio_setup(echoPin, INPUT);  //in
	gpio_setup(trigPin, OUTPUT);  //out
	gpio_setup(blinkPin, OUTPUT); //out

	while (1) {
		mcu_sleep(50);
		btInput = 0;
		len = host_receive((unsigned char *) buf, 64);
		if (len > 0) {
			btInput = (char) buf[0];
			switch (btInput) {
			case 'S':
				gpio_write(blinkPin, 1);
				stopMotor(leftMotor, rightMotor);
				tmpTime1 = time_ms();
				runMotor = 0;
				break;

			case 'A':
				gpio_write(blinkPin, 1);
				go(leftMotor, rightMotor);
				runMotor = 1;
				incrStopTime(1);
				break;

			case 'L':
				gpio_write(blinkPin, 1);
				turnLeft(leftMotor, rightMotor, 95);
				go(leftMotor, rightMotor);
				runMotor = 1;
				incrStopTime(1);

				break;
			case 'R':
				gpio_write(blinkPin, 1);
				//    reverse(1000);
				//    stopMotor();
				turnRight(leftMotor, rightMotor, 95);
				go(leftMotor, rightMotor);
				runMotor = 1;
				incrStopTime(1);

				break;
			case 'B':
				gpio_write(blinkPin, 1);
				turnLeft(leftMotor, rightMotor, 95);
				turnLeft(leftMotor, rightMotor, 95);
				go(leftMotor, rightMotor);
				runMotor = 1;
				incrStopTime(1);
				break;
			default:
				break;
			}
		}
		if (runMotor) {
			gpio_write(trigPin, 0);
			gpio_write(trigPin, 1);

			gpio_write(trigPin, 0);

			pulseIn2(1, 10000);
			long startTime = time_us();

			pulseIn2(0, 10000);
			long endTime = time_us();
			int duration = endTime - startTime;

			int distance = duration / 2 / 29; //29.1?

			if ((distance != 0) && (distance < 15)) {
				reverse(leftMotor, rightMotor, 95);
				stopMotor(leftMotor, rightMotor);
				turnLeft(leftMotor, rightMotor, 95);
				go(leftMotor, rightMotor);
			} else {
				go(leftMotor, rightMotor);
			}
		}
		endT = time_ms();
		if ((endT - startT) > 10000) {
			incrStopTime(0);
			startT = time_ms();
			distTraveled = (endT - elapTime - stopTime) * 12 / 60000;  // approximate feed driven based to static RPM of servos.
		};
		len = mcu_snprintf(buf, 16, "%d\n", distTraveled);
		host_send((unsigned char*) buf, len);
	}
}