#include "main.h"
#include "board.h"
#include "commands.h"
#include "usb.h"

static void GPIO_Init();
static void HandleSerialByte(uint8_t);

int main(void)
{
	GlobalInterruptEnable();
	SetupHardware();

	/* Create a regular character stream for the interface so that it can be used with the stdio.h functions */
	CDC_Device_CreateStream(&VirtualSerial_CDC_Interface, &USBSerialStream);

	for (;;)
	{
		CheckInputs();
		
		CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
		USB_USBTask();

		/* Must read data from the host, or it will lock up while waiting for the device */
		int16_t data = CDC_Device_ReceiveByte(&VirtualSerial_CDC_Interface);
		if (data >= 0) {
			HandleSerialByte((uint8_t)(data & 0xFF));
		}

		CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
		USB_USBTask();
	}
}

/** Configures the board hardware and chip peripherals for the demo's functionality. */
void SetupHardware(void)
{
	/* Disable watchdog if enabled by bootloader/fuses */
	MCUSR &= ~(1 << WDRF);
	wdt_disable();

	/* Disable clock division */
	clock_prescale_set(clock_div_1);

	/* Hardware Initialization */
	GPIO_Init();
	USB_Init();
}

void GPIO_Init() {
	// Port B:
	// 0, 1, 2: Inputs Disconnect, Sel B, Sel A -> Pullup
	// 7: SYS_LED
	DDRB = 1<<7;
	PINB |= (1 << 0) | (1 << 1) | (1 << 2);
	// Port C:
	// 2, 4, 5, 6, 7: C_SENSE, VBUS_CTRL_DET, HS, ~USBON, VBUS_DUT_DET
	// DDRC = 0;
	// Port D:
	// 0, 1: IND_PORTA, EN_POW_CTRL
	// 2, 3: IND_PORTB, EN_POW_DUT
	// 4: ~OE
	// 5: ~CTRL/DUT
	// 6: UNRST
	// 7: 5VMCU
	DDRD = ~((1<<7) | (1<<6));
}

static int gotCommand = 0;
void CheckInputs(void)
{
	uint8_t portValue = (~PINB) & 7;
	static int lV = 0;
	if ((portValue == 0 || portValue == lV) && !gotCommand) {
		return;
	}

	lV = portValue;
	gotCommand = 0;

	if (portValue & (1<<0)) {
		disconnect();
	} else if (portValue & (1<<1)) {
		connectDUT();
	} else if (portValue & (1<<2)) {
		connectCTL();
	}
}

static const struct {
	char const *name;
	void (*fun)(void);
} commands[] = {
	{ "C", connectCTL },
	{ "PC", connectPowerCTL },
	{ "DC", connectDataCTL },
	{ "D", connectDUT },
	{ "PD", connectPowerDUT },
	{ "DD", connectDataDUT },
	{ "dPC", disconnectPowerCTL },
	{ "dPD", disconnectPowerDUT },
	{ "dP", disconnectPower },
	{ "dD", disconnectData },
	{ "d", disconnect },
	{ "I", setID },
	{ "i", clearID },
	{ "s", status },
	{ "x", debug }
};

static void HandleCommand(char *cmd, int cmdLen) {
	for (int i = 0; i < (sizeof commands) / (sizeof commands[0]); i++) {
		if (!strncmp(commands[i].name, cmd, cmdLen)) {
			gotCommand = 1;
			commands[i].fun();
			return;
		}
	}

	fputs("Unknown command\r\n", &USBSerialStream);
}

static void HandleSerialByte(uint8_t b) {
	static char cmd[64];
	static int cmdLen = 0;

	if (b != '\r' && b != '\n') {
		if (cmdLen < sizeof cmd - 1) {
			cmd[cmdLen++] = b;
			fputc(b, &USBSerialStream);
		}
	} else {
		if (cmdLen > 0) {
			fputs("\r\n", &USBSerialStream);
			CDC_Device_Flush(&VirtualSerial_CDC_Interface);
			cmd[cmdLen] = '\0';
			HandleCommand(cmd, cmdLen);
			cmdLen = 0; 
		}
	}
}