/**
 * @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 void SdlFilterInterrupt(void);

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;
static PuControllersPidInput_st pmpRoPressurePidInput_st = {0};
static PuFiltersSensorData_st receivedFilterData_st;
/******************************************************************************
 * Public Function Definitions
 ******************************************************************************/
void SdlInit(void)
{
    testBenchStarted_b = false;

    HalTimerStart(MAP_HAL_TIMER_FILTER);
    NmsCanInit();
    PuFiltersInit();
	ProcessBoardInit();
	HalTimerConfigureCallback(MAP_HAL_TIMER_FILTER, SdlFilterInterrupt);

	PuControllersInitInputs(&pmpRoPressurePidInput_st);
}


void SdlRun(void)
{
	static uint64 lastPressureUpdateMs_u64 = 0uLL;
	uint64 currentTimeMs_u64;
	static uint8 pressureSetpoint_u8 = 0u;
	uint8 turnOnTestbench = 0u;

	NmsCanGetObj_u8(0x2007, 0x0, &turnOnTestbench);
	NmsCanRun();

//	if (turnOnTestbench == 1)
//	{
//		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
//	{
//		ProcessBoardRun(false);
//	}
}



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();
}