/**
 * @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               120000uLL
#define PU_PUMP_MAX_SPEED                           10u
#define PU_PUMP_MIN_SPEED                           4u
#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);
static void ProcessBoardGrundfosPumpHandler(void);
/******************************************************************************
 * 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(void)
{
	AnalogMesaurementRun();

	/* 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);
	PressureSensorGetVal(&pressureSensorPS4_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();
}


/******************************************************************************
 * Private function definitions
 ******************************************************************************/
static void ProcessBoardGrundfosPumpHandler(void)
{
    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);
    }

    GrundfosPmpEnable(PU_PMP_ENABLE);

//    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(9u);

    /* 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)(pressureSensorPS3_gst.rawT_f32 * 100.0f));
}


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