/* main.c */

#include "sensors.h"
#include "i2c.h"
#include "timer.h"
#include "uart.h"
#include "interrupt.h"
#include "event.h"
#include "led.h"
#include "status.h"
#include "watchdog.h"
#include "thrust.h"
#include "panic.h"
#include "sdcard.h"
#include "log.h"
#include "spi.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))

#define BUTTON_PRESSED (!((FP0XVAL) & 0x00010000))

char buffer[512];

void init_pins(void)
{
	PINSEL0 = 0x2a09a255; /* P0.0 and P0.1 assigned to UART */
			      /* P0.2 and P0.3 assigned to I2C  */
			      /* P0.4 and P0.6 assigned to CAP0.[12] */
	                      /* P0.7 and P0.9 assigned to MAT2.[02] */
			      /* P0.8 assigned to UART1 */
			      /* P0.12 and P0.13 assigned to MAT1.[01] */
			      /* P0.14 assigned to SPI1 */

	PINSEL1 = 0x00000140; /* P0.19 and P0.20 assigned to SPI1 */

	SCS = 1;
	FP0XDIR = 0x04200000; /* P0.26 and P0.21 are outputs */
	FP0XVAL = 0x0;
}

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

void timer_event_handler(void)
{
	unsigned int timestamp = timer_read();

	log_put_header(timestamp);
	sensors_start_sample();
}

void menu_handler(void);

void wait_for_button_pressed(bool target)
{
	bool pressed;

	led_set(!target);

	/* Very crude debouncing */
	timer_delay_ms(100);

	target = target ? TRUE:FALSE;

	do {
		pressed = BUTTON_PRESSED;
	} while (pressed != target);
	led_set(pressed);
}

void calibrate_escs()
{
	wait_for_button_pressed(0);

	putstr("Calibration mode\r\n");

	wait_for_button_pressed(1);
	wait_for_button_pressed(0);

	set_thrust(0, 1.0);
	set_thrust(1, 1.0);
	set_thrust(2, 1.0);
	set_thrust(3, 1.0);
	putstr("Max throttle set\r\n");

	wait_for_button_pressed(1);
	wait_for_button_pressed(0);

	set_thrust(0, 0.0);
	set_thrust(1, 0.0);
	set_thrust(2, 0.0);
	set_thrust(3, 0.0);
	putstr("Zero throttle set\r\n");

	wait_for_button_pressed(1);
	wait_for_button_pressed(0);

	putstr("Exit calibration mode\r\n");
}

#ifdef USE_UART
void dump_buffer(char *buffer, unsigned int length, unsigned int addr);
void dump_buffer(char *buffer, unsigned int length, unsigned int addr)
{
	unsigned int i;

	for (i = 0; i < length; i++) {
		if ((i % 16) == 0) {
			puthex(addr+i);
			putstr(":");
		}
		putstr(" ");
		puthex(buffer[i]);
		if ((i % 16) == 15) {
			putstr("\r\n");
		}
	}
	if ((i % 16) != 0)
		putstr("\r\n");
}
#else
#define dump_buffer(a, b, c) ((void)0)
#endif

int main(void) {
	int i;

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

	event_register(EVENT_UART_INPUT, menu_handler);

	event_register(EVENT_TIMER, timer_event_handler);

	for (i = 0; i < 10; i++) {
		if (init_sdcard())
			break;
	}

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

	if (BUTTON_PRESSED)
		calibrate_escs();

	putstr("prompt> ");

	timer_delay_ms(1000);
	if (!sensors_init())
		putstr("Sensor initialisation failed\r\n");

	/* Flight is potentially live after this. */
	timer_set_period(TIMER_MS(5));
#if 1
	sensors_start_zero();
#endif

	led_init();

	init_watchdog();

	/* Good luck! */
	while (1) {
		CHECKPOINT(0);
		led_update();
		CHECKPOINT(1);
		if (!event_dispatch())
			sdcard_poll();
		CHECKPOINT(2);
		watchdog_check();
	}

	return 0;
}

static int power = 0;
static unsigned int sd_address = 0;

unsigned int read_number(void)
{
	unsigned int number;
	unsigned int base;
	int digit;
	char c;

	number = 0;
	base = 10;

	while (1) {
		if (getch(&c)) {
			digit = -1;
			if ((c == 0x0a) || (c == 0x0d))
				break;
			putch(c);
			if (c == 'x')
				base = 16;
			if ((c >= '0') && (c <= '9'))
				digit = c - '0';
			if ((c >= 'A') && (c <= 'F'))
				digit = c - 'A' + 10;

			if ((digit >= 0) && (digit < (int)base)) {
				number = number * base;
				number += digit;
			}
		}
	}
	putstr("\r\n");
	return number;
}

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

	while (getch(&c)) {
#if 0
		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 'H':
		case '?':
			reply("Help is not available. Try a psychiatrist.");
			break;
		case 'D':
			sensors_dump();
			break;
		case 'N':
			putstr("The time is ");
			puthex(timer_read());
			reply(".");
			break;
		case 'I':
			init_sdcard();
			break;
		case 'R':
			sd_address = 0;
		case 'S':
			if (sdcard_read(sd_address++, buffer, 512)) {
				dump_buffer(buffer, 512, (sd_address-1) * 512);
				reply ("SD card read success");
			} else {
				reply("SD card read failed");
			}
			break;
		case 'A':
			sd_address = read_number();
			putstr("SD read address set to 0x");
			puthex(sd_address);
			reply(".");
			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;
#if 0
		case 'T':
			sdcard_start_write();
			break;
#endif
		case 'L':
			spi_drain();
			break;
		case '0' & 0xdf:
			set_thrust(0, 0.0);
			set_thrust(1, 0.0);
			set_thrust(2, 0.0);
			set_thrust(3, 0.0);
			power = 0;
			break;
#if 0
		case '1' & 0xdf:
			power--;
			if (power < 0)
				power = 15;
			power = power % 16;
			putstr("Power setting: ");
			putint(power);
			putstr("\r\n");
			set_thrust(0, ((float)power)/16.0);
			break;
		case '2' & 0xdf:
			power++;
			power = power % 16;
			putstr("Power setting: ");
			putint(power);
			putstr("\r\n");
			set_thrust(0, ((float)power)/16.0);
			break;
#endif
#if 0
		case '1' & 0xdf:
			set_thrust(0, 0.5);
			break;
		case '2' & 0xdf:
			set_thrust(1, 0.5);
			break;
		case '3' & 0xdf:
			set_thrust(2, 0.5);
			break;
		case '4' & 0xdf:
			set_thrust(3, 0.5);
			break;
		case '5' & 0xdf:
			set_thrust(0, 1.0);
			break;
		case '6' & 0xdf:
			set_thrust(1, 1.0);
			break;
		case '7' & 0xdf:
			set_thrust(2, 1.0);
			break;
		case '8' & 0xdf:
			set_thrust(3, 1.0);
			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> ");
	}
}