#include <clocks/clocks/clocks.h>
#include <drivers/timer/timer.h>
#include <drivers/adc/adc.h>
#include <drivers/uart/uart.h>
#include <drivers/sdcard/sdcard.h>
#include <app/keyboard/keyboard.h>
#include <app/lcd/lcd.h>
#include <app/path/path.h>
#include <app/tool/tool.h>
#include "pins.h"

extern void keyboardClick(enum KB_CLICKTYPE clickType, uint8_t key);
extern void joystickButton(struct PIO_irq* const module);
extern void endstopActive(struct PIO_irq* const module);
extern void SDCard_disconnect(struct PIO_irq* const module);
extern void screen_showCoord(struct TIMER_module* timer);
extern void joystick_readValues(struct TIMER_module* timer);

struct TIMER_module timer_ms;
struct TIMER_module timer_coord;
struct TIMER_module timer_joy;
struct ADC_module joyX;
struct ADC_module joyY;
struct PIO_irq joySW;
struct PIO_irq endstop[6];
struct PIO_irq SDCardDetect;
struct UART_module USB;
struct UART_module RS232;


static void InitClocks()
{
	OSC_mainck_crystal_enable(62);
	OSC_mainck_select(true);
	OSC_mainck_rc_disable();
	OSC_plla_enable(1, 24, 64);
	PMC_master_clock_configure(PMC_OSC_PLLA, PMC_PRES_1, PMC_DIV_3);
}

static void InitPins()
{
	/* Keyboard */
	PIO_output_disable(PIN_KB_C0);
	PIO_pull(PIN_KB_C0, PIO_PULLUP);
	PIO_output_disable(PIN_KB_C1);
	PIO_pull(PIN_KB_C1, PIO_PULLUP);
	PIO_output_disable(PIN_KB_C2);
	PIO_pull(PIN_KB_C2, PIO_PULLUP);
	PIO_output_disable(PIN_KB_C3);
	PIO_pull(PIN_KB_C3, PIO_PULLUP);

	PIO_output_enable(PIN_KB_R0);
	PIO_setpin_low(PIN_KB_R0);
	PIO_output_enable(PIN_KB_R1);
	PIO_setpin_low(PIN_KB_R1);
	PIO_output_enable(PIN_KB_R2);
	PIO_setpin_low(PIN_KB_R2);
	PIO_output_enable(PIN_KB_R3);
	PIO_setpin_low(PIN_KB_R3);

	/* USB */
	PIO_peripheral_configure(PIN_USB_RXD, PIO_PER_C);
	PIO_peripheral_configure(PIN_USB_TXD, PIO_PER_C);

	/* RS232 */
	PIO_peripheral_configure(PIN_RS232_RXD, PIO_PER_C);
	PIO_peripheral_configure(PIN_RS232_TXD, PIO_PER_C);

	/* LCD */
	PIO_peripheral_configure(PIN_LCD_CLK, PIO_PER_B);
	PIO_peripheral_configure(PIN_LCD_READ, PIO_PER_B);
	PIO_peripheral_configure(PIN_LCD_WRITE, PIO_PER_B);

	/* Joystick */
	PIO_pull(PIN_JOYX, PIO_PULLOFF);
	PIO_pull(PIN_JOYY, PIO_PULLOFF);
	PIO_output_disable(PIN_JOY_SW);
	PIO_pull(PIN_JOY_SW, PIO_PULLUP);
	PIO_filter_debouncer_enable(PIN_JOY_SW, 5);

	/* Endstops */
	PIO_output_disable(PIN_ESTOP_X1);
	PIO_pull(PIN_ESTOP_X1, PIO_PULLUP);
	PIO_filter_debouncer_enable(PIN_ESTOP_X1, 5);
	PIO_output_disable(PIN_ESTOP_X2);
	PIO_pull(PIN_ESTOP_X2, PIO_PULLUP);
	PIO_filter_debouncer_enable(PIN_ESTOP_X2, 5);
	PIO_output_disable(PIN_ESTOP_Y1);
	PIO_pull(PIN_ESTOP_Y1, PIO_PULLUP);
	PIO_filter_debouncer_enable(PIN_ESTOP_Y1, 5);
	PIO_output_disable(PIN_ESTOP_Y2);
	PIO_pull(PIN_ESTOP_Y2, PIO_PULLUP);
	PIO_filter_debouncer_enable(PIN_ESTOP_Y2, 5);
	PIO_output_disable(PIN_ESTOP_Z1);
	PIO_pull(PIN_ESTOP_Z1, PIO_PULLUP);
	PIO_filter_debouncer_enable(PIN_ESTOP_Z1, 5);
	PIO_output_disable(PIN_ESTOP_Z2);
	PIO_pull(PIN_ESTOP_Z2, PIO_PULLUP);
	PIO_filter_debouncer_enable(PIN_ESTOP_Z2, 5);

	/* Driver boards */
	PIO_output_enable(PIN_DRV_DIR1);
	PIO_setpin_low(PIN_DRV_DIR1);
	PIO_output_enable(PIN_DRV_DIR2);
	PIO_setpin_low(PIN_DRV_DIR2);
	PIO_output_enable(PIN_DRV_DIR3);
	PIO_setpin_low(PIN_DRV_DIR3);
	PIO_output_enable(PIN_DRV_DIR4);
	PIO_setpin_low(PIN_DRV_DIR4);

	PIO_output_enable(PIN_DRV_STP1);
	PIO_setpin_low(PIN_DRV_STP1);
	PIO_output_enable(PIN_DRV_STP2);
	PIO_setpin_low(PIN_DRV_STP2);
	PIO_output_enable(PIN_DRV_STP3);
	PIO_setpin_low(PIN_DRV_STP3);
	PIO_output_enable(PIN_DRV_STP4);
	PIO_setpin_low(PIN_DRV_STP4);
	PIO_peripheral_configure(PIN_DRV_STP1, PIO_PER_B);
	PIO_peripheral_configure(PIN_DRV_STP2, PIO_PER_B);
	PIO_peripheral_configure(PIN_DRV_STP3, PIO_PER_B);
	PIO_peripheral_configure(PIN_DRV_STP4, PIO_PER_B);

	PIO_output_disable(PIN_DRV_ALERT1);
	PIO_pull(PIN_DRV_ALERT1, PIO_PULLDOWN);
	PIO_output_disable(PIN_DRV_ALERT2);
	PIO_pull(PIN_DRV_ALERT2, PIO_PULLDOWN);
	PIO_output_disable(PIN_DRV_ALERT3);
	PIO_pull(PIN_DRV_ALERT3, PIO_PULLDOWN);
	PIO_output_disable(PIN_DRV_ALERT4);
	PIO_pull(PIN_DRV_ALERT4, PIO_PULLDOWN);
	PIO_output_disable(PIN_DRV_ALERT5);
	PIO_pull(PIN_DRV_ALERT5, PIO_PULLDOWN);

	PIO_output_enable(PIN_DRV_ENA);
	PIO_setpin_low(PIN_DRV_ENA);

	/* SD card */
	PIO_pull(PIN_SD_DATA0, PIO_PULLOFF);
	PIO_pull(PIN_SD_DATA1, PIO_PULLOFF);
	PIO_pull(PIN_SD_DATA2, PIO_PULLOFF);
	PIO_pull(PIN_SD_DATA3, PIO_PULLOFF);
	PIO_pull(PIN_SD_CMD, PIO_PULLOFF);
	PIO_pull(PIN_SD_CLK, PIO_PULLOFF);
	PIO_peripheral_configure(PIN_SD_DATA0, PIO_PER_C);
	PIO_peripheral_configure(PIN_SD_DATA1, PIO_PER_C);
	PIO_peripheral_configure(PIN_SD_DATA2, PIO_PER_C);
	PIO_peripheral_configure(PIN_SD_DATA3, PIO_PER_C);
	PIO_peripheral_configure(PIN_SD_CMD, PIO_PER_C);
	PIO_peripheral_configure(PIN_SD_CLK, PIO_PER_D);
	PIO_output_disable(PIN_SD_DETECT);
	PIO_filter_debouncer_enable(PIN_SD_DETECT, 5);
}

static void SetIrqPriority()
{
	//NVIC_SetPriority(SPI0_IRQn, 0);
	//NVIC_SetPriority(TC11_IRQn, 0);
	NVIC_SetPriority(TC10_IRQn, 1);
	NVIC_SetPriority(TC9_IRQn, 1);
	NVIC_SetPriority(AFEC0_IRQn, 2);
	NVIC_SetPriority(AFEC1_IRQn, 2);
	NVIC_SetPriority(PIOA_IRQn, 3);
	NVIC_SetPriority(PIOB_IRQn, 3);
}

static void InitModules()
{
	/* Slow timer */
	TIMER_init(&timer_ms, TC0, TIMER_CHANNEL_0, TIMER_CLK_SLCK, 200000);
	TIMER_start(&timer_ms, TIMER_MODE_SINGLE);
	while(TIMER_isBusy(&timer_ms));

	/* Show coordination timer */
	TIMER_init(&timer_coord, TC0, TIMER_CHANNEL_1, TIMER_CLK_SLCK, 100000);
	TIMER_registerCallback(&timer_coord, screen_showCoord);

	/* Joystick timer */
	TIMER_init(&timer_joy, TC0, TIMER_CHANNEL_2, TIMER_CLK_SLCK, 50000);
	TIMER_registerCallback(&timer_joy, joystick_readValues);

	/* Applications */
	LCD_init();
	KEYBOARD_init(keyboardClick);
	PATH_init();
	TOOL_init();

	/* Other drivers */
	ADC_init(&joyX, AFEC1);
	ADC_setFrequency(&joyX, 4000000);
	ADC_setSignMode(&joyX, ADC_SIGN_3);
	ADC_enable_channel(&joyX, ADC_CHANNEL_6, NULL);
	ADC_init(&joyY, AFEC0);
	ADC_setFrequency(&joyY, 4000000);
	ADC_setSignMode(&joyY, ADC_SIGN_3);
	ADC_enable_channel(&joyY, ADC_CHANNEL_8, NULL);
	PIO_interrupt_init(&joySW, PIN_JOY_SW, PIO_IRQ_RISING_EDGE, joystickButton);
	PIO_interrupt_enable(&joySW);

	PIO_interrupt_init(&endstop[0], PIN_ESTOP_X1, PIO_IRQ_FALLING_EDGE, endstopActive);
	PIO_interrupt_init(&endstop[1], PIN_ESTOP_X2, PIO_IRQ_FALLING_EDGE, endstopActive);
	PIO_interrupt_init(&endstop[2], PIN_ESTOP_Y1, PIO_IRQ_FALLING_EDGE, endstopActive);
	PIO_interrupt_init(&endstop[3], PIN_ESTOP_Y2, PIO_IRQ_FALLING_EDGE, endstopActive);
	PIO_interrupt_init(&endstop[4], PIN_ESTOP_Z1, PIO_IRQ_FALLING_EDGE, endstopActive);
	PIO_interrupt_init(&endstop[5], PIN_ESTOP_Z2, PIO_IRQ_FALLING_EDGE, endstopActive);
	for(uint8_t i = 0; i < 6; ++i) {
		PIO_interrupt_enable(&endstop[i]);
	}

	PIO_interrupt_init(&SDCardDetect, PIN_SD_DETECT, PIO_IRQ_RISING_EDGE, SDCard_disconnect);
	PIO_interrupt_enable(&SDCardDetect);
	SDCARD_init(30);

	PMC_peripheral_clock_enable(ID_PIOA);
	NVIC_EnableIRQ(ID_PIOA);
	PMC_peripheral_clock_enable(ID_PIOB);
	NVIC_EnableIRQ(ID_PIOB);
	PMC_peripheral_clock_enable(ID_PIOC);
	NVIC_EnableIRQ(ID_PIOC);
	PMC_peripheral_clock_enable(ID_PIOD);
	NVIC_EnableIRQ(ID_PIOD);
	PMC_peripheral_clock_enable(ID_PIOE);
	NVIC_EnableIRQ(ID_PIOE);
}

void StartupInit()
{
	InitClocks();
	InitPins();
	SetIrqPriority();
	InitModules();
}