#include "common.h"
#include "bus/uart.h"
#include "bus/mosfet.h"
#include "bus/pwm.h"
#include "bus/i2c.h"

#include <avr/interrupt.h>

#define DEADBAND      2.0f
#define HYSTERESIS_C  0.5f
#define HYSTERESIS_H  0.5f

// https://en.wikipedia.org/wiki/Bang%E2%80%93bang_control
// https://support.75f.io/hc/en-us/articles/360044956794-Deadband-and-Hysteresis
// https://www.elotech.de/fileadmin/user_upload/PID-Regelungsgrundlagen.pdf
// https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PID.html
// https://www.ni.com/en/shop/labview/pid-theory-explained.html
// https://en.wikipedia.org/wiki/Proportional%E2%80%93integral%E2%80%93derivative_controller
// https://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlPID
// https://www.youtube.com/watch?v=wkfEZmsQqiA&list=PLn8PRpmsu08pQBgjxYFXSsODEF3Jqmm-y

enum state_e
{
	S_IDLE,
	S_HEAT_UP,
	S_COOL_DOWN,
	S_DEHUMIDIFY
};

static enum state_e state;
static float temp, temp_target;
static float dewp, dewp_target;
static float rhum;

static int   Init(void);
static void  Update(void);
static void  SetTarget(float t, float td);
static void  FetchSensorValues(void);

static int Init(void)
{
	mem_block_t mem;

	state = S_IDLE;

	// MOSFETS control things like the heating element
	// so they are the highest priority to initialize
	// to a default state via MOS_Init().

	MOS_Init();

	// The serial interface is required for output
	// functions like Info() and Error() and it uses
	// IRQs, so we need to initialize it as soon as
	// possible and make sure to enable interrupts.

	UART_Init();
	sei();

	Info("Initializing...");

	// The watchdog timer is clocked from a separate
	// on-chip oscillator which runs at 1 MHz. Eight
	// different clock cycle periods can be selected
	// to determine the reset period. If the reset
	// period expires, the chip resets and executes
	// from the reset vector.

	// The update loop must call WDT_Reset to reset
	// the timer, kind of like a dead man's switch
	// allowing us to detect infinite loops and any
	// other error that halts execution.

	if (WDT_HasTriggered()) {
		Info("Unexpected system reset.");
	}

	WDT_Enable();
	WDT_SetTimeoutFlag(WDT2000); // 2 seconds

	// See if there are persistent target settings
	// stored in EEPROM and load them. Some sanity
	// checking must be done before using those.

	if (MEM_Read(&mem)) { // Any valid block found?
		Info("Found persistent configuration in EEPROM!");
		Info("Using targets TEMP=%.2fC, DEWP=%.2fC.",
			mem.temp, mem.dewp);

		temp_target = mem.temp;
		dewp_target = mem.dewp;
	}

	// mem.temp = 20.50f;
	// mem.dewp = 10.25f;
	// MEM_Write(&mem);
	// MEM_Dump();
	// MEM_Free();

	// There is a possiblity to use interrupt signals
	// for I2C communication but only as one large
	// branching routine for the whole I2C system.
	// The blocking approach used right now is fine.

	I2C_Init();
	PWM_Init();

	MOS_Enable(MOS03);      // Lights
	// MOS_Enable(MOS01);   // Peltier
	// MOS_Disable(MOS02);  // Heating

	PWM_SetValue(FAN01, 50);  // Fan Peltier Hot side
	PWM_SetValue(FAN02, 50);  // Fan Peltier Cold Side
	PWM_SetValue(FAN03, 50);  // Fan Heating

	// The I2C_SetChannel command changes the channel
	// setting of the PCA9546 I2C multiplexer. Any
	// command after it will be sent to the device
	// listening on that channel.

	I2C_SetChannel(AHT01);
	I2C_AHT20_Init();

	I2C_SetChannel(AHT02);
	I2C_AHT20_Init();

	I2C_SetChannel(AHT03);
	I2C_AHT20_Init();

	return 0;
}

static void Update(void)
{
	char ch;
	cmd_t cmd;
	float terr;
	float derr;

	// Parse serial commands
	while ((ch = UART_Getc()) >= 0) {
		if (!CMD_Parse(ch, &cmd)) {
			continue;
		}
		switch(cmd.type) {
		case T_SET: // Set new persistent target
			SetTarget(cmd.args[0], cmd.args[1]);
			return;
		}
	}

	// Get latest sensor values
	FetchSensorValues();

	// Two-step (bang-bang) controller testing

	// TODO: Sanity check setpoint!
	// TODO: Check thermistor for overheating!
	// TODO: Implement control stages based on hysteresis
	// TODO: PID control for fan pulse-width modulation?

	// The dead band represents the lower and upper limits
	// of the error between which the controller doesn't
	// react.

	state = S_IDLE;
	terr = temp - temp_target;
	derr = dewp - dewp_target;

	if (terr < -DEADBAND / 2) {
		state = S_HEAT_UP;
	}

	if (terr > DEADBAND / 2) {
		state = S_COOL_DOWN;
	}

	switch (state) {
	case S_IDLE:
		MOS_Disable(MOS01);
		MOS_Disable(MOS02);
		PWM_SetValue(FAN01, 20);
		PWM_SetValue(FAN02, 20);
		PWM_SetValue(FAN03, 20);
		break;
	case S_HEAT_UP:
		MOS_Enable(MOS02);
		MOS_Disable(MOS01);
		PWM_SetValue(FAN01, 20);
		PWM_SetValue(FAN02, 60);
		PWM_SetValue(FAN03, 60);
		break;
	case S_COOL_DOWN:
		MOS_Enable(MOS01);
		MOS_Disable(MOS02);
		PWM_SetValue(FAN01, 60);
		PWM_SetValue(FAN02, 60);
		PWM_SetValue(FAN03, 60);
		break;
	case S_DEHUMIDIFY:
		UNUSED(derr);
		break;
	}
}

static void SetTarget(float t, float td)
{
	mem_block_t mem;

	Print("\r\n");
	Info("Updating target configuration:");
	Info("Setting temperature to %.2fC.", t);
	Info("Setting dewpoint to %.2fC.", td);

	// Even with wear leveling there is a finite number
	// of EEPROM write cycles so we should always check
	// for redundant values. This could be handled by
	// the underlying memory implementation.

	if (MEM_Read(&mem)) {
		if (t == mem.temp && td == mem.dewp) {
			return; // Nothing to do
		}
	}

	// Update current targets
	mem.temp = temp_target = t;
	mem.dewp = dewp_target = td;

	// Store in EEPROM
	MEM_Write(&mem);
}

static void FetchSensorValues(void)
{
	word raw;
	float t[6], rh[3], td[3];

	Print("\r\n");
	Info("Fetching sensor values...");

	I2C_SetChannel(AHT01);
	I2C_AHT20_Read(&t[0], &rh[0]);
	I2C_SetChannel(AHT02);
	I2C_AHT20_Read(&t[1], &rh[1]);
	I2C_SetChannel(AHT03);
	I2C_AHT20_Read(&t[2], &rh[2]);

	raw = I2C_ADS1115_ReadRaw(ADS01);
	t[3] = SteinhartHart(Resistance(raw));
	raw = I2C_ADS1115_ReadRaw(ADS02);
	t[4] = SteinhartHart(Resistance(raw));
	raw = I2C_ADS1115_ReadRaw(ADS03);
	t[5] = SteinhartHart(Resistance(raw));

	td[0] = Dewpoint(t[0], rh[0]);
	td[1] = Dewpoint(t[1], rh[1]);
	td[2] = Dewpoint(t[2], rh[2]);

	temp = (t[0] + t[1] + t[2]) / 3;
	rhum = (rh[0] + rh[1] + rh[2]) / 3;
	dewp = (td[0] + td[1] + td[2]) / 3;

	Info("T1=%.2fC, TD1=%.2fC, RH1=%.2f%%, NT1=%.2fC", t[0], td[0], rh[0], t[3]);
	Info("T2=%.2fC, TD2=%.2fC, RH2=%.2f%%, NT2=%.2fC", t[1], td[1], rh[1], t[4]);
	Info("T3=%.2fC, TD3=%.2fC, RH3=%.2f%%, NT3=%.2fC", t[2], td[2], rh[2], t[5]);
	Info("T_AVG=%.2fC, TD_AVG=%.2fC, RH_AVG=%.2f%%", temp, dewp, rhum);
	Info("T_TAR=%.2fC, TD_TAR=%.2fC", temp_target, dewp_target);
	Info("STATE=%s", (state == S_IDLE)        ? "S_IDLE"        :
	                 (state == S_HEAT_UP)     ? "S_HEAT_UP"     :
	                 (state == S_COOL_DOWN)   ? "S_COOL_DOWN"   :
	                 (state == S_DEHUMIDIFY)  ? "S_DEHUMIDIFY"  :
	                 "UNKNOWN");
}

int main(void)
{
	Init();

	for (;;) {
		WDT_Reset();
		Update();
		WDT_Reset();
		Sleep(1000);
	}

	return 0;
}