#include <stdbool.h>

#include <fxp.h>

#include "bmp280.h"
#include "power_switch.h"
#include "measurement.h"
#include "charge_pump.h"
#include "rs485.h"
#include "flash_config.h"
#include "addon_io.h"

#include "charge_control.h"

static const char *CHARGE_STATE_TEXT[CHARGE_NUM_STATES] = {
	"WAIT_CHARGEPUMP",
	"INITIAL",
	"INITIAL_HOLD",
	"TRANSITION",
	"FLOAT",
	"SLEEP",
	"HIGH_INTERNAL_TEMPERATURE",
	"LOW_EXTERNAL_TEMPERATURE"
};

static const char *DISCHARGE_STATE_TEXT[DISCHARGE_NUM_STATES] = {
	"WAIT_CHARGEPUMP",
	"OK",
	"VOLTAGE_LOW",
	"OVERCURRENT"
};

static enum ChargeState charge_state;
static enum DischargeState discharge_state;

static bool charge_state_entered;
static bool discharge_state_entered;

static uint64_t charge_state_entered_timestamp;
static uint64_t discharge_state_entered_timestamp;

static fxp_t u_bat_regulation_corridor;

static fxp_t u_bat_initial_full;
static fxp_t u_bat_initial_low;

static fxp_t u_bat_initial_hold_cancel;

static fxp_t u_bat_float_full;
static fxp_t u_bat_float_low;

static fxp_t min_charge_pump_excess_voltage;

static fxp_t u_bat_load_on;
static fxp_t u_bat_load_off;

static fxp_t load_current_limit;

static fxp_t internal_temperature_limit;
static fxp_t internal_temperature_recovery;

static fxp_t external_temperature_limit;
static fxp_t external_temperature_recovery;

static fxp_t sleep_solar_current;
static fxp_t sleep_solar_excess_voltage;


static enum ChargeState control_solar_charging(
		fxp_t corridor_high,
		fxp_t corridor_low,
		uint64_t uptime_ms,
		struct MeasurementResult *meas,
		enum ChargeState current_state,
		uint64_t time_in_state)
{
	static uint64_t last_switch_change_time = 0;
	uint64_t solar_switch_onoff_duration = uptime_ms - last_switch_change_time;

	bool last_switch_state = power_switch_solar_status();

	if(meas->u_bat >= corridor_high) {
		power_switch_solar_off();
	} else if(meas->u_bat <= corridor_low) {
		power_switch_solar_on();
	}

	bool current_switch_state = power_switch_solar_status();

	if(last_switch_state != current_switch_state) { // switch changed
		last_switch_change_time = uptime_ms;
		solar_switch_onoff_duration = 0;
	}

	// internal temperature limit: prevent overheating of the power transistors.
	if(meas->avg_temperature > internal_temperature_limit) {
		return CHARGE_HIGH_INTERNAL_TEMPERATURE;
	}

	// external temperature limit: prevent charging the battery if temperature is too low.
	if(bmp280_are_measurements_valid() &&
			bmp280_get_temperature() < external_temperature_limit) {
		return CHARGE_LOW_EXTERNAL_TEMPERATURE;
	}

	// low-current limit (go to sleep at night)
	if((time_in_state > FLASH_CONFIG_SLEEP_STATE_DELAY)
			&& (current_switch_state == true)
			&& (solar_switch_onoff_duration > FLASH_CONFIG_SLEEP_SWITCH_DELAY)
			&& (meas->avg_i_solar < sleep_solar_current)) {
		return CHARGE_SLEEP;
	}

	return current_state;
}


static void solar_fsm_update(uint64_t uptime_ms, struct MeasurementResult *meas)
{
	uint64_t charge_time_in_state = uptime_ms - charge_state_entered_timestamp;

	switch(charge_state) {
		case CHARGE_WAIT_CHARGEPUMP:
			// force the solar switch off until the charge pump voltage reaches a safe level.
			if(charge_state_entered) {
				power_switch_solar_off();
			}

			// calculate charge pump output excess voltage over battery voltage
			// and compare to the threshold
			if(fxp_sub(meas->u_sw, meas->u_bat) > min_charge_pump_excess_voltage) {
				charge_state = CHARGE_INITIAL;
			}
			break;

		case CHARGE_INITIAL:
			charge_state = control_solar_charging(
					u_bat_initial_full,
					u_bat_initial_low,
					uptime_ms,
					meas,
					charge_state,
					charge_time_in_state);

			// switch to hold state when high threshold is reached
			if(meas->u_bat >= u_bat_initial_full) {
				charge_state = CHARGE_INITIAL_HOLD;
			}

			break;

		case CHARGE_INITIAL_HOLD:
			charge_state = control_solar_charging(
					u_bat_initial_full,
					u_bat_initial_low,
					uptime_ms,
					meas,
					charge_state,
					charge_time_in_state);

			// cancel charge hold if battery voltage is below threshold
			if(meas->u_bat <= u_bat_initial_hold_cancel) {
				charge_state = CHARGE_INITIAL;
			}

			// time limit for initial hold charging
			if(charge_time_in_state > FLASH_CONFIG_INITIAL_CHARGE_HOLD_TIME) {
				charge_state = CHARGE_TRANSITION;
			}

			break;

		case CHARGE_TRANSITION:
			if(charge_time_in_state < FLASH_CONFIG_INITIAL_TO_FLOAT_TRANSITION_TIME) {
				// dynamically adjust thresholds
				fxp_t u_bat_full =
					fxp_add(u_bat_initial_full,
						fxp_mult(
							fxp_sub(u_bat_float_full, u_bat_initial_full),
							fxp_div(charge_time_in_state, FLASH_CONFIG_INITIAL_TO_FLOAT_TRANSITION_TIME)));

				fxp_t u_bat_low = fxp_sub(u_bat_full, u_bat_regulation_corridor);

				charge_state = control_solar_charging(
						u_bat_full,
						u_bat_low,
						uptime_ms,
						meas,
						charge_state,
						charge_time_in_state);
			} else {
				// time limit for transition to float charging reached
				charge_state = CHARGE_FLOAT;
				break;
			}

			break;

		case CHARGE_FLOAT:
			charge_state = control_solar_charging(
					u_bat_float_full,
					u_bat_float_low,
					uptime_ms,
					meas,
					charge_state,
					charge_time_in_state);

			// temperature limit
			if(meas->temperature > internal_temperature_limit) {
				charge_state = CHARGE_HIGH_INTERNAL_TEMPERATURE;
				break;
			}
			break;

		case CHARGE_SLEEP:
			if(charge_state_entered) {
				power_switch_solar_off();
			}

			{
				fxp_t solar_excess_voltage = fxp_sub(meas->u_solar, meas->u_bat);

				if(solar_excess_voltage > sleep_solar_excess_voltage) {
					// resume operation
					charge_state = CHARGE_WAIT_CHARGEPUMP;
					break;
				}
			}
			break;


		case CHARGE_HIGH_INTERNAL_TEMPERATURE:
			if(charge_state_entered) {
				power_switch_solar_off();
			}

			if(meas->temperature < internal_temperature_recovery) {
				charge_state = CHARGE_WAIT_CHARGEPUMP;
				break;
			}
			break;


		case CHARGE_LOW_EXTERNAL_TEMPERATURE:
			if(charge_state_entered) {
				power_switch_solar_off();

				// switch on the heater via the isolated I/O addon board
				addon_io_iso_out_on(0);
			}

			// this state can only be entered if the BMP280 measurement is valid, so
			// no need to check it again here.
			if(bmp280_get_temperature() > external_temperature_recovery) {
				charge_state = CHARGE_WAIT_CHARGEPUMP;

				// switch the heater off again when this state is left
				addon_io_iso_out_off(0);
				break;
			}
			break;


		default:
			// unknown state
			break;
	}
}


static void load_fsm_update(uint64_t uptime_ms, struct MeasurementResult *meas)
{
	uint64_t discharge_time_in_state = uptime_ms - discharge_state_entered_timestamp;

	switch(discharge_state) {
		case DISCHARGE_WAIT_CHARGEPUMP:
			// force the load off until the charge pump voltage reaches a safe level.
			if(discharge_state_entered) {
				power_switch_load_off();
			}

			// calculate charge pump output excess voltage over battery voltage
			// and compare to the threshold
			if(fxp_sub(meas->u_sw, meas->u_bat) > min_charge_pump_excess_voltage) {
				discharge_state = DISCHARGE_OK;
			}
			break;

		case DISCHARGE_OK:
			// Battery voltage is in a safe range, so keep the load switched on
			if(discharge_state_entered) {
				power_switch_load_on();
			}

			if((meas->i_load > load_current_limit)
					&& (discharge_time_in_state > FLASH_CONFIG_LOAD_CURRENT_INRUSH_TIME)) {
				discharge_state = DISCHARGE_OVERCURRENT;
			}

			if(meas->avg_u_bat < u_bat_load_off) {
				discharge_state = DISCHARGE_VOLTAGE_LOW;
			}
			break;

		case DISCHARGE_VOLTAGE_LOW:
			// Battery voltage is too low, so keep the load switched off
			if(discharge_state_entered) {
				power_switch_load_off();
			}

			// Can only switch on again after a specific amount of time has passed
			if((meas->avg_u_bat > u_bat_load_on)
					&& (discharge_time_in_state > FLASH_CONFIG_LOAD_ON_DELAY)) {
				discharge_state = DISCHARGE_OK;
			}
			break;

		case DISCHARGE_OVERCURRENT:
			// Battery voltage is too low, so keep the load switched off
			if(discharge_state_entered) {
				power_switch_load_off();
			}

			// no way out except reset
			break;

		default:
			// unknown state
			break;
	}
}


void charge_control_init(void)
{
	charge_state    = CHARGE_WAIT_CHARGEPUMP;
	discharge_state = DISCHARGE_WAIT_CHARGEPUMP;

	charge_state_entered    = true;
	discharge_state_entered = true;

	/* calculate thresholds */
	u_bat_regulation_corridor = fxp_div(FXP_FROM_INT(FLASH_CONFIG_U_BAT_REGULATION_CORRIDOR),
	                                    FXP_FROM_INT(1000));

	u_bat_initial_full = fxp_div(FXP_FROM_INT(FLASH_CONFIG_U_BAT_INITIAL_FULL), FXP_FROM_INT(1000));
	u_bat_initial_low  = fxp_sub(u_bat_initial_full, u_bat_regulation_corridor);

	u_bat_initial_hold_cancel = fxp_div(FXP_FROM_INT(FLASH_CONFIG_U_BAT_INITIAL_HOLD_CANCEL), FXP_FROM_INT(1000));

	u_bat_float_full = fxp_div(FXP_FROM_INT(FLASH_CONFIG_U_BAT_FLOAT_FULL), FXP_FROM_INT(1000));
	u_bat_float_low  = fxp_sub(u_bat_float_full, u_bat_regulation_corridor);

	min_charge_pump_excess_voltage = fxp_div(FXP_FROM_INT(FLASH_CONFIG_MIN_CHARGE_PUMP_EXCESS_VOLTAGE),
	                                         FXP_FROM_INT(1000));

	u_bat_load_on  = fxp_div(FXP_FROM_INT(FLASH_CONFIG_U_BAT_LOAD_ON), FXP_FROM_INT(1000));
	u_bat_load_off = fxp_div(FXP_FROM_INT(FLASH_CONFIG_U_BAT_LOAD_OFF), FXP_FROM_INT(1000));

	load_current_limit = fxp_div(FXP_FROM_INT(FLASH_CONFIG_LOAD_CURRENT_LIMIT_MA), FXP_FROM_INT(1000));

	internal_temperature_limit = fxp_div(FXP_FROM_INT(FLASH_CONFIG_INTERNAL_TEMPERATURE_LIMIT), FXP_FROM_INT(10));
	internal_temperature_recovery = fxp_div(FXP_FROM_INT(FLASH_CONFIG_INTERNAL_TEMPERATURE_RECOVERY), FXP_FROM_INT(10));

	external_temperature_limit = fxp_div(FXP_FROM_INT(FLASH_CONFIG_EXTERNAL_TEMPERATURE_LIMIT), FXP_FROM_INT(10));
	external_temperature_recovery = fxp_div(FXP_FROM_INT(FLASH_CONFIG_EXTERNAL_TEMPERATURE_RECOVERY), FXP_FROM_INT(10));

	sleep_solar_current = fxp_div(FXP_FROM_INT(FLASH_CONFIG_SLEEP_SOLAR_CURRENT), FXP_FROM_INT(1000));
	sleep_solar_excess_voltage = fxp_div(FXP_FROM_INT(FLASH_CONFIG_SLEEP_SOLAR_EXCESS_VOLTAGE), FXP_FROM_INT(1000));
}


void charge_control_update(uint64_t uptime_ms, struct MeasurementResult *meas)
{
	/* state change tracking for efficient transistions. */
	enum ChargeState    last_charge_state    = charge_state;
	enum DischargeState last_discharge_state = discharge_state;

	if(charge_state_entered) {
		rs485_enqueue("STATE:CHARGE:");
		rs485_enqueue(CHARGE_STATE_TEXT[charge_state]);
		rs485_enqueue("\n");
		charge_state_entered_timestamp = uptime_ms;
	}

	if(discharge_state_entered) {
		rs485_enqueue("STATE:DISCHG:");
		rs485_enqueue(DISCHARGE_STATE_TEXT[discharge_state]);
		rs485_enqueue("\n");
		discharge_state_entered_timestamp = uptime_ms;
	}

	/* generalized charge pump control */
	if(charge_state_entered || discharge_state_entered) {
		if(charge_state == CHARGE_WAIT_CHARGEPUMP
				|| discharge_state == DISCHARGE_WAIT_CHARGEPUMP) {
			// either charge or discharge control is waiting for the charge
			// pump, so power it up!
			charge_pump_start();
		} else if(((charge_state == CHARGE_SLEEP) || charge_control_is_charge_blocked())
				&& ((discharge_state == DISCHARGE_VOLTAGE_LOW) || charge_control_is_discharge_blocked())) {
			// no power from the solar panel and the battery voltage is too
			// low, so both switches are off and we can safely stop the charge
			// pump
			charge_pump_stop();
		}
	}

	solar_fsm_update(uptime_ms, meas);
	load_fsm_update(uptime_ms, meas);

	charge_state_entered    = charge_state    != last_charge_state;
	discharge_state_entered = discharge_state != last_discharge_state;
}


bool charge_control_is_idle(void)
{
	return (((charge_state == CHARGE_SLEEP)
					|| (charge_state == CHARGE_HIGH_INTERNAL_TEMPERATURE)
					|| (charge_state == CHARGE_LOW_EXTERNAL_TEMPERATURE))
				&& ((discharge_state == DISCHARGE_VOLTAGE_LOW)
					|| (discharge_state == DISCHARGE_OVERCURRENT)));
}


bool charge_control_is_charge_blocked(void)
{
	switch(charge_state) {
		case CHARGE_HIGH_INTERNAL_TEMPERATURE:
		case CHARGE_LOW_EXTERNAL_TEMPERATURE:
		case CHARGE_WAIT_CHARGEPUMP:
			return true;

		default:
			return false;
	}
}


bool charge_control_is_discharge_blocked(void)
{
	switch(discharge_state) {
		case DISCHARGE_OVERCURRENT:
		case DISCHARGE_WAIT_CHARGEPUMP:
			return true;

		default:
			return false;
	}
}