/*
 * temperatureRegulation.c
 *
 *  Created on: Apr 16, 2023
 *      Author: Flax
 *
 *      Prefix : REG
 */

// ==============
// Local includes
// ==============
#include "temperatureRegulation.h"
#include "register.h"

// ===============
// Local constants
// ===============
#define cREGTempSPTMax				(400U)
#define cREGTempSPTMin				(50U)
#define cREGTempSPTDft				(250U)
#define cREGTempSPTStbyDft			(100U)

#define cREGPwmMax					(1023)

#define cREGErrorTempThres			(50)
#define cREGErrorTimeout			(5000)

#define cRegPIDProp					(10)
#define cRegPIDInteg				(5)
#define cRegPIDDeriv				(5)

// Temperature measure transfer function parameters
// OPA gain : 681
// Input divider bridge gain : 100k / (100k + 5k6) = 0.946969
// On legacy Arduino code : gain = 0.39, offset = 23.9, resolution 10 bits
// Translated to 12 bits : 0.0975 ~= 100 / 1024
#define ADC_TO_TEMP_GAIN_NUM		(100U)
#define ADC_TO_TEMP_GAIN_DEN		(1024U)
#define ADC_TO_TEMP_OFFSET			(24U)

#define cREGAdcErrorThres			(4090U)

#define cREGAdcToTempFilter			(50U)

// ===============
// Local variables
// ===============
static bool RegOffState_B;
static bool RegStandbyState_B;
static bool RegErrorState_B;

static uint16_t RegTempSPT_U16;
static uint16_t RegTempSPTStby_U16;
static uint16_t RegTempMeas_U16[2U];
static uint16_t RegTempMeasBuffer_U16A[2U];
static int32_t  RegPwmDyc_S32;
static uint32_t RegPwmDyc_U32;
static uint16_t RegTempDeg_U16;
static uint16_t RegTempDegFilt_U16;
static uint16_t RegTempFiltCnt_U16;
static uint32_t RegErrorTimeoutCnt_U32;

// PID
static int32_t RegEpsilon_S32A[2U];


// ==========================
// Local functions prototypes
// ==========================

// ===========================
// Local functions definitions
// ===========================

// ============================
// Shared functions definitions
// ============================

void REGInit (void)
{
	RegOffState_B = true;
	RegStandbyState_B = false;

	// TODO : load NVM values
	RegTempSPT_U16 = cREGTempSPTDft;
	RegTempSPTStby_U16 = cREGTempSPTStbyDft;
	RegErrorTimeoutCnt_U32 = cREGErrorTimeout;
}

void REGStart (void)
{
	mREDAdcEnable();
	LL_TIM_CC_EnableChannel(TIM2, LL_TIM_CHANNEL_CH1);
	LL_TIM_EnableCounter(TIM2);
}

void REGStop (void)
{
	mREGAdcStopConversion();
}

void REGCyclicTask (void)
{
	if (mREGAdcIsReady())
	{
		// Trigger conversion
		mREGAdcStartConversion();

		// Read ADC input
		RegTempMeasBuffer_U16A[1] = mREGAdcReadValue();

		if (RegTempMeasBuffer_U16A[1] >= cREGAdcErrorThres)
		{
			// Error
			RegErrorState_B = true;
		}
	}
	else
	{
		// ADC not ready
		RegTempMeasBuffer_U16A[1] = 0xFFFF;
	}

	if (RegTempFiltCnt_U16 < cREGAdcToTempFilter)
		{
			// RegTempFiltCnt_U16++;
		}
		else
		{
	// Calculate average
	RegTempMeasBuffer_U16A[0] += RegTempMeasBuffer_U16A[1];
	RegTempMeasBuffer_U16A[0] = RegTempMeasBuffer_U16A[0] >> 1U;
	RegTempMeas_U16[1] = RegTempMeas_U16[0]; // Memorise previous value
	RegTempMeas_U16[0] = RegTempMeasBuffer_U16A[0];
		}

	// Convert to degrees
	RegTempDeg_U16 = (uint16_t)(((((uint32_t)ADC_TO_TEMP_GAIN_NUM * (uint32_t)(RegTempMeas_U16[0])) / (uint32_t)ADC_TO_TEMP_GAIN_DEN) + (uint32_t)ADC_TO_TEMP_OFFSET) & (uint32_t)0x0000FFFF);

	// PID
	RegEpsilon_S32A[1] = RegEpsilon_S32A[0]; // Memorise previous value
	RegEpsilon_S32A[0] = RegTempSPT_U16 - RegTempDeg_U16;

	// Check error - Overheat
	if (RegEpsilon_S32A[0] > cREGErrorTempThres)
	{
		if (RegErrorTimeoutCnt_U32 > 0)
		{
			RegErrorTimeoutCnt_U32--;
		}
		else
		{
			// Error - Too much temperature error for too long
			RegErrorState_B = true;
		}
	}
	else
	{
		RegErrorTimeoutCnt_U32 = cREGErrorTimeout;
	}

	// Process PID output
	RegPwmDyc_S32 = (RegEpsilon_S32A[0] * cRegPIDProp)
					+ ((RegEpsilon_S32A[0] - RegEpsilon_S32A[1]) * cRegPIDDeriv)
					+ ((RegEpsilon_S32A[0] + RegEpsilon_S32A[1]) * cRegPIDInteg);

	// Check regulation state before processing the output PWM
	// if ((RegErrorState_B == false) && (RegOffState_B == false))
	if ((RegOffState_B == false))
	{
		if (RegPwmDyc_S32 > (int32_t)cREGPwmMax)
		{
			RegPwmDyc_U32 = (uint32_t)cREGPwmMax;
		}
		else if (RegPwmDyc_S32 > 0)
		{// Only keep positive values
			RegPwmDyc_U32 = RegPwmDyc_S32;
		}
		else
		{
			RegPwmDyc_U32 = 0U;
		}
	}
	else
	{ // Shut the PWM down
		RegPwmDyc_U32 = 0U;
	}

	// Set PWM output duty cycle
	// Output is reversed
	mREGPwmSetCompareValue(cREGPwmMax - RegPwmDyc_U32);

	// Filter physical temperature - for cleaner display
	if (RegTempFiltCnt_U16 < cREGAdcToTempFilter)
	{
		RegTempFiltCnt_U16++;
	}
	else
	{ // Sub-sampling for cleaner display
		RegTempFiltCnt_U16 = 0U;
		RegTempDegFilt_U16 = RegTempDeg_U16;
	}
}

bool REGIsError (void)
{
	return RegErrorState_B;
}

bool REGIsStandby(void)
{
	return RegStandbyState_B;
}

bool REGIsOff(void)
{
	return RegOffState_B;
}

void REGErrorReset (void)
{
	RegOffState_B = false;
}

void REGStandbySet(void)
{
	RegStandbyState_B = true;
}

void REGStandbyReset(void)
{
	RegStandbyState_B = false;
}

void REGOffSet(void)
{
	RegOffState_B = true;
}

void REGOffReset(void)
{
	RegOffState_B = false;
}

bool REGSPTSet (uint16_t temp_spt_u16)
{
	bool ret_B = false;
	if ((temp_spt_u16 <= (uint16_t)cREGTempSPTMax) && (temp_spt_u16 >= (uint16_t)cREGTempSPTMin))
	{
		RegTempSPT_U16 = temp_spt_u16;
		ret_B = true;
	}
	return (ret_B);
}

uint16_t REGSPTGet (void)
{
	return RegTempSPT_U16;
}

uint16_t REGTempGet (void)
{
	return RegTempDegFilt_U16;
}

bool REGSPTStbySet (uint16_t temp_spt_u16)
{
	bool ret_B = false;
	if ((temp_spt_u16 <= (uint16_t)cREGTempSPTMax) && (temp_spt_u16 >= (uint16_t)cREGTempSPTMin))
	{
		RegTempSPTStby_U16 = temp_spt_u16;
		ret_B = true;
	}
	return (ret_B);
}

uint16_t REGSPTStbyGet (void)
{
	return RegTempSPTStby_U16;
}

uint16_t REGTempPhysDeg (void)
{
	return (RegTempDeg_U16);
}