/**
 * @file	   puFilters.c
 *
 * @copyright  Nehemis SARL reserves all rights even in the event of industrial
 *             property rights. We reserve all rights of disposal such as
 *             copying and passing on to third parties.
 *
 * @brief	   
 *
 */


/******************************************************************************
 * Include Header Files
 ******************************************************************************/

#include "filter.h"
#include "analogMeasurement.h"
#include "flowmeter.h"
#include "pressureSensor.h"
#include "pressureController.h"
#include "grundfos.h"
#include "nms_can.h"
/******************************************************************************
 * Macro constant declarations
 ******************************************************************************/
#define PU_FILTERS_PS1_COEFF_B1		0.136728735997319f
#define PU_FILTERS_PS1_COEFF_B2		0.136728735997319f
#define PU_FILTERS_PS1_COEFF_A2		-0.726542528005361f

#define PU_FILTERS_PS2_COEFF_B1		0.099424464720624f
#define PU_FILTERS_PS2_COEFF_B2		0.099424464720624f
#define PU_FILTERS_PS2_COEFF_A2		-0.801151070558751f

#define PU_FILTERS_PS3_COEFF_B1		0.099424464720624f
#define PU_FILTERS_PS3_COEFF_B2		0.099424464720624f
#define PU_FILTERS_PS3_COEFF_A2		-0.801151070558751f

#define PU_FILTERS_FIRST_ORDER_TS 		0.01f
#define PU_FILTERS_FIRST_ORDER_TAU 		0.015f
#define PU_FILTERS_FIRST_ORDER_TAU_PMP 	1.0f
/******************************************************************************
 * Type declarations
 ******************************************************************************/
/******************************************************************************
 * Module Global Variable Declarations
 ******************************************************************************/
static PuFilter_st ps2RoFilter_gst;
static PressureSensorMain_st ps1_gst;
static PuFilter_st pmpFirstOrderFilter_gst;
static float32 pmpPressureMeasFilt_gf32	= 0.0f;

/******************************************************************************
 * Private Function Declarations
 ******************************************************************************/
static void PuFiltersFirstOrderFilterInit(PuFilter_st *filter_pst, float32 b1_f32,
		float32 b2_f32, float32 a2_f32);
static void PuFiltersButterWorthFilterInit(PuFilter_st *filter_pst, float32 b1_f32,
		float32 b2_f32, float32 a2_f32);
static float32 PuFiltersApply(PuFilter_st *filter_pst, float32 inputSample_f32);
/******************************************************************************
 * Extern Function Declarations
 ******************************************************************************/
void PuFiltersInit(void)
{
	/* Compute first-order filter coefficients for MV07 filter */
	float32 firstOrderAlpha_f32			= 2 * (PU_FILTERS_FIRST_ORDER_TAU / PU_FILTERS_FIRST_ORDER_TS);
	float32 firstOrderCoeffA2_f32		= (1 - firstOrderAlpha_f32) / (1 + firstOrderAlpha_f32);
	float32 firstOrderCoeffB1_f32		= 1 / (1 + firstOrderAlpha_f32);
	float32 firstOrderCoeffB2_f32		= firstOrderCoeffB1_f32;


	firstOrderAlpha_f32			= 2 * (PU_FILTERS_FIRST_ORDER_TAU_PMP / PU_FILTERS_FIRST_ORDER_TS);
	firstOrderCoeffA2_f32		= (1 - firstOrderAlpha_f32) / (1 + firstOrderAlpha_f32);
	firstOrderCoeffB1_f32		= 1 / (1 + firstOrderAlpha_f32);
	firstOrderCoeffB2_f32		= firstOrderCoeffB1_f32;


	PuFiltersFirstOrderFilterInit(&pmpFirstOrderFilter_gst, firstOrderCoeffB1_f32, firstOrderCoeffB2_f32, firstOrderCoeffA2_f32);
	PuFiltersButterWorthFilterInit(&ps2RoFilter_gst, 		PU_FILTERS_PS2_COEFF_B1, PU_FILTERS_PS2_COEFF_B2, PU_FILTERS_PS2_COEFF_A2);
}


PuFiltersSensorData_st PuFiltersRun(void)
{
	ps1_gst.channel_u32 	= PS3_ADC_CHANNEL;
	PressureSensorGetVal(&ps1_gst);

	PuFiltersSensorData_st msg_st = {
        .pressureRoPs2_f32			= ps1_gst.rawT_f32,
        .pmpPressureMeasFilt_f32	= pmpPressureMeasFilt_gf32
    };

	ps1_gst.voltage_f32		= PuFiltersApply(&ps2RoFilter_gst, ps1_gst.voltage_f32);
	pmpPressureMeasFilt_gf32= PuFiltersApply(&pmpFirstOrderFilter_gst,  ps1_gst.rawT_f32);

	return msg_st;
}


void PuQDrainSpFilterInit(QDrainSpFilter_st* filter_pst, float32 Ts_f32)
{
    filter_pst->prevInput_f32 	= 0.0f;
    filter_pst->prevOutput_f32 	= 1200.0f; // as per start_index = 1000
    filter_pst->sampleTime_f32 	= Ts_f32;
}


float32 PuQDrainSpFilterUpdate(QDrainSpFilter_st* filter_pst, float32 currentInput_f32)
{
    float32 A_f32, B_32;
    static float32 xN1_f32 = 0.0f;
    static float32 yN1_f32	= 0.0f;

    float32 Ts_f32 		= filter_pst->sampleTime_f32;
    float32 xN_f32     	= currentInput_f32;

    xN1_f32    	= filter_pst->prevInput_f32;
    yN1_f32    	= filter_pst->prevOutput_f32;

    static float32 yN_f32     	= 0.0f;

    if (xN_f32 > xN1_f32)
    {
        A_f32 	= 1.0f / 200.0f;
        B_32 	= 1.0f / 200.0f;
    }
    else
    {
        A_f32 	= 1.0f / 400.0f;
        B_32	= 1.0f / 400.0f;
    }

    float32 alpha_f32 = -A_f32 * Ts_f32 * 0.5f;
    float32 beta_f32  = B_32 * Ts_f32 * 0.5f;

    yN_f32 = ((1.0f + alpha_f32) * yN1_f32 + beta_f32 * (xN_f32 + xN1_f32)) / (1.0f - alpha_f32);

    NmsCanPutObj_u16(0x2013, 0x03, (uint16)yN_f32);

    // update state
    filter_pst->prevInput_f32 	= xN_f32;
    filter_pst->prevOutput_f32 	= yN_f32;

    return yN_f32;
}
/******************************************************************************
 * Private Function Definitions
 ******************************************************************************/


/**
 * @brief Initialize the butterworth filter structure.
 *
 * @param[in,out] filter_pst Pointer to the filter object.
 * @param[in]     b1_f32     Coefficient for current input.
 * @param[in]     b2_f32     Coefficient for previous input.
 * @param[in]     a2_f32     Coefficient for previous output (negative sign already applied).
 */
static void PuFiltersButterWorthFilterInit(PuFilter_st *filter_pst, float32 b1_f32,
		float32 b2_f32, float32 a2_f32)
{
	filter_pst->b1_f32		= b1_f32;
	filter_pst->b2_f32		= b2_f32;
	filter_pst->a2_f32		= a2_f32;

	filter_pst->xPrev_f32	= 0.0f;
	filter_pst->yPrev_f32	= 0.0f;
}

/**
 * @brief Initialize the first order filter.
 *
 * @param[in,out] filter_pst Pointer to the filter object.
 * @param[in]     b1_f32     Coefficient for current input.
 * @param[in]     b2_f32     Coefficient for previous input.
 * @param[in]     a2_f32     Coefficient for previous output (negative sign already applied).
 */
static void PuFiltersFirstOrderFilterInit(PuFilter_st *filter_pst, float32 b1_f32,
		float32 b2_f32, float32 a2_f32)
{
	filter_pst->b1_f32		= b1_f32;
	filter_pst->b2_f32		= b2_f32;
	filter_pst->a2_f32		= a2_f32;

	filter_pst->xPrev_f32	= 0.0f;
	filter_pst->yPrev_f32	= 0.0f;
}


/**
 * @brief Apply the butterworth filter to a new input sample.
 *
 * @param[in,out] filter_pst 		Pointer to the filter state.
 * @param[in]     inputSample_f32	Current input sample.
 *
 * @return Filtered output value.
 */
static float32 PuFiltersApply(PuFilter_st *filter_pst, float32 inputSample_f32)
{
	float32 output_f32		= filter_pst->b1_f32 * inputSample_f32 + filter_pst->b2_f32 * filter_pst->xPrev_f32 - filter_pst->a2_f32 * filter_pst->yPrev_f32;

	filter_pst->xPrev_f32	= inputSample_f32;
	filter_pst->yPrev_f32	= output_f32;

	return output_f32;
}
/******************************************************************************
 * Callback Function Definitions
 ******************************************************************************/

