/**
 * @file	  pressureController.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 "pressureController.h"
#include "math.h"
/******************************************************************************
 * Macro constant declarations
 ******************************************************************************/

/******************************************************************************
 * Type declarations
 ******************************************************************************/

/******************************************************************************
 * Module Global Variable Declarations
 ******************************************************************************/

/******************************************************************************
 * Private Function Declarations
 ******************************************************************************/

/******************************************************************************
 * Extern Function Declarations
 ******************************************************************************/
void PuControllersInitInputs(PuControllersPidInput_st *pumpPin_pst)
{
	/* === Pump RO Pressure ===	*/
	pumpPin_pst->kpGain_f32 				= 6.0f;
	pumpPin_pst->kiGain_f32 				= 5.0f;
	pumpPin_pst->kdGain_f32 				= -3.0f;
	pumpPin_pst->satDeriv_f32 				= 5.0f;
	pumpPin_pst->sampleTime_f32 			= 0.1f;
	pumpPin_pst->commandMin_f32 			= 0.0f;
	pumpPin_pst->commandMax_f32				= 100.0f;
	pumpPin_pst->regulatorActivation_f32 	= 1.0f;
	pumpPin_pst->integralResetFlag_b		= false;
}


void PuControllersPidLoop(const PuControllersPidInput_st *input_pst, PuControllersPidOutput_st *output_pst, PuControllersPidState_st *state_pst)
{
    float32 derivTerm_f32;

    /* External Trigger  */
    if (input_pst->integralResetFlag_b)
	{
		state_pst->integState_f32 = 0.0f;
	}
    if(input_pst->regulatorActivation_f32 == 0.0f)
    {
    	state_pst->prevMeasFilt2_f32 	= 0.0f;
    	state_pst->sumOutput_f32 		= 0.0f;
    	state_pst->integState_f32 		= 0.0f;

    	output_pst->derivOutput_f32 = 0.0f;
    	output_pst->integOutput_f32 = 0.0f;
    	output_pst->propOutput_f32 	= 0.0f;

    }
    /* Check if the regulator control should be active */
    if (input_pst->regulatorActivation_f32 > 0.0f)
    {
    	 /* If regulator was inactive previously, reset integral and derivative states */
        if (!state_pst->isRegulatorActive_b)
        {
            state_pst->prevMeasFilt2_f32 	= 0.0f;
            state_pst->integState_f32 		= 0.0f;
            state_pst->isRegulatorActive_b 	= true;
        }

        /* Compute derivative component based on measurement change rate */
        derivTerm_f32 = -(input_pst->measFilt2_f32 - state_pst->prevMeasFilt2_f32) * input_pst->kdGain_f32 / input_pst->sampleTime_f32;

        /* Apply saturation limits to derivative output to prevent excessive corrections */
        if (derivTerm_f32 > input_pst->satDeriv_f32)
        {
            output_pst->derivOutput_f32 = input_pst->satDeriv_f32;
        }
        else if (derivTerm_f32 < -input_pst->satDeriv_f32)
        {
            output_pst->derivOutput_f32 = -input_pst->satDeriv_f32;
        }
        else
        {
            output_pst->derivOutput_f32 = derivTerm_f32;
        }

        /* Output current integral state */
        output_pst->integOutput_f32 = state_pst->integState_f32;

        /* Compute proportional output based on setpoint error */
        derivTerm_f32 = input_pst->setpoint_f32 - input_pst->measFilt_f32;

        if (fabsf(derivTerm_f32) > input_pst->tolerance_f32)
		{
        	output_pst->propOutput_f32 = input_pst->kpGain_f32 * derivTerm_f32;
		}
        else
        {
        	output_pst->propOutput_f32 = 0.0f;
        	output_pst->derivOutput_f32 = 0.0;
        }
        /* Sum PID components to form the overall regulator output */
        state_pst->sumOutput_f32 = output_pst->propOutput_f32 + output_pst->integOutput_f32 + output_pst->derivOutput_f32;

        /* Calculate integral contribution based on error */
        derivTerm_f32 *= input_pst->kiGain_f32;

        /* Adjust integral contribution to respect command saturation limits */
        if (state_pst->prevCommandInput_f32 >= input_pst->commandMax_f32)
        {
            derivTerm_f32 = (float32)(fmin((float64)derivTerm_f32, 0.0f));
        }
        else if (input_pst->commandMin_f32 >= state_pst->prevCommandInput_f32)
        {
            derivTerm_f32 = (float32)(fmax(0.0f, (float64)derivTerm_f32));
        }

        /* Update previous measurement for derivative calculation in next cycle */
        state_pst->prevMeasFilt2_f32 = input_pst->measFilt2_f32;

        if (input_pst->kpGain_f32 == 0.0f)// I controller only
		{
        	state_pst->integState_f32 += input_pst->sampleTime_f32 * derivTerm_f32;
		}
        /* Integrate integral term with adjusted contribution */
        else
        {
        	if (fabsf(output_pst->propOutput_f32 / input_pst->kpGain_f32) > input_pst->tolerance_f32)
			{
        		state_pst->integState_f32 += input_pst->sampleTime_f32 * derivTerm_f32;
			}
        }

    }
    else if (state_pst->isRegulatorActive_b)
    {
    	/* Disable regulator and reset summed output if regulator is deactivated */
        state_pst->sumOutput_f32 = 0.0f;
        state_pst->isRegulatorActive_b = false;
    }

    /* Combine regulator output with feed-forward command to form total command */
    derivTerm_f32 = input_pst->commandRm_f32 + state_pst->sumOutput_f32;

    /* Ensure final command output respects specified saturation limits */
    if (derivTerm_f32 > input_pst->commandMax_f32)
    {
        output_pst->commandSetpoint_f32 = input_pst->commandMax_f32;
    }
    else if (derivTerm_f32 < input_pst->commandMin_f32)
    {
        output_pst->commandSetpoint_f32 = input_pst->commandMin_f32;
    }
    else
    {
        output_pst->commandSetpoint_f32 = derivTerm_f32;
    }

    /* Store last command to aid future saturation checks */
    state_pst->prevCommandInput_f32 = derivTerm_f32;
}


/******************************************************************************
 * Private Function Definitions
 ******************************************************************************/

/******************************************************************************
 * Callback Function Definitions
 ******************************************************************************/