/**
 * @file ad5934_driver.c
 * @brief Implementation of AD5934 I2C impedance converter driver.
 *
 * This driver provides functions to configure and read from the AD5934
 * 16-bit, 200kHz I2C impedance converter. The device supports frequency sweep
 * measurements and provides magnitude and phase data.
 *
 * @note Based on AD5934 datasheet – Rev. E – October 2017
 */

#include "ad5934_driver.h"

volatile AD5934_State_t ad5934_state = AD5934_IDLE;
volatile uint32_t state_timer = 0;
volatile uint32_t g_ms_counter = 0; /* Incrementado a cada 1ms no Timer */

uint8_t current_mux_channel = 0;
uint8_t current_hw_mux_connection = AD5934_CH_REF100R_LOW_GAIN; //  Connection
uint8_t current_ec_mux = AD5934_CH_EC_MID_GAIN;
uint8_t current_rtd_mux = AD5934_CH_RTD_LOW_GAIN;
uint8_t current_ref_mux = AD5934_CH_REF100R_LOW_GAIN;

uint8_t sweep_count = 0; /* Conta quantos sweeps foram feitos no canal atual (0 a 7) */

AD5934_filter_t g_rtd_filter = {0};
AD5934_filter_t g_ec_filter  = {0};
AD5934_filter_t g_ref_100R_LowGain_filter = {0};
AD5934_filter_t g_ref_100R_MidGain_filter = {0};
AD5934_filter_t g_ref_1K_MidGain_filter = {0};
AD5934_filter_t g_ref_1K_HighGain_filter = {0};
AD5934_filter_t g_ref_10K_HighGain_filter = {0};

int16_t rtd_now, ref_now, ec_now;
int16_t rtd_now_i, ref_now_i, ec_now_i;

uint8_t is_new_channel = 1;

uint8_t is_startup=1;

/******************************************************************************
* @brief Set an AD5934 internal register value.
*
* @param registerAddress - Address of AD5934 register. LB Byte Address when more than 1 byte
*
* @param registerValue - Value of data to be written in the register.
*
* @param numberOfBytes - Number of bytes to be written in the register
*
* @return None.
******************************************************************************/
void AD5934_SetRegisterValue(uint8_t registerAddress, uint32_t registerValue, uint8_t numberOfBytes)
{
	uint8_t writeData[2] = {0, 0};
	HAL_StatusTypeDef status;
	union Bytes2Long AD5934_reg;

	AD5934_reg.number = registerValue;

	for (uint8_t i = 0; i < numberOfBytes; i++)
	{
		writeData[0] = registerAddress - i;
		writeData[1] = AD5934_reg.bytes[i];
		status = HAL_I2C_Master_Transmit(&hi2c2, AD5934_I2C_ADDRESS, writeData, 2, 5);
	}

	return;

}

/******************************************************************************
* @brief Read an AD5934 internal register value.
*
* @param registerAddress - Address of AD5933 register.
*
* @param numberOfBytes - Number of bytes to be read from the register.
*
* @return Register value.
******************************************************************************/
uint32_t AD5934_GetRegisterValue(uint8_t registerAddress, uint8_t numberOfBytes)
{
    uint8_t readData[1]   = {0};
    uint8_t writeData[2]; //= {AD5934_ADDR_POINTER, registerAddress};
    union Bytes2Long AD5934_reg;
    HAL_StatusTypeDef status;

    if(numberOfBytes > 4)
    	return 0xFFFFFFFF; //Overflow

    AD5934_reg.number = 0;
    for (uint8_t i = 0; i < numberOfBytes; i++)
    {
    	writeData[0] = AD5934_ADDR_POINTER;
    	writeData[1] = registerAddress - i;
    	status = HAL_I2C_Master_Transmit(&hi2c2, AD5934_I2C_ADDRESS, writeData, 2, 5);

    	//HAL_Delay(1);
    	readData[0] = 0;
    	status = HAL_I2C_Master_Receive(&hi2c2, AD5934_I2C_ADDRESS, readData, 1, 5);
    	AD5934_reg.bytes[i]=readData[0];

    }

    return AD5934_reg.number;
}


/************************************************************************************
 * @brief Lê um registrador do AD5934 usando a abstração Mem_Read (mais segura).
 *
 * @param regAddr Endereço do registrador no AD5934.
 * @return uint32_t  Valor lido do registrador.
 ***********************************************************************************/
uint32_t AD5934_ReadRegister(uint8_t regAddr, uint8_t numberOfBytes)
{
	uint32_t registerValue = 0;
	uint8_t readData[4] = {0,0,0,0}; // Buffer para receber 16 bits (AD5934 usa regs de 16-bit)
	uint8_t writeData[2] = {AD5934_ADDR_POINTER,regAddr};
	//HAL_StatusTypeDef status;

    //Limit buffer size to prevent stack overflow (AD5934 regs are max 4 bytes) */
    if (numberOfBytes > 4)
    {
    	return 0xFFFFFFFF;
    }

    HAL_I2C_Master_Transmit(&hi2c2, AD5934_I2C_ADDRESS, writeData, 2, 5);

	//HAL_Delay(1);

    // Usamos Mem_Read que faz: START -> ADDR+W -> REG_ADDR -> REPEATED_START -> ADDR+R -> DATA -> STOP
    HAL_I2C_Mem_Read(&hi2c2, AD5934_I2C_ADDRESS, AD5934_BLOCK_READ, I2C_MEMADD_SIZE_8BIT, readData, numberOfBytes, 5);

    /* 5. Reconstruct value from Big-Endian buffer */
    for (uint8_t i = 0; i < numberOfBytes; i++)
    {
    	registerValue = (registerValue << 8) | readData[i];
    }

    return registerValue;

}


/******************************************************************************
* @brief Configure and Start the AD5934 frequency sweep parameters.
*
* @param None.
*
* @return None.
******************************************************************************/
void AD5934_Init(void)
{

	ADG715_ResetChannels(); // Disconnects Analog Switch on the CE / RTD / Ref Resistor

	/************ Config Sweep******************/

	// Place AD5934 in reset
	AD5934_SetRegisterValue(AD5934_CONTROL_REG_LB, (AD5934_CONTROL_FUNCTION(AD5934_RESET)), 1);

	// Configure starting frequency
	AD5934_SetRegisterValue(AD5934_START_FREQ_REG_LB, AD5934_FREQ_2K293HZ, 3);

	// Configure frequency increment step
	AD5934_SetRegisterValue(AD5934_FREQ_INCR_REG_LB, AD5934_FREQ_0HZ, 3);

	// Configure number of steps
	AD5934_SetRegisterValue(AD5934_NR_INCR_REG_LB, AD5934_STEP_FREQ_0, 2);

	// Set 128 Settling Time
	AD5934_SetRegisterValue(AD5934_NR_SETTLE_REG_LB, AD5934_SETTLING_TIME_90,  2);

}

/******************************************************************************
* @brief Re-Start the AD5934 frequency sweep parameters.
*
* @param None.
*
* @return None.
******************************************************************************/
void AD5934_RestartSweep(void)
{
	// Place AD5934 in standby
	AD5934_SetRegisterValue(AD5934_CONTROL_REG_HB, (AD5934_CONTROL_FUNCTION(AD5934_STANDBY) | AD5934_CONTROL_RANGE(AD5934_400mVpp_RANGE) |  AD5934_PGA_GAIN(AD5934_PGA_GAIN_X1)), 1);

	// Initialize starting frequency, Start frequency sweep, standby
	AD5934_SetRegisterValue(AD5934_CONTROL_REG_HB, ((AD5934_CONTROL_FUNCTION(AD5934_INIT_START_FREQ)) | AD5934_CONTROL_RANGE(AD5934_400mVpp_RANGE) |  AD5934_PGA_GAIN(AD5934_PGA_GAIN_X1)), 1);

	// Disable Internal Reset State
	AD5934_SetRegisterValue(AD5934_CONTROL_REG_LB, AD5934_CONTROL_FUNCTION(AD5934_RESERVED), 1);

	// Configure Range Output, PGA gain andPlace AD5934 in sweep
	AD5934_SetRegisterValue(AD5934_CONTROL_REG_HB, (AD5934_CONTROL_FUNCTION(AD5934_START_FREQ_SWEEP) | AD5934_CONTROL_RANGE(AD5934_400mVpp_RANGE) |  AD5934_PGA_GAIN(AD5934_PGA_GAIN_X1)), 1);

}

/******************************************************************************
* @brief Repeat Sweep at the AD5934 with the same sweep parameters.
*
* @param None.
*
* @return none.
******************************************************************************/
void AD5934_Repeat_Sweep(void)
{
	// Repeat AD5934 Sweep
	AD5934_SetRegisterValue(AD5934_CONTROL_REG_HB, (AD5934_CONTROL_FUNCTION(AD5934_REPEAT_FREQ) | AD5934_CONTROL_RANGE(AD5934_400mVpp_RANGE) |  AD5934_PGA_GAIN(AD5934_PGA_GAIN_X1)), 1);
}

/******************************************************************************
* @brief Stop the AD5934 to sweep.
*
* @param None.
*
* @return None.
******************************************************************************/
void AD5934_StopSweep(void)
{
	// Power Down
	//AD5934_SetRegisterValue(AD5934_CONTROL_REG_HB, (AD5934_CONTROL_FUNCTION(AD5934_POWER_DOWN) | AD5934_CONTROL_RANGE(AD5934_400mVpp_RANGE) |  AD5934_PGA_GAIN(AD5934_PGA_GAIN_X1)), 1);

	// Place AD5934 in standby (instead of power down to save startup time)
	AD5934_SetRegisterValue(AD5934_CONTROL_REG_HB, (AD5934_CONTROL_FUNCTION(AD5934_STANDBY) | AD5934_CONTROL_RANGE(AD5934_400mVpp_RANGE) |  AD5934_PGA_GAIN(AD5934_PGA_GAIN_X1)), 1);

}


/******************************************************************************
* @brief Check if PT100 or PT1000 to Calculate Temperature.
*
* @param mag_ref - Magnitude of the Reference Resistor.
*
* @param mag_dut - Magnitude of the device under test.
*
* @return Temperature in Celsius.
******************************************************************************/
float AD5934_Calculate_Temperature(float mag_ref_pt100, float mag_ref_pt1000, float mag_dut)
{
	uint8_t ch_ref;
	float gain_factor, temperature, drift, mag_ref;

	if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_7) == GPIO_PIN_SET) // OFF = PT1000, ON = PT100, PA7 has internal pull-up and switch connects to GND
	{
		ch_ref = AD5934_CH_RTD_LOW_GAIN; //PT100
		gain_factor = AD5934_GAIN_FACTOR_100R;
		drift = AD5934_DRIFT_PT100;
		mag_ref = mag_ref_pt100;
	}
	else
	{
		ch_ref = AD5934_CH_RTD_MID_GAIN; //PT1000
		gain_factor = AD5934_GAIN_FACTOR_1K;
		drift = AD5934_DRIFT_PT1000;
		mag_ref = mag_ref_pt1000;
	}

	float ratio_mag = mag_ref / mag_dut;

	// This correction isn't validated
	/*if(ch_ref==AD5934_CH_REF100R_LOW_GAIN) // only for PT100
		impedance_dut = AD5934_Linear_Correction(gain_factor * ratio_mag);
	else*/

	float impedance_dut = gain_factor * ratio_mag;

	// Calculate impedance ratio with the Reference Resistor
	float ratio = impedance_dut / gain_factor;

	// Calculate impedance discriminant with the ratio
	float discriminant = (AD5934_RTD_A*AD5934_RTD_A)-(4.0f * AD5934_RTD_B * (1.0f - ratio));

    if (discriminant >= 0.0f && impedance_dut >= gain_factor)
    {
        // Ramo T >= 0°C: solução quadrática exata
        temperature = (-AD5934_RTD_A + sqrtf(discriminant))/(2.0f * AD5934_RTD_B);
    }
    else
    {
        // Ramo T < 0°C (ou discriminante inválido por ruído): aproximação linear
        temperature = (ratio - 1.0f) / AD5934_RTD_ALPHA;
    }

	return AD5934_Round_Float_Precision((temperature-drift),1);

}

/**
 * @brief Compensate magnitude to a value in 25C.
 *
 * @param mag_dut The current Magnitude of the DUT (Device Under Test).
 * @param temperature_dut The current temperature of the DUT (Device Under Test).
 *
 * @return The value of the Magnitude of the dut when it in 25C.
 */
float AD5934_EC_Compensate_Magnitude_To_25C(float mag_dut, float temperature_dut)
{
	float factor = 1.0f + AD5934_EC_ALPHA_PER_C * (temperature_dut - AD5934_EC_TEMP_REF_C);

	if (factor < 0.1f)
		factor = 0.1f;

	return (mag_dut/factor);
}

/**
 * @brief Calibrates EC sensor reading to 25°C reference point
 * @param temp_C Temperature of the liquid in Celsius
 * @return Temperature-compensated Correction factor value at 25°C reference
 */
float AD5934_EC_Calibrate_Temperature(float temp_C)
{
	// Calibration data points for standard solution (1413 uS/cm)
    const float temps[] = {5, 10, 15, 18, 20, 22, 25, 30, 35, 40, 45, 50};
    const float conductivities[] = {893, 1018, 1149, 1225, 1278, 1329, 1413, 1552, 1691, 1843, 1988, 2145};

    // Find the two nearest temperature points for interpolation
    int32_t temp_index = -1;
    for (int i = 0; i < 12; i++)
    {
        if (temps[i] >= temp_C)
        {
            temp_index = i;
            break;
        }
    }

    // Handle boundary cases
    if (temp_index == -1) {
        // Temperature higher than max point (50°C)
        temp_index = 10;
    } else if (temp_index == 0) {
        // Temperature lower than min point (5°C)
        temp_index = 0;
    }

    // Interpolate conductivity at measured temperature
    float temp1 = temps[temp_index - 1];
    float temp2 = temps[temp_index];
    float cond1 = conductivities[temp_index - 1];
    float cond2 = conductivities[temp_index];

    // Linear interpolation to get conductivity at measured temperature
    float interpolated_cond = cond1 + (cond2 - cond1) * (temp_C - temp1) / (temp2 - temp1);

    // Return real conductivity at the temperature
    return(interpolated_cond/1413.0f);
}

/******************************************************************************
* @brief Get Real and Imaginary values and calculate Magnitude.
*
* @param: none
*
* @return Magnitude (float).
******************************************************************************/
float AD5934_GetMagnitude(void)
{

	// Get Real Data register
	int16_t real_value = ((AD5934_GetRegisterValue(AD5934_REAL_REG_HB,2)));

	// Get Imaginary Data register
	int16_t imag_value = ((AD5934_GetRegisterValue(AD5934_IMG_REG_HB,2)));

	// Calculate Magnitude using float function sqrtf
	float magnitude = sqrtf(((float)real_value * (float)real_value) + ((float)imag_value * (float)imag_value));

	// Return magnitude value
	return (magnitude);
}


/******************************************************************************
* @brief Get Real and Imaginary values and calculate Magnitude.
*
* @param: none
*
* @return Magnitude (float).
******************************************************************************/
float AD5934_GetImpedance(float mag_ref, float mag_dut)
{
	uint8_t ch_ref;
	float gain_factor;

	if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_7) == GPIO_PIN_SET) // OFF = PT1000, ON = PT100, PA7 has internal pull-up and switch connects to GND
	{
		ch_ref = AD5934_CH_RTD_LOW_GAIN; //PT100
		gain_factor = AD5934_GAIN_FACTOR_100R;
	}
	else
	{
		ch_ref = AD5934_CH_RTD_MID_GAIN; //PT1000
		gain_factor = AD5934_GAIN_FACTOR_1K;
	}

	float impedance_ref = (1/(mag_ref * gain_factor));

	float impedance_dut = (1/(mag_dut * impedance_ref));

	return impedance_dut;
}

/******************************************************************************
* @brief Get Real and Imaginary values and calculate Magnitude.
*
* @param: none
*
* @return Magnitude (float).
******************************************************************************/
float AD5934_GetAdmittance(float mag_ref_10mS_MidGain, float mag_ref_1mS_MidGain, float mag_ref_1mS_HighGain, float mag_ref_0_1mS_HighGain, float mag_dut)
{
	uint8_t ch_ref;
	float mag_ref_Min, mag_ref_Max, res_ref_Min, res_ref_Max;

	if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_6) == GPIO_PIN_SET) // Hardware Setup: Gain Selection (PA6)
	{
		ch_ref = AD5934_CH_EC_HIGH_GAIN; // 5mS
		mag_ref_Min = mag_ref_0_1mS_HighGain; // Ref 10k
		mag_ref_Max = mag_ref_1mS_HighGain; // Ref 1K
		res_ref_Min = AD5934_EC_0_1MS;
		res_ref_Max = AD5934_EC_1MS;
	}
	else
	{
		ch_ref = AD5934_CH_EC_MID_GAIN; //10mS
		mag_ref_Min = mag_ref_1mS_MidGain; // Ref 1K
		mag_ref_Max = mag_ref_10mS_MidGain; // Ref 100R
		res_ref_Min = AD5934_EC_1MS;
		res_ref_Max = AD5934_EC_10MS;
	}

	float gain_factor = (res_ref_Max - res_ref_Min)/(mag_ref_Max - mag_ref_Min);

	float offset = mag_ref_Max - (res_ref_Max/gain_factor);

	float admittance = (mag_dut - offset) * gain_factor;

	float y_cell = admittance / (1 - (AD5934_EC_IN_SERIES_RESISTOR * admittance)); // Resistor in series with EC is 100 Ohms on PCB

	//float y_cell = admittance;

	return (y_cell*1000); // in mS
}
/******************************************************************************
* @brief Get Real and Imag values from Magnitude.
*
* @param: none
*
* @return Magnitude (int16).
******************************************************************************/
uint32_t AD5934_Get_Real_Imag_Numbers(void)
{
	uint32_t real_imag_reg =  AD5934_GetRegisterValue(AD5934_IMG_REG_LB, 4); //AD5934_ReadRegister(AD5934_REAL_REG_LB, 4); //

	return (real_imag_reg);
}





/**
 * @brief Converte um valor float em uma escala de FLOAT_MIN a FLOAT_MAX para uint16_t em escala de UINT16_MIN a UINT16_MAX
 *
 * @param value Valor float entre FLOAT_MIN e FLOAT_MAX
 * @param scale Escala de Ref Res, EC e RTD
 *
 * @return uint16_t Valor convertido na escala de UINT16_MIN a UINT16_MAX
 */
uint16_t AD5934_Compress_To_IntScale(float value, uint8_t scale)
{
	// Proteção contra índice inválido para evitar HardFault
	if (scale >= AD5934_CH_MAX)
	      return 0;

    // Verifica limites do valor de entrada
    if (value < AD5934_Scale_Float_Min[scale])
          return AD5934_Scale_Out_Min[scale];
    else if (value > AD5934_Scale_Float_Max[scale])
          return AD5934_Scale_Out_Max[scale];

    // Converte o valor usando a fórmula: ((value - FLOAT_MIN) / (FLOAT_MAX - FLOAT_MIN)) * (UINT16_MAX - UINT16_MIN) + UINT16_MIN
    //uint16_t result = (uint16_t)(((value - AD5934_Scale_Float_Min[scale]) / (AD5934_Scale_Float_Max[scale] - AD5934_Scale_Float_Min[scale])) * ((float)AD5934_Scale_Out_Max[scale] - (float)AD5934_Scale_Out_Min[scale]) + (float)AD5934_Scale_Out_Min[scale]);

    float factor = ((float)AD5934_Scale_Out_Max[scale] - (float)AD5934_Scale_Out_Min[scale]) / (AD5934_Scale_Float_Max[scale] - AD5934_Scale_Float_Min[scale]);
    uint16_t result = (uint16_t)((float)AD5934_Scale_Out_Min[scale] + (value - AD5934_Scale_Float_Min[scale]) * factor);

    // Garante que o resultado não exceda o limite superior
    if (result > AD5934_Scale_Out_Max[scale])
         return (AD5934_Scale_Out_Max[scale]);

    return result;
}


/**
 * @brief Reduz a precisão decimal de um float através de truncamento.
 *
 * @param valor O valor float original.
 * @param casas_decimais Quantidade de casas que devem permanecer após a vírgula.
 * @return float O valor com as casas decimais excedentes removidas.
 */
float AD5934_Round_Float_Precision(float value, uint8_t number_of_decimals)
{
    float multiply = 1.0f;

    for (uint8_t i = 0; i < number_of_decimals; i++)
    	multiply *= 10.0f;

    return roundf(value * multiply) / multiply;
}


/**
 * @brief Performs a linear transformation optimized to match the provided dataset.
 *
 * This function implements a linear regression model: y = mx + c
 * Derived from least squares fitting of the input/output pairs:
 * m (slope) ≈ 1.02236
 * c (offset) ≈ -2.1163
 *
 * The implementation is branchless (no conditionals) to ensure constant
 * execution time (O(1)) and prevent pipeline stalls in ARM Cortex-M3.
 *
 * @param input_val The raw float value from sensor/ADC.
 * @return float The transformed value following the requested trend.
 */
float AD5934_Linear_Correction(float raw_value)
{
    /* Pre-calculated constants from regression analysis */
    const float slope = 1.02281f;
    const float offset = (-2.1409f);

    /* Single FPU instruction (if FPU available) or optimized software float mul/add */
    return (raw_value * slope) + offset;
}

/**
 * @brief Calibrate AD5934 using dry probe and standard solution
 *
 * @param dry_probe_real Raw real value from dry probe measurement
 * @param dry_probe_imag Raw imaginary value from dry probe measurement
 * @param standard_real Raw real value from 1413 uS/cm standard solution
 * @param standard_imag Raw imaginary value from 1413 uS/cm standard solution
 * @return float Calibration factor for converting raw readings to admittance
 */
float AD5934_Calibrate(float dry_probe_real, float dry_probe_imag, float standard_real, float standard_imag)
{
    // Calculate magnitude of dry probe reading
    float dry_magnitude = sqrtf(dry_probe_real * dry_probe_real + dry_probe_imag * dry_probe_imag);

    // Calculate magnitude of standard solution reading
    float standard_magnitude = sqrtf(standard_real * standard_real + standard_imag * standard_imag);

    // Calculate calibration factor based on known standard (1413 uS/cm)
    // This assumes the standard provides a known conductance value
    float calibration_factor = 1413.0f / standard_magnitude;

    return calibration_factor;
}

/**
 * @brief State Machine to process values from AD5934 without blocking delays
 *
 * @param none
 *
 * @return none
 */
void AD5934_Process_System(void)
{
	switch (ad5934_state)
    {
        case AD5934_IDLE:
        	ADG715_SetChannels(current_hw_mux_connection);	// Starts changing the mux channel
            is_new_channel = 1; // Forces first channel
            state_timer = g_ms_counter;
            ad5934_state = AD5934_WAIT_MUX;
            break;

        case AD5934_WAIT_MUX:
            HAL_Delay(15);
        	ad5934_state = AD5934_START_CONVERSION;
            break;

        case AD5934_START_CONVERSION:
			/* DECISÃO DE COMANDO: Novo canal vs Leituras sucessivas */
			if (is_new_channel) // Decision: Channel Switched or Next Sample in the Burst ?
			{
				AD5934_RestartSweep();  // Clean some AD5934 internal registers to Re-Start Sweep
				is_new_channel = 0; 	// Resets the flag to read the next Sample Burst
			}
			else
			{
				AD5934_Repeat_Sweep(); // Get Sample and just repeat reading
			}

			ad5934_state = AD5934_WAIT_CONVERSION; // Next State
            break;

        case AD5934_WAIT_CONVERSION:
        	uint8_t status = (uint8_t)AD5934_GetRegisterValue(AD5934_STATUS_REG, 1); // Check if the current converion is ready in the AD5934
            if ((status & AD5934_STATUS_DATA_VALID) != 0)
            {
            	uint32_t magnitude_int =  AD5934_Get_Real_Imag_Numbers();
            	int16_t mag_real = (int16_t)(magnitude_int>>16);
            	int16_t mag_imag = (int16_t)(magnitude_int & 0x0000FFFF);
            	float mag_float = sqrtf(((float)mag_real * (float)mag_real) + ((float)mag_imag * (float)mag_imag));

            	switch (current_mux_channel)
				{
					case AD5934_ID_RTD:
						AD5934_RTD_NewSample(mag_float);
						break;
					case AD5934_ID_EC:
						AD5934_EC_NewSample(mag_float);
						break;
					case AD5934_ID_REF_100R_LOWGAIN:
						AD5934_Reference_100R_LowGain_NewSample(mag_float);
						break;
					case AD5934_ID_REF_100R_MIDGAIN:
						AD5934_Reference_100R_MidGain_NewSample(mag_float);
						break;
					case AD5934_ID_REF_1K_MIDGAIN:
						AD5934_Reference_1K_MidGain_NewSample(mag_float);
						break;
					case AD5934_ID_REF_1K_HIGHGAIN:
						AD5934_Reference_1K_HighGain_NewSample(mag_float);
						break;
					case AD5934_ID_REF_10K_HIGHGAIN:
						AD5934_Reference_10K_HighGain_NewSample(mag_float);
						break;
				}
            	if(sweep_count<(AD5934_BURST_SIZE-1))
            	{
            		sweep_count++;
            		ad5934_state = AD5934_START_CONVERSION;
            	}
            	else
            	{
            		sweep_count = 0;
            		ad5934_state = AD5934_SWITCH_MUX;
            	}
            }
            break;


        case AD5934_SWITCH_MUX:
        	AD5934_StopSweep();		// Put AD5934 in Standby
        	current_mux_channel = (uint8_t)((current_mux_channel + 1) % 7); // Pointer to the next channel (Circular Buffer 0 to  6)
        	switch (current_mux_channel)
        	{
				case AD5934_ID_RTD:
					if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_7) == GPIO_PIN_SET) // OFF = PT1000, ON = PT100, PA7 has internal pull-up and switch connects to GND
						current_rtd_mux = current_hw_mux_connection = AD5934_CH_RTD_LOW_GAIN; //PT100
					else
						current_rtd_mux = current_hw_mux_connection = AD5934_CH_RTD_MID_GAIN; //PT1000
					break;

				case AD5934_ID_EC:
					if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_6) == GPIO_PIN_SET) // Hardware Setup: Gain Selection (PA6)
						current_ec_mux = current_hw_mux_connection = AD5934_CH_EC_HIGH_GAIN;
					else
						current_ec_mux = current_hw_mux_connection = AD5934_CH_EC_MID_GAIN;
					break;

				case AD5934_ID_REF_100R_LOWGAIN:
					current_ref_mux = current_hw_mux_connection = AD5934_CH_REF100R_LOW_GAIN; // 100 Ohms Reference Resistor
					break;
				case AD5934_ID_REF_100R_MIDGAIN:
					current_ref_mux = current_hw_mux_connection = AD5934_CH_REF100R_MID_GAIN; // 100 Ohms Reference Resistor
					break;
				case AD5934_ID_REF_1K_MIDGAIN:
					current_ref_mux = current_hw_mux_connection = AD5934_CH_REF1K_MID_GAIN; // 1K Reference Resistor
					break;
				case AD5934_ID_REF_1K_HIGHGAIN:
					current_ref_mux = current_hw_mux_connection = AD5934_CH_REF1K_HIGH_GAIN; // 1K Reference Resistor
					break;
				case AD5934_ID_REF_10K_HIGHGAIN:
					current_ref_mux = current_hw_mux_connection = AD5934_CH_REF10K_HIGH_GAIN; // 1K Reference Resistor
					break;

			}
        	ADG715_SetChannels(current_hw_mux_connection); 							// Change the analog mux
        	HAL_Delay(12);
        	is_new_channel = 1;												// Channel Switched
		  	ad5934_state = AD5934_START_CONVERSION;							// Next State
            break;
    }
}


/**
* Simple insertion sort - BURST_SIZE is small, so the cost is negligible
* and it avoids depending on qsort/heap allocation.
**/
void AD5934_Sort_Array(float *v, uint8_t n)
{
    for (uint8_t i = 1; i < n; i++)
    {
        float key = v[i];
        int8_t j = (int8_t)i - 1;
        while (j >= 0 && v[j] > key)
        {
            v[j + 1] = v[j];
            j--;
        }
        v[j + 1] = key;
    }
}

/**
 * Trimmed mean: sorts the buffer IN PLACE (the caller's burst buffer is
 * about to be reset right after this call anyway, so preserving its
 * original order is not needed), discards TRIM_COUNT values from each
 * end, and averages the remainder. Sorting in place avoids a temporary
 * array and removes the need for memcpy()/string.h entirely.
 **/
float AD5934_Trimmed_Mean(float *buffer, uint8_t n, uint8_t trim)
{
    AD5934_Sort_Array(buffer, n);

    uint8_t start = trim;
    uint8_t end   = n - trim; /* exclusive */

    if (end <= start) /* guard against a misconfigured trim value */
    {
        start = 0;
        end   = n;
    }

    float sum = 0.0f;
    uint8_t count = 0;
    for (uint8_t i = start; i < end; i++)
    {
        sum += buffer[i];
        count++;
    }
    return sum / (float)count;
}

/*
 *  Simple average of the history buffer (stage-2 sliding window)
 */
float AD5934_History_Average(const float *hist, uint8_t n)
{
    float sum = 0.0f;
    for (uint8_t i = 0; i < n; i++)
        sum += hist[i];
    return sum / (float)n;
}


/*
 * Common processing core: applied identically to all 3 channels.
 * Receives the current raw reading and the channel's state (global
 * arrays/struct).
 **/
void AD5934_Process_Sample(AD5934_filter_t *ch, float raw)
{
    /* --- Stage 1: accumulate into the current burst --- */
    ch->burst[ch->burst_index] = raw;
    ch->burst_index++;

    if (ch->burst_index < AD5934_BURST_SIZE)
        return; /* still collecting the burst, nothing else to do */

    /* Burst complete: compute the trimmed mean */
    float burst_result = AD5934_Trimmed_Mean(ch->burst, AD5934_BURST_SIZE, AD5934_TRIM_COUNT);
    ch->burst_index = 0; /* reset for the next burst */

    /* --- Stage 2a: rolling history / sliding window --- */
    ch->history[ch->history_index] = burst_result;
    ch->history_index = (uint8_t)((ch->history_index + 1) % AD5934_HISTORY_SIZE);
    if (ch->history_count < AD5934_HISTORY_SIZE)
        ch->history_count++;

    float window_average = AD5934_History_Average(ch->history, ch->history_count);

    /* --- Stage 2b: first-order IIR low-pass filter on the burst result --- */
    if (!ch->iir_initialized)
    {
        ch->iir_state = burst_result;
        ch->iir_initialized = 1;
    }
    else
    {
        ch->iir_state = AD5934_IIR_ALPHA * burst_result + (1.0f - AD5934_IIR_ALPHA) * ch->iir_state;
    }

    /* Final output: combines the IIR state (fast response to real drift)
     * with the window average (more stable, slower response). Adjust
     * the weighting per channel if needed. */
    ch->filtered_value = 0.9f * ch->iir_state + 0.1f * window_average;
    //ch->filtered_value =  window_average;
    ch->value_valid = 1;
}

void AD5934_MovingAvg_Init(AD5934_AvgFilter_t *filt)
{
    for (uint8_t i = 0; i < AD5934_HISTORY_SIZE; i++) {
        filt->buffer[i] = 0.0f;
    }
    filt->sum = 0.0f;
    filt->index = 0;
    filt->count = 0;
    filt->average = 0.0f;
}

void AD5934_MovingAvg_Process(AD5934_AvgFilter_t *filt, float new_sample)
{
	for (uint8_t i = (AD5934_HISTORY_SIZE-1); i > 0; i--)
		filt->buffer[i] = filt->buffer[i-1];

	filt->buffer[0] = new_sample;

	for (uint8_t i = 0; i < AD5934_HISTORY_SIZE; i++)
		filt->sum += new_sample;

	filt->average = (filt->sum / (float)AD5934_HISTORY_SIZE);
}


/* ---------------------------------------------------------------------- */
/*                   PUBLIC FUNCTIONS - ONE PER CHANNEL                   */
/* ---------------------------------------------------------------------- */

/* Call this every time a sweep completes with the mux on the RTD
 * (PT100/PT1000) channel */
void AD5934_RTD_NewSample(float raw)
{
	AD5934_Process_Sample(&g_rtd_filter, raw);
}


/* Call this every time a sweep completes with the mux on the EC
 * (conductivity probe) channel */
void AD5934_EC_NewSample(float raw)
{
	AD5934_Process_Sample(&g_ec_filter, raw);
}


/* Call this every time a sweep completes with the mux on the on-board
 * precision reference resistor channel */
void AD5934_Reference_100R_LowGain_NewSample(float raw)
{
	AD5934_Process_Sample(&g_ref_100R_LowGain_filter, raw);
}

/* Call this every time a sweep completes with the mux on the on-board
 * precision reference resistor channel */
void AD5934_Reference_100R_MidGain_NewSample(float raw)
{
	AD5934_Process_Sample(&g_ref_100R_MidGain_filter, raw);
}

/* Call this every time a sweep completes with the mux on the on-board
 * precision reference resistor channel */
void AD5934_Reference_1K_MidGain_NewSample(float raw)
{
	AD5934_Process_Sample(&g_ref_1K_MidGain_filter, raw);
}

/* Call this every time a sweep completes with the mux on the on-board
 * precision reference resistor channel */
void AD5934_Reference_1K_HighGain_NewSample(float raw)
{
	AD5934_Process_Sample(&g_ref_1K_HighGain_filter, raw);
}

/* Call this every time a sweep completes with the mux on the on-board
 * precision reference resistor channel */
void AD5934_Reference_10K_HighGain_NewSample(float raw)
{
	AD5934_Process_Sample(&g_ref_10K_HighGain_filter, raw);
}

/*****************************************************************************
 * ADG715
*****************************************************************************
 * @brief Writes data into a register.
 *
 * @param value - Data value to write.
 *
 * @return None.
*******************************************************************************/

void ADG715_SetRegisterValue(char value)
{
	uint8_t  writeData[1]	= {value};
	HAL_StatusTypeDef status;

	status = HAL_I2C_Master_Transmit(&hi2c2, ADG715_I2C_ADDRESS, writeData, 1, 1);

	return;
}

/******************************************************************************
* @brief Disconnect all Channels on the ADG715.
*
* @param: channel
*
* @return none.
******************************************************************************/

void ADG715_ResetChannels(void)
{
	ADG715_SetRegisterValue(0);
}

/******************************************************************************
* @brief Set Channels on the ADG715.
*
* @param: channel
*
* @return none.
******************************************************************************/
void ADG715_SetChannels(AD5934_Channel_t channel)
{
    // Garante que o índice recebido não ultrapassa os limites do vetor
    if (channel >= AD5934_CH_MAX)
        return;

    // Envia o byte combinado via I2C (Make-before-break nativo por barramento)
    ADG715_SetRegisterValue(ADG715_Channel_Map[channel]);

}


