/* main.c */

#include "wmp.h"
#include "i2c.h"
#include "timer.h"
#include "uart.h"
#include "interrupt.h"
#include "event.h"
#include "stick.h"

#define PINSEL0 (*((volatile unsigned int *) 0xE002C000))
#define PINSEL1 (*((volatile unsigned int *) 0xE002C004))
#define FP0XDIR (*((volatile unsigned int *) 0x3FFFC000))
#define FP0XVAL (*((volatile unsigned int *) 0x3FFFC014))

#define SCS (*((volatile unsigned int *) 0xe01fc1a0))


void init_pins(void)
{
	PINSEL0 = 0x00a88055; /* P0.0 and P0.1 assigned to UART */
			      /* P0.2 and P0.3 assigned to I2C  */
	                      /* P0.7 and P0.9 assigned to MAT2.[02] */
	                      /* P0.10 and P0.11 assigned to CAP1.[01] */
	PINSEL1 = 0x20000828; /* P0.21 and P0.30 assigned to MAT3.[03] */
	                      /* P0.17 and P0.18 assigned to CAP1.[23] */
	SCS = 1;
	FP0XDIR = 0x04000000; /* P0.26 is an output */
	FP0XVAL = 0x0;
}

#ifdef USE_UART
void reply(char *str)
{
	putstr(str);
	putstr("\r\n");
}
#else
#define reply(x) ((void)0)
#endif

unsigned int count = 0;

void minmax_sample(void)
{
	int count;
	unsigned int fast_roll_min, fast_roll_max;
	unsigned int fast_pitch_min, fast_pitch_max;
	unsigned int fast_yaw_min, fast_yaw_max;
	unsigned int slow_roll_min, slow_roll_max;
	unsigned int slow_pitch_min, slow_pitch_max;
	unsigned int slow_yaw_min, slow_yaw_max;

	putstr("Sampling min/max values\r\n");
	if (!wmp_sample()) {
		putstr("\r\nRead error\r\n");
		return;
	}

	fast_roll_min = fast_roll_max = wmp_roll;
	fast_pitch_min = fast_pitch_max = wmp_pitch;
	fast_yaw_min = fast_yaw_max = wmp_yaw;

	slow_roll_min = slow_roll_max = wmp_roll;
	slow_pitch_min = slow_pitch_max = wmp_pitch;
	slow_yaw_min = slow_yaw_max = wmp_yaw;

	count = 0;

	while (1) {
		if (!wmp_sample()) {
			putstr("\r\nRead error\r\n");
			return;
		}
		if (wmp_roll_fast) {
			if (wmp_roll < fast_roll_min)
				fast_roll_min = wmp_roll;
			if (wmp_roll > fast_roll_max)
				fast_roll_max = wmp_roll;
		} else {
			if (wmp_roll < slow_roll_min)
				slow_roll_min = wmp_roll;
			if (wmp_roll > slow_roll_max)
				slow_roll_max = wmp_roll;
		}
		if (wmp_pitch_fast) {
			if (wmp_pitch < fast_pitch_min)
				fast_pitch_min = wmp_pitch;
			if (wmp_pitch > fast_pitch_max)
				fast_pitch_max = wmp_pitch;
		} else {
			if (wmp_pitch < slow_pitch_min)
				slow_pitch_min = wmp_pitch;
			if (wmp_pitch > slow_pitch_max)
				slow_pitch_max = wmp_pitch;
		}
		if (wmp_yaw_fast) {
			if (wmp_yaw < fast_yaw_min)
				fast_yaw_min = wmp_yaw;
			if (wmp_yaw > fast_yaw_max)
				fast_yaw_max = wmp_yaw;
		} else {
			if (wmp_yaw < slow_yaw_min)
				slow_yaw_min = wmp_yaw;
			if (wmp_yaw > slow_yaw_max)
				slow_yaw_max = wmp_yaw;
		}
		count++;
		if (count > 1000) {
			putstr("(");
			puthex(slow_roll_min);
			putstr(", ");
			puthex(slow_pitch_min);
			putstr(", ");
			puthex(slow_yaw_min);
			putstr(") (");
			puthex(slow_roll_max);
			putstr(", ");
			puthex(slow_pitch_max);
			putstr(", ");
			puthex(slow_yaw_max);
			putstr(") (");
			puthex(fast_roll_min);
			putstr(", ");
			puthex(fast_pitch_min);
			putstr(", ");
			puthex(fast_yaw_min);
			putstr(") (");
			puthex(fast_roll_max);
			putstr(", ");
			puthex(fast_pitch_max);
			putstr(", ");
			puthex(fast_yaw_max);
			putstr(")                   \r");
			count = 0;
		}
		timer_delay_ms(2);
	}
}

void average_sample(void)
{
	int i;
	int roll_total;
	int pitch_total;
	int yaw_total;

	putstr("Sampling average values\r\n");

	roll_total = 0;
	pitch_total = 0;
	yaw_total = 0;

	for (i = 0; i < 0x1000; i++) {
		if (!wmp_sample()) {
			putstr("\r\nRead error\r\n");
			return;
		}
		roll_total += wmp_roll;
		pitch_total += wmp_pitch;
		yaw_total += wmp_yaw;
		timer_delay_ms(2);
	}
	putstr("(");
	puthex(roll_total);
	putstr(", ");
	puthex(pitch_total);
	putstr(", ");
	puthex(yaw_total);
	putstr(")\r\n");
}

void timer_event_handler(void)
{
	wmp_start_sample();
}

void menu_handler(void);

int main(void) {
	armed = FALSE;

	init_interrupt();
	init_uart();
	init_i2c();
	init_pins();
	init_timer();

	event_register(EVENT_UART_INPUT, menu_handler);

	event_register(EVENT_I2C_COMPLETE, wmp_event_handler);

	event_register(EVENT_TIMER, timer_event_handler);

	putstr("Your entire life has been a mathematical error... a mathematical error I'm about to correct!\r\n");

	putstr("prompt> ");

	FP0XVAL &= ~0x04000000;
	timer_delay_ms(1000);
	FP0XVAL |= 0x04000000;
	timer_delay_ms(1000);
	FP0XVAL &= ~0x04000000;
	if (!wmp_init())
		putstr("WMP initialisation failed\r\n");

	/* Flight is potentially live after this. */
	timer_set_period(5*TIMER_MS);
	wmp_start_zero();

	FP0XVAL |= 0x04000000;

	/* Good luck! */
	while (1) {
		FP0XVAL ^= 0x04000000;
		event_dispatch();
	}

	return 0;
}

void menu_handler(void)
{
	int i;
	char c;

	while (getch(&c)) {
#if 1
		continue; /* Yes, let's just ignore UART input now. */
#endif
		if (c == 0x0a)
			continue;
		putch(c);
		putstr("\r\n");
		switch (c & 0xdf) {
		case 0x0a:
		case 0x0d:
			break;
		case 'A':
			reply("apple");
			break;
		case 'C':
			count++;
			putstr("The current count is ");
			putint(count);
			reply(".");
			break;
		case 'H':
		case '?':
			reply("Help is not available. Try a psychiatrist.");
			break;
		case 'T':
			putstr(" I2C status is: ");
			puthex(i2c_statreg());
			reply(".");
			putstr("I2C register is: ");
			puthex(i2c_conreg());
			reply(".");
			break;
		case 'I':
			putstr("Initialising WMP... ");
			if (wmp_init())
				reply("done");
			else
				reply("FAIL");
			break;
		case 'M':
			putstr("Reading from WMP... ");
			if (wmp_sample()) {
				putstr("(");
				puthex(wmp_roll);
				putstr(", ");
				puthex(wmp_pitch);
				putstr(", ");
				puthex(wmp_yaw);
				reply(").");
			} else
				reply("FAIL");
			break;
		case 'L':
			minmax_sample();
			break;
		case 'V':
			average_sample();
			break;
		case 'D':
			putstr("Reading calibration data... ");
			if (wmp_read_calibration_data()) {
				putstr("\r\n");
				for (i = 0; i < 0x10 ; i++) {
					puthex(wmp_calibration_data[i]);
					putstr(" ");
				}
				putstr("\r\n");
				for (i = 0x10; i < 0x20 ; i++) {
					puthex(wmp_calibration_data[i]);
					putstr(" ");
				}
				putstr("\r\n");
			} else {
				reply("FAIL");
			}
			break;
		case 'N':
			putstr("The time is ");
			puthex(timer_read());
			reply(".");
			break;
		case 'P':
			putstr("Initialising timer... ");
			/* We want a 100Hz loop but two samples per iteration.
			 * So, we go for 200Hz. */
			timer_set_period(5*TIMER_MS);
			reply("done");
			wmp_start_zero();
			break;
		case 'W':
			for (i = 0; i < 4; i++) {
				putstr("Width ");
				putint(i);
				putstr(": ");
				putint(timer_input(i));
				if (!timer_valid(i))
					putstr(" (invalid)");
				putstr("\r\n");
			}
			if (!timer_allvalid()) {
				putstr("ALL INVALID!\r\n");
			}
			break;
		case '0' & 0xdf:
			timer_set_pwm_value(0, 0);
			timer_set_pwm_value(1, 0);
			timer_set_pwm_value(2, 0);
			timer_set_pwm_value(3, 0);
			break;
#if 0
		case '1' & 0xdf:
			timer_set_pwm_value(0, PWM_MAX/2);
			break;
		case '2' & 0xdf:
			timer_set_pwm_value(1, PWM_MAX/2);
			break;
		case '3' & 0xdf:
			timer_set_pwm_value(2, PWM_MAX/2);
			break;
		case '4' & 0xdf:
			timer_set_pwm_value(3, PWM_MAX/2);
			break;
		case '5' & 0xdf:
			timer_set_pwm_value(0, PWM_MAX);
			break;
		case '6' & 0xdf:
			timer_set_pwm_value(1, PWM_MAX);
			break;
		case '7' & 0xdf:
			timer_set_pwm_value(2, PWM_MAX);
			break;
		case '8' & 0xdf:
			timer_set_pwm_value(3, PWM_MAX);
			break;
#endif
		case '9' & 0xdf:
			timer_set_pwm_invalid(0);
			timer_set_pwm_invalid(1);
			timer_set_pwm_invalid(2);
			timer_set_pwm_invalid(3);
			break;
		default:
			reply("Unrecognised command.");
			break;
		}
		putstr("prompt> ");
	}
}