#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"
#include "rs485.h"
#include "charge_pump.h"
#include "charge_control.h"
#include "power_switch.h"
#include "measurement.h"

volatile int wait_frame = 1;


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 TIM1 for PWM generation
	rcc_periph_clock_enable(RCC_TIM1);

	// enable GPIO clocks:
	// Port A is needed for the charge pump, extension port and analog input
	rcc_periph_clock_enable(RCC_GPIOA);

	// Port B is needed for the LEDs
	rcc_periph_clock_enable(RCC_GPIOB);

	// USART1 is used for RS485
	rcc_periph_clock_enable(RCC_USART1);

	// ADC1 for analog measuremnts
	rcc_periph_clock_enable(RCC_ADC1);

	// DMA1 is used for ADC data transfer
	rcc_periph_clock_enable(RCC_DMA1);
}


/* 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;
}


static void update_leds(uint64_t uptime_ms, struct MeasurementResult *meas_data)
{
	static fxp_t charge_in_mAs = 0;
	static fxp_t charge_out_mAs = 0;

	static uint64_t charge_pulse_until = 0;
	static uint64_t discharge_pulse_until = 0;

	charge_in_mAs = fxp_add(charge_in_mAs, meas_data->i_solar);
	charge_out_mAs = fxp_add(charge_out_mAs, meas_data->i_load);

	if(charge_in_mAs > FXP_FROM_INT(1000)) {
		led_chplex_on(LED_CHPLEX_IDX_CHARGE_PULSE);
		charge_pulse_until = uptime_ms + 12;

		charge_in_mAs = fxp_sub(charge_in_mAs, FXP_FROM_INT(1000));
	} else if(uptime_ms > charge_pulse_until) {
		led_chplex_off(LED_CHPLEX_IDX_CHARGE_PULSE);
	}

	if(charge_out_mAs > FXP_FROM_INT(1000)) {
		led_chplex_on(LED_CHPLEX_IDX_DISCHARGE_PULSE);
		discharge_pulse_until = uptime_ms + 12;

		charge_out_mAs = fxp_sub(charge_out_mAs, FXP_FROM_INT(1000));
	} else if(uptime_ms > discharge_pulse_until) {
		led_chplex_off(LED_CHPLEX_IDX_DISCHARGE_PULSE);
	}

	if(charge_control_is_charge_blocked()) {
		led_chplex_on(LED_CHPLEX_IDX_ERR_TEMP);
	} else {
		led_chplex_off(LED_CHPLEX_IDX_ERR_TEMP);
	}

	if(charge_control_is_discharge_blocked()) {
		led_chplex_on(LED_CHPLEX_IDX_ERR_LOAD);
	} else {
		led_chplex_off(LED_CHPLEX_IDX_ERR_LOAD);
	}

	if(power_switch_solar_status()) {
		led_chplex_on(LED_CHPLEX_IDX_SOLAR_ON);
	} else {
		led_chplex_off(LED_CHPLEX_IDX_SOLAR_ON);
	}

	if(power_switch_load_status()) {
		led_chplex_on(LED_CHPLEX_IDX_LOAD_ON);
	} else {
		led_chplex_off(LED_CHPLEX_IDX_LOAD_ON);
	}
}


static void report_status(struct MeasurementResult *meas_data)
{
	char number[FXP_STR_MAXLEN];

	rs485_enqueue("MEAS:");

	fxp_format(meas_data->u_bat, number, 3);
	rs485_enqueue(number);
	rs485_enqueue(":");

	fxp_format(meas_data->u_solar, number, 3);
	rs485_enqueue(number);
	rs485_enqueue(":");

	fxp_format(meas_data->u_sw, number, 3);
	rs485_enqueue(number);
	rs485_enqueue(":");

	fxp_format(meas_data->i_solar, number, 3);
	rs485_enqueue(number);
	rs485_enqueue(":");

	fxp_format(meas_data->i_load, number, 3);
	rs485_enqueue(number);
	rs485_enqueue(":");

	fxp_format(meas_data->temperature, number, 2);
	rs485_enqueue(number);
	rs485_enqueue("\n");
}


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

	struct MeasurementResult meas_data;

	init_clock();

	rs485_init();
	charge_pump_init();
	power_switch_init();
	measurement_init();
	charge_control_init();

	led_chplex_init();
	led_chplex_on(LED_CHPLEX_IDX_SOLAR_ON);

	init_systick(1000);

	rs485_enqueue("LNSC-2420 v" VERSION " initialized.\n");

	// triggered every 1 ms
	while (1) {

		if(!ledtest_done) {
			ledtest_done = ledtest(timebase_ms);
			led_chplex_periodic();
		} else if(!startup_done) {
			charge_pump_start();

			startup_done = true;
		} else {
			measurement_start();

			// measurement takes some time, so do other things before waiting for
			// completion. This is a good place for tasks that are not critical in
			// latency, such as updating the LEDs, sending the state over RS485 etc.

			update_leds(timebase_ms, &meas_data);
			led_chplex_periodic();

			// Send the status data from the last cycle.
			if(timebase_ms % 500 == 0) {
				report_status(&meas_data);
			}

			measurement_wait_for_completion();
			measurement_finalize(&meas_data);

			// Update the charge controller immediately after the measurement.
			// This ensures fast reaction time to overcurrent/overvoltage.
			charge_control_update(timebase_ms, &meas_data);
		}

		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);
}