/**
 * @file	   sdl.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	   This file contains the initialization and execution logic for
 *             the Valve Controller and CANopen stack. It defines the main
 *             scheduler loop for running these components within the firmware.
 *
 * @todo	   SDL_MIN_OPERATIONAL_NODES to be changed to 20, are the node is fixed.
 * 			   (electronic issue in the board)
 *
 */

/******************************************************************************
 * Include Header Files
 ******************************************************************************/
/* CANopen includes */
#include <co_canopen.h>
#include <gen_define.h>
#include <co_canopen.h>
#include "co_sdo.h"
#include "co_odaccess.h"

/* User includes */
#include "sdl.h"
#include "processBoard.h"
#include "enduranceTestBench.h"
#include "hal_system.h"
#include "nms_can.h"

#include "hal_timer.h"
#include "stdlib.h"
#include "filter.h"
#include "pressureController.h"
/******************************************************************************
 * Macro constant declarations
 ******************************************************************************/

/******************************************************************************
 * Type Declarations
 ******************************************************************************/
static inline float32 RevModelConvertToPercent(float32 value_f32, float32 minVal_f32, float32 maxVal_f32)
{
    float32 clamped = value_f32;

    if (clamped < minVal_f32)
    {
        clamped = minVal_f32;
    }
    else if (clamped > maxVal_f32)
    {
        clamped = maxVal_f32;
    }

    return 100.0f * (clamped - minVal_f32) / (maxVal_f32 - minVal_f32);
}
/******************************************************************************
 * Global Declarations
 ******************************************************************************/
static bool testBenchStarted_b;
/******************************************************************************
 * Public Function Definitions
 ******************************************************************************/
void SdlInit(void)
{
    testBenchStarted_b = false;
    NmsCanInit();
	ProcessBoardInit();

#ifdef PREESURE_CONTROLLER
	static PuControllersPidInput_st pmpRoPressurePidInput_st = {0};
	static PuFiltersSensorData_st receivedFilterData_st;

	HalTimerStart(MAP_HAL_TIMER_FILTER);
	HalTimerConfigureCallback(MAP_HAL_TIMER_FILTER, SdlFilterInterrupt);
	PuFiltersInit();
	PuControllersInitInputs(&pmpRoPressurePidInput_st);
#endif
}


void SdlRun(void)
{
	NmsCanRun();

#ifdef PREESURE_CONTROLLER
	static uint64 lastPressureUpdateMs_u64 = 0uLL;
	uint64 currentTimeMs_u64;
	static uint8 pressureSetpoint_u8 = 0u;

	if (NmsCanAreAllNodesOperational())
	{
		EnduranceTestBenchRun(&testBenchStarted_b);

		if (testBenchStarted_b)
		{
			ProcessBoardRun(true);
			HalSystemGetRunTimeMs(&currentTimeMs_u64);

			if ((currentTimeMs_u64 - lastPressureUpdateMs_u64) >= 5000)
			{
				if (pressureSetpoint_u8 + 2 <= 15)
				{
					pressureSetpoint_u8 += 2;
				}
				else
				{
					pressureSetpoint_u8 = 0;
				}
				lastPressureUpdateMs_u64 = currentTimeMs_u64;
			}

			SdlEnablePressureControl(pressureSetpoint_u8);
		}
	}
#else
	if (NmsCanAreAllNodesOperational())
	{
		EnduranceTestBenchRun(&testBenchStarted_b);
		if (testBenchStarted_b)
		{
			ProcessBoardRun();
		}
		else
		{
			ProcessBoardRun();
		}
	}
#endif
}


#ifdef PREESURE_CONTROLLER
void SdlEnablePressureControl(uint8 pressureSetpoint_u8)
{
	static PuControllersPidOutput_st pmpRoPressurePidOutput_st 	= {0};
	static PuControllersPidState_st pmpRoPressurePidState_st 	= {0};

	pmpRoPressurePidState_st.isRegulatorActive_b = true;

	/* Coming from reverse model */
	float32 cmdPumpRoRm_f32 = 0.0f;
	pmpRoPressurePidInput_st.commandRm_f32 	= cmdPumpRoRm_f32;

	/* Coming from filters */
	pmpRoPressurePidInput_st.measFilt_f32 	= receivedFilterData_st.pressureRoPs2_f32;
	pmpRoPressurePidInput_st.measFilt2_f32  = receivedFilterData_st.pmpPressureMeasFilt_f32;

	pmpRoPressurePidInput_st.setpoint_f32 	= pressureSetpoint_u8;

	PuControllersPidLoop(&pmpRoPressurePidInput_st, &pmpRoPressurePidOutput_st, &pmpRoPressurePidState_st);
	ProcessBoardGrundfosPumpHandler(pmpRoPressurePidOutput_st.commandSetpoint_f32 / 10.0f);
}


static void SdlFilterInterrupt(void)
{
	receivedFilterData_st = PuFiltersRun();
}

#endif
