#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/adc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/dma.h>
#include <libopencm3/stm32/timer.h>
#include <libopencm3/stm32/rtc.h>
#include <libopencm3/stm32/pwr.h>
#include <libopencm3/stm32/exti.h>
#include <libopencm3/cm3/nvic.h>
#include <libopencm3/cm3/systick.h>

#include <libopencmsis/core_cm3.h>

#include <fxp.h>
#include <fxp_basic.h>

#include "led_chplex.h"

volatile int wait_frame = 1;


static void init_gpio(void)
{
	// Set up UART TX on PB6 for debugging
	/*gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO6);
	gpio_set_af(GPIOB, GPIO_AF0, GPIO6);*/

}


static void init_clock(void)
{
	/* Set STM32 to 48 MHz. */
	rcc_clock_setup_in_hse_8mhz_out_48mhz();    // generate 48 MHz from external 8 MHz crystal
	//rcc_clock_setup_in_hsi_out_48mhz();         // generate ~48 MHz from internal RC oscillator

	// enable GPIO clocks:
	// Port B is needed for the LEDs
	rcc_periph_clock_enable(RCC_GPIOB);
}


/* Set up systick to fire freq times per second */
static void init_systick(int freq)
{
	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB);
	/* clear counter so it starts right away */
	STK_CVR = 0;

	systick_set_reload(rcc_ahb_frequency / freq);
	systick_counter_enable();
	systick_interrupt_enable();
}


static bool ledtest(uint64_t timebase_ms)
{
	if(timebase_ms == 0) {
		led_chplex_mask(0x3F); // all on
	} else if(timebase_ms == 1000) {
		led_chplex_mask(0x01);
	} else if(timebase_ms == 1200) {
		led_chplex_mask(0x02);
	} else if(timebase_ms == 1400) {
		led_chplex_mask(0x04);
	} else if(timebase_ms == 1600) {
		led_chplex_mask(0x08);
	} else if(timebase_ms == 1800) {
		led_chplex_mask(0x10);
	} else if(timebase_ms == 2000) {
		led_chplex_mask(0x20);
	} else if(timebase_ms == 2200) {
		led_chplex_mask(0x10);
	} else if(timebase_ms == 2400) {
		led_chplex_mask(0x08);
	} else if(timebase_ms == 2600) {
		led_chplex_mask(0x04);
	} else if(timebase_ms == 2800) {
		led_chplex_mask(0x02);
	} else if(timebase_ms == 3000) {
		led_chplex_mask(0x01);
	} else if(timebase_ms == 3200) {
		led_chplex_mask(0x00);
		return true;
	}

	return false;
}


int main(void)
{
	//uint32_t cpuload = 0;
	uint64_t timebase_ms = 0;
	bool ledtest_done = false;

	init_clock();
	init_gpio();

	led_chplex_init();
	led_chplex_on(LED_CHPLEX_IDX_SOLAR_ON);

	init_systick(1000);

	// triggered every 1 ms
	while (1) {

		if(!ledtest_done) {
			ledtest_done = ledtest(timebase_ms);
		} else if(timebase_ms % 500 == 0) {
			led_chplex_toggle(LED_CHPLEX_IDX_DISCHARGE_PULSE);
		}


		led_chplex_periodic();

		timebase_ms++;

		while(wait_frame) {
			__WFI();
		}

		wait_frame = 1;
	}

	return 0;
}


/* Called when systick fires */
void sys_tick_handler(void)
{
	wait_frame = 0;
}


void hard_fault_handler(void)
{
	while (1);
}