#pragma config(Motor, port2, leftMotor, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor, port3, rightMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(UART_Usage, UART1, uartUserControl, baudRate1200, IOPins, None, None)
#pragma config(Motor, port9, armMotor, tmotorServoContinuousRotation, openLoop, reversed)

// ANCHOR Baud rate library
typedef unsigned long uint32_t;
typedef unsigned short uint16_t;
typedef struct
{
	uint16_t SR;
	uint16_t RESERVED0;
	uint16_t DR;
	uint16_t RESERVED1;
	uint16_t BRR;
	uint16_t RESERVED2;
	uint16_t CR1;
	uint16_t RESERVED3;
	uint16_t CR2;
	uint16_t RESERVED4;
	uint16_t CR3;
	uint16_t RESERVED5;
	uint16_t GTPR;
	uint16_t RESERVED6;
} USART_TypeDef;

/* Peripheral memory map */
#define PERIPH_BASE ((unsigned long)0x40000000)
#define APB1PERIPH_BASE PERIPH_BASE
#define USART2_BASE (APB1PERIPH_BASE + 0x4400)
#define USART3_BASE (APB1PERIPH_BASE + 0x4800)
#define USART2 ((USART_TypeDef *)USART2_BASE)
#define USART3 ((USART_TypeDef *)USART3_BASE)
void setBaud(const TUARTs nPort, int baudRate)
{
	uint32_t tmpreg = 0x00, apbclock = 0x00;
	uint32_t integerdivider = 0x00;
	uint32_t fractionaldivider = 0x00;
	/* pclk1 - 36MHz */
	apbclock = 36000000;
	/* Determine the integer part */
	integerdivider = ((0x19 * apbclock) / (0x04 * (baudRate)));
	tmpreg = (integerdivider / 0x64) << 0x04;
	/* Determine the fractional part */
	fractionaldivider = integerdivider - (0x64 * (tmpreg >> 0x04));
	tmpreg |= ((((fractionaldivider * 0x10) + 0x32) / 0x64)) & 0x0F;
	/* Write to USART BRR */
	USART_TypeDef *uart = USART2;
	if (nPort == UART2)
	{
		uart = USART3;
	}
	uart->BRR = (uint16_t)tmpreg;
}

// ANCHOR Variable state definitions
enum IRState {
	Black,
	White
};

enum AutonomousState {
	Disabled,
	LeftInner,
	LeftOuter,
	RightInner,
	RightOuter
}

int recvdCodes = 0;
AutonomousState autonomousState = Disabled;
IRState floorColor = Black;

// SECTION Function/task definitions
task sendTask();
task detectTask();
task recvTask();

// Main program loop
task main() {
	// Prep IR sensors
	setBaud(UART1, 600);

	startTask(sendTask);
	startTask(detectTask);
	startTask(recvTask);

	while (true) {
		// Toggle autonomous mode
		if (vexRT[Btn8U]) {
			clearTimer(T1);
			autonomousState = RightOuter;
		} else if (vexRT[Btn8L]) {
			clearTimer(T1);
			autonomousState = RightInner;
		} else if (vexRT[Btn7R]) {
			clearTimer(T1);
			autonomousState = LeftInner;
		} else if (vexRT[Btn7U]) {
			clearTimer(T1);
			autonomousState = LeftOuter;
		} else if (vexRT[Btn8D]) {
			autonomousState = Disabled;
		}

		// SECTION IR motor control
		if (autonomousState == RightInner) {
			// ANCHOR Right inner
			if (time1[T1] >= 1000 && floorColor == Black) {
				motor[leftMotor] = 50;
				motor[rightMotor] = 25;
			} else if (time1[T1] < 1000 && floorColor == Black) {
				motor[leftMotor] = 45;
				motor[rightMotor] = 30;
			} else if (floorColor == White) {
				motor[leftMotor] = -40;
				motor[rightMotor] = 40;
			}
		} else if (autonomousState == RightOuter) {
			// TODO Right outer
		} else if (autonomousState == LeftInner) {
			// ANCHOR Left inner
			if (time1[T1] >= 1000 && floorColor == Black) {
				motor[leftMotor] = 25;
				motor[rightMotor] = 50;
			} else if (time1[T1] < 1000 && floorColor == Black) {
				motor[leftMotor] = 30;
				motor[rightMotor] = 45;
			} else if (floorColor == White) {
				motor[leftMotor] = 40;
				motor[rightMotor] = -40;
			}
		} else if (autonomousState == LeftOuter) {
			// TODO Left outer
		} 
		// !SECTION IR motor control
		// SECTION Drive control
		else {
			// Store joystick axises in variables to allow modification
			int verticalAxis = vexRT[Ch3];
			int horizontalAxis = vexRT[Ch4];
			int armAxis = vexRT[Ch2];

			// Check if the joystick is pushed enough, if it isn't a significant value,
			// then act as if it was never pushed at all, by setting the value to 0
			// ANCHOR Deadzones
			if (verticalAxis < 15 && verticalAxis > -15) verticalAxis = 0;
			if (horizontalAxis < 15 && horizontalAxis > -15) horizontalAxis = 0;
			if (armAxis < 15 && armAxis > -15) armAxis = 0;

			// Halve the joystick values if the slow mode button is pressed to allow
			// precision for specific tasks
			// ANCHOR Slow/fast mode
			if (vexRT[Btn6U]) {
				verticalAxis = verticalAxis / 2;
				horizontalAxis = horizontalAxis / 2;
				if (armAxis > 0) { // If putting arm up
					armAxis = armAxis * 0.95;
				} else {
					armAxis = armAxis / 4;
				}
			} else if (vexRT[Btn6D]) {
				verticalAxis = verticalAxis * 2;
				horizontalAxis = horizontalAxis * 2;
				armAxis = armAxis * 2;
			}

			// ANCHOR Set motor speeds
			motor[leftMotor] = (verticalAxis + horizontalAxis)/2;
			motor[rightMotor] = (verticalAxis - horizontalAxis)/2;

			// Check for arm stable button else joystick
			if (vexRT[Btn5U]) {
				motor[armMotor] = 15;
			} else {
				motor[armMotor] = armAxis;
			}
		}
		// !SECTION Drive control

		sleep(3);
	}
}

task sendTask() {
	while (true) {
		sendChar(UART1, 17);
		sleep(100);
	}
}

// Make a task with the job of constantly calculating the floor color based on recvd code count
task detectTask()
{
	int cached = recvdCodes; // Cache the recvdCodes variable to allow checking for change

	while (true) {
		// Check if the amount of recvd codes has not changed
		if (cached == recvdCodes) { // If it has not changed, the codes were absorbed by black tape
			// clearTimer(T2);
			floorColor = Black;
		} else { // If it has changed, the codes were reflected, so it is seeing white
			clearTimer(T1);
			floorColor = White;
		}
		cached = recvdCodes;
		sleep(100);
	}
}

// Make a task with the job of constantly checking the IR sensor for recvd codes, and storing it
task recvTask()
{
	while (true)
	{
		int charr = getChar(UART1);
		if (charr != -1)
		{
			recvdCodes = recvdCodes + 1;
		}

		sleep(20); // Free up cycle time, so other tasks are able to run consistently
	}
}
// !SECTION Function/task definitions