#include <libopencm3/stm32/rcc.h>
#include <libopencm3/cm3/systick.h>

#include <libopencmsis/core_cm3.h>

#include <string.h>

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

#include "clock.h"
#include "led_chplex.h"
#include "rs485.h"
#include "charge_pump.h"
#include "charge_control.h"
#include "power_switch.h"
#include "measurement.h"
#include "deepsleep.h"
#include "bmp280.h"
#include "addon_io.h"

#include "pinout.h"
#include "flash_config.h"

volatile int wait_frame = 1;

static enum {
	BMP280_NOT_PRESENT,
	BMP280_IDLE,
	BMP280_MEASURING,
} bmp280_state;

/* 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 void config_err_blink_code(uint64_t timebase_ms)
{
	if(timebase_ms % 500 < 250) {
		led_chplex_mask(0x3F); // all on
	} else {
		led_chplex_mask(0x00); // all off
	}
}


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->avg_i_solar);
	charge_out_mAs = fxp_add(charge_out_mAs, meas_data->avg_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(":");

	rs485_enqueue(charge_control_is_charge_blocked() ? "1" : "0");
	rs485_enqueue(":");

	rs485_enqueue(charge_control_is_discharge_blocked() ? "1" : "0");

	if(bmp280_state != BMP280_NOT_PRESENT
			&& bmp280_are_measurements_valid()) {
		rs485_enqueue(":");
		fxp_format(bmp280_get_temperature(), number, 2);
		rs485_enqueue(number);
		rs485_enqueue(":");

		fxp_format(bmp280_get_pressure(), number, 2);
		rs485_enqueue(number);
	}

	rs485_enqueue("\n");
}


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

	rs485_enqueue("AVGD:");

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

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

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

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

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

	fxp_format(meas_data->avg_temperature, number, 2);
	rs485_enqueue(number);
	rs485_enqueue(":");

	rs485_enqueue(charge_control_is_charge_blocked() ? "1" : "0");
	rs485_enqueue(":");

	rs485_enqueue(charge_control_is_discharge_blocked() ? "1" : "0");

	if(bmp280_state != BMP280_NOT_PRESENT
			&& bmp280_are_measurements_valid()) {
		rs485_enqueue(":");

		fxp_format(bmp280_get_temperature(), number, 2);
		rs485_enqueue(number);
		rs485_enqueue(":");

		fxp_format(bmp280_get_pressure(), number, 2);
		rs485_enqueue(number);
	}

	rs485_enqueue("\n");
}


static void low_power_mode(uint32_t duration_sec)
{
	// stop the systick counter for reliable deep sleep
	systick_counter_disable();

	// prepare the individual modules for sleep
	rs485_deepsleep_prepare();
	led_chplex_deepsleep_prepare();

	// enter deep sleep for the given duration
	deepsleep(duration_sec);

	// resume the modules
	led_chplex_deepsleep_resume();
	rs485_deepsleep_resume();

	systick_counter_enable();
	rs485_enqueue("PWR:DEEPSLEEP:EXIT\n");
}


static void handle_bmp280(uint64_t timebase_ms)
{
	switch(bmp280_state) {
		case BMP280_NOT_PRESENT:
			// do nothing
			break;

		case BMP280_IDLE:
			if(timebase_ms % 1000 == 137) {
				bmp280_start_measurement();
				bmp280_state = BMP280_MEASURING;
			}
			break;

		case BMP280_MEASURING:
			if(timebase_ms % 10 == 0 && bmp280_loop()) {
				bmp280_state = BMP280_IDLE;
			}
			break;
	}
}


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

	bool charge_control_was_idle = false;
	uint64_t charge_control_idle_since = 0;

	struct MeasurementResult meas_data;

	memset(&meas_data, 0, sizeof(meas_data));

	init_clock();
	init_rtc();

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

	led_chplex_init();
	led_chplex_on(LED_CHPLEX_IDX_SOLAR_ON);

	if(bmp280_init()) {
		bmp280_state = BMP280_IDLE;
	} else {
		bmp280_state = BMP280_NOT_PRESENT;
	}

	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) {
			if(flash_config_is_present()) {
				charge_pump_start();

				startup_done = true;
			} else {
				config_err_blink_code(timebase_ms);
				led_chplex_periodic();
			}
		} 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);
			}

			// Send the averaged measurement data from the last cycle.
			if(timebase_ms % 500 == 20) {
				report_averaged(&meas_data);
			}

			// update BMP280
			handle_bmp280(timebase_ms);

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

			// deep sleep control
			if(bmp280_state != BMP280_MEASURING) { // general blockers
				if(charge_control_is_idle()) {
					if(!charge_control_was_idle) {
						charge_control_was_idle = true;
						charge_control_idle_since = timebase_ms;
					} else {
						// charge control already idle
						if((timebase_ms - charge_control_idle_since) > FLASH_CONFIG_DEEPSLEEP_DELAY) {
							low_power_mode(FLASH_CONFIG_DEEPSLEEP_DURATION);
							charge_control_was_idle = false;

							// correct the time base after deep sleep
							timebase_ms += FLASH_CONFIG_DEEPSLEEP_DURATION * 1000;
						}
					}
				} else {
					charge_control_was_idle = false;
				}
			}
		}

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