/**
 * @file	   processBoard.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 provides the firmware for running the Production unit board.
 *
 */

/******************************************************************************
 * Include Header Files
 ******************************************************************************/
#include "processBoard.h"
#include "flowmeter.h"
#include "pressureSensor.h"
#include "grundfos.h"
#include "od_entries.h"
#include "nms_can.h"
#include "analogMeasurement.h"
#include "hal_system.h"
/******************************************************************************
 * Type  declarations
 ******************************************************************************/

/******************************************************************************
 * Macro Constant Declarations
 ******************************************************************************/
#define NMS_CAN_CANOPEN_MASTER_LINE					1u

#define PU_PUMP_SPEED_CHANGE_INTERVAL               900000uLL
#define PU_PUMP_MAX_SPEED                           10u
#define PU_PUMP_MIN_SPEED                           2u
#define PU_PMP_ENABLE								1u
#define PU_PMP_DISABLE								0u
#define PU_PMP_RATED_SPEED							3600uL
#define PU_MAX_PRESSURE								20u
/******************************************************************************
 * Global variables
 ******************************************************************************/
FlowmeterMain_st flowmeterFM1_gst;
FlowmeterMain_st flowmeterFM2_gst;
FlowmeterMain_st flowmeterFM3_gst;
FlowmeterMain_st flowmeterFM4_gst;

PressureSensorMain_st pressureSensorPS1_gst;
PressureSensorMain_st pressureSensorPS2_gst;
PressureSensorMain_st pressureSensorPS3_gst;
PressureSensorMain_st pressureSensorPS4_gst;

GrundfosMain_st grundfosPMP_gst;
/******************************************************************************
 * Private Function Declarations
 ******************************************************************************/
static void ProcessBoardFlowmeterDataOUT(float32 rawQ_f32);
static void ProcessBoardPressureSensorDataOUT(float32 rawT_f32);
static void ProcessBoardPumpSpeedDataOUT(uint32 pmpSpeed_u32);
/******************************************************************************
 * Extern Function Declarations
 ******************************************************************************/
void ProcessBoardInit(void)
{
	HalSystemInit();
	AnalogMeasurementInit();

	/* Initializing the structures (Pressure sensor, Flow
	 * meter, and Grundfos) with their respective ADC channel */
	pressureSensorPS1_gst.channel_u32 	= PS1_ADC_CHANNEL;
	pressureSensorPS2_gst.channel_u32 	= PS2_ADC_CHANNEL;
	pressureSensorPS3_gst.channel_u32 	= PS3_ADC_CHANNEL;
	pressureSensorPS4_gst.channel_u32 	= PS4_ADC_CHANNEL;

	grundfosPMP_gst.channel_u32 		= PMP_ADC_CHANNEL;

	flowmeterFM1_gst.channel_u32 		= FM1_ADC_CHANNEL;
	flowmeterFM2_gst.channel_u32 		= FM2_ADC_CHANNEL;
	flowmeterFM3_gst.channel_u32 		= FM3_ADC_CHANNEL;
	flowmeterFM4_gst.channel_u32 		= FM4_ADC_CHANNEL;

	FlowmeterInit(&flowmeterFM1_gst);
	FlowmeterInit(&flowmeterFM2_gst);
	FlowmeterInit(&flowmeterFM3_gst);
	FlowmeterInit(&flowmeterFM4_gst);

	PressureSensorInit(&pressureSensorPS1_gst);
	PressureSensorInit(&pressureSensorPS2_gst);
	PressureSensorInit(&pressureSensorPS3_gst);
	PressureSensorInit(&pressureSensorPS4_gst);

	GrundfosPmpInit(&grundfosPMP_gst);
}

void ProcessBoardRun(bool pumpTurnOn_b)
{
	AnalogMesaurementRun();

	if (pumpTurnOn_b == true)
	{
		GrundfosPmpEnable(PU_PMP_ENABLE);
		/* Flowmeter data IN */
		FlowmeterGetFlow(&flowmeterFM1_gst);
		FlowmeterGetFlow(&flowmeterFM2_gst);
		FlowmeterGetFlow(&flowmeterFM3_gst);
		FlowmeterGetFlow(&flowmeterFM4_gst);

		/* Pressure sensor data IN */
//		PressureSensorGetVal(&pressureSensorPS1_gst);
//		PressureSensorGetVal(&pressureSensorPS2_gst);
//		PressureSensorGetVal(&pressureSensorPS3_gst);

		/* Flowmeter data OUT */
		ProcessBoardFlowmeterDataOUT(flowmeterFM1_gst.rawQ_f32);
		ProcessBoardFlowmeterDataOUT(flowmeterFM2_gst.rawQ_f32);
		ProcessBoardFlowmeterDataOUT(flowmeterFM3_gst.rawQ_f32);
		ProcessBoardFlowmeterDataOUT(flowmeterFM4_gst.rawQ_f32);

		/* Pressure sensor data OUT */
		ProcessBoardPressureSensorDataOUT(pressureSensorPS1_gst.rawT_f32);
		ProcessBoardPressureSensorDataOUT(pressureSensorPS2_gst.rawT_f32);
		ProcessBoardPressureSensorDataOUT(pressureSensorPS3_gst.rawT_f32);
		ProcessBoardPressureSensorDataOUT(pressureSensorPS4_gst.rawT_f32);

		ProcessBoardPumpSpeedDataOUT(grundfosPMP_gst.rawQ_f32);

		ProcessBoardGrundfosPumpHandler(pumpTurnOn_b);
	}
	else
	{
		GrundfosPmpEnable(PU_PMP_DISABLE);
	}
}


void ProcessBoardGrundfosPumpHandler(uint8 speed_u8)
{
    GrundfosPmpSetSpeed(speed_u8);

    /* Grundfos Pump feedback speed OUT */
    GrundfosPmpFeedback(&grundfosPMP_gst);

    if ((uint8)(grundfosPMP_gst.rawQ_f32) > PU_PMP_RATED_SPEED)
    {
    	speed_u8 -= 2u;
    }
}
/******************************************************************************
 * Private function definitions
 ******************************************************************************/
//static void ProcessBoardGrundfosPumpHandler(bool pumpTurnOn_b)
//{
//    static uint8 speed_u8         = PU_PUMP_MIN_SPEED;
//    static uint64 startTime_u64   = 0uLL;
//    uint64 currentTimeMs_u64;
//
//    HalSystemGetRunTimeMs(&currentTimeMs_u64);
//
//    if (startTime_u64 == 0uLL)
//    {
//        HalSystemGetRunTimeMs(&startTime_u64);
//    }
//
//    if (pumpTurnOn_b == true)
//    {
//    	GrundfosPmpEnable(PU_PMP_ENABLE);
//    }
//    else
//    {
//    	GrundfosPmpEnable(PU_PMP_DISABLE);
//    }
//
//
//    if ((currentTimeMs_u64 - startTime_u64) >= PU_PUMP_SPEED_CHANGE_INTERVAL)
//    {
//    	if (pressureSensorPS1_gst.rawT_f32 > PU_MAX_PRESSURE || pressureSensorPS2_gst.rawT_f32 > PU_MAX_PRESSURE ||
//    	    pressureSensorPS3_gst.rawT_f32 > PU_MAX_PRESSURE || pressureSensorPS4_gst.rawT_f32 > PU_MAX_PRESSURE)
//    	{
//    	    /* Decrease speed if pressure is too high */
//    	    if (speed_u8 > PU_PUMP_MIN_SPEED)
//    	    {
//    	        speed_u8 -= 2u;
//    	    }
//    	}
//    	else if (speed_u8 <= PU_PUMP_MAX_SPEED)
//        {
//            speed_u8 += 2u;
//        }
//        else
//        {
//            speed_u8 = PU_PUMP_MIN_SPEED;
//        }
//
//        startTime_u64 = currentTimeMs_u64;
//    }
//
//    GrundfosPmpSetSpeed(speed_u8);
//
//    /* Grundfos Pump feedback speed OUT */
//    GrundfosPmpFeedback(&grundfosPMP_gst);
//
//    if ((uint8)(grundfosPMP_gst.rawQ_f32) > PU_PMP_RATED_SPEED)
//    {
//    	speed_u8 -= 2u;
//    }
//}

static void ProcessBoardFlowmeterDataOUT(float32 rawQ_f32)
{
	NmsCanPutObj_u16(OD_ENTRY_PU_FLOWMETER_DATA_OUT_INDEX, OD_ENTRY_PU_FLOWMETER1_DATA_OUT_SUB_INDEX, (uint16)(flowmeterFM1_gst.rawQ_f32 * 100.0f));
	NmsCanPutObj_u16(OD_ENTRY_PU_FLOWMETER_DATA_OUT_INDEX, OD_ENTRY_PU_FLOWMETER2_DATA_OUT_SUB_INDEX, (uint16)(flowmeterFM2_gst.rawQ_f32 * 100.0f));
	NmsCanPutObj_u16(OD_ENTRY_PU_FLOWMETER_DATA_OUT_INDEX, OD_ENTRY_PU_FLOWMETER3_DATA_OUT_SUB_INDEX, (uint16)(flowmeterFM3_gst.rawQ_f32 * 100.0f));
	NmsCanPutObj_u16(OD_ENTRY_PU_FLOWMETER_DATA_OUT_INDEX, OD_ENTRY_PU_FLOWMETER4_DATA_OUT_SUB_INDEX, (uint16)(flowmeterFM4_gst.rawQ_f32 * 100.0f));
}


static void ProcessBoardPressureSensorDataOUT(float32 rawT_f32)
{
	NmsCanPutObj_u16(OD_ENTRY_PU_PRESSURE_DATA_OUT_INDEX, OD_ENTRY_PU_PRESSURE1_DATA_OUT_SUB_INDEX, (uint16)pressureSensorPS1_gst.rawT_f32 * 100u);
	NmsCanPutObj_u16(OD_ENTRY_PU_PRESSURE_DATA_OUT_INDEX, OD_ENTRY_PU_PRESSURE2_DATA_OUT_SUB_INDEX, (uint16)pressureSensorPS2_gst.rawT_f32 * 100u);
	NmsCanPutObj_u16(OD_ENTRY_PU_PRESSURE_DATA_OUT_INDEX, OD_ENTRY_PU_PRESSURE3_DATA_OUT_SUB_INDEX, (uint16)(pressureSensorPS3_gst.rawT_f32 * 100.0f));
	NmsCanPutObj_u16(OD_ENTRY_PU_PRESSURE_DATA_OUT_INDEX, OD_ENTRY_PU_PRESSURE4_DATA_OUT_SUB_INDEX, (uint16)pressureSensorPS4_gst.rawT_f32 * 100u);
}


static void ProcessBoardPumpSpeedDataOUT(uint32 pmpSpeed_u32)
{
	NmsCanPutObj_u16(OD_ENTRY_PU_GRUNDFOS_PUMP_CONTROL_INDEX, OD_ENTRY_PU_GRUNDFOS_PUMP_FEEDBACK_SUB_INDEX, (uint16)grundfosPMP_gst.rawQ_f32);
}