/************************************************************************************************************************
 * Heaters_print.c
 **************************************************************************************************************************/

////////////////////////////////State machine operation////////////////////////////////////
//the state machine operation is used to operate in runtime correct profile flow execution
//by recieved esign flow of the user from the UI
///////////////////////////////////////////////////////////////////////////////////////////
#include "include.h"
#include <DataDef.h>
#include <math.h>
#include "heaters.h"
#include "Heaters_ex.h"

#include "PMR/Hardware/HardwarePidControlType.pb-c.h"
#include "PMR/Hardware/HardwarePidControl.pb-c.h"
#include "PMR/Hardware/HardwareBlower.pb-c.h"
/*#include "PMR/Diagnostics/SetHeaterStateRequest.pb-c.h"
#include "PMR/Diagnostics/SetHeaterStateResponse.pb-c.h"
#include "PMR/Diagnostics/HeaterType.pb-c.h"
#include "PMR/Diagnostics/HeaterState.pb-c.h"*/
#include "PMR/common/MessageContainer.pb-c.h"
#include "PMR/Diagnostics/EventType.pb-c.h"

#include "../AlarmHandling/AlarmHandling.h"
#include "../control/control.h"
#include "../control/pidalgo.h"
#include "../control/MillisecTask.h"
#include "../Ids/Ids_ex.h"
#include "../General/process.h"
#include "../Waste/Waste_ex.h"

#include <ti/sysbios/knl/Task.h>
#include <driverlib/timer.h>
#include <inc/hw_ints.h>
#include <inc/hw_memmap.h>

#include "Drivers/Heater/Heater.h"
#include "Drivers/Heater/TemperatureSensor.h"
#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
#include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
#include "drivers/I2C_Communication/Head_Card/ADC/Head_ADC.h"
#include "Drivers/I2C_Communication/I2C.h"
#include <Drivers/I2C_Communication/I2C_Task.h>
#include <Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.h>
#include "drivers/Flash_ram/MCU_E2Prom.h"
#include "StateMachines/Initialization/InitSequence.h"

/******************** Data Structures  ********************************************/
#define MIN_ALLOWED_PWM 0
#define MAX_ALLOWED_PWM 255
#define ARC_ALARM_LIMIT 3
#define V0_INIT_STOP_FAN_TIME 5

#define PRESSURE_SENSOR_CP      (0.242)
#define PRESSURE_SENSOR_B       (-0.134)
#define V0_DEFAULT_VALUE        1050
#define FAN_MIN_RPM_THRESHOLD   4000
extern uint16_t Head_Fan_Tach[2];
extern uint8_t Head_Fan_PWM_Command[2];

uint32_t    PressureSensorV0[2] = {1100, 1100};
uint8_t     HeadBlowersCloseLoopTime[2] = {2, 2};
uint8_t     HeadBlowersEnable = 1;
uint8_t     BlowerIdToSensorId[2] = {HEAD_PT100_ZONE_7_0X86_0, HEAD_PT100_ZONE_5_0X84_0}; //HEAD_FAN_RIGHT, HEAD_FAN_LEFT
uint32_t HeadBlowersInitControlId = 0xFF;
uint32_t HeadBlowersV0InitCount = 0;
uint32_t HeadBlowersV0InitInProgress = 0;
float PressureSensor_CP = PRESSURE_SENSOR_CP;
float PressureSensor_B = PRESSURE_SENSOR_B;

typedef struct
{
    bool                m_isEnabled;
    float               m_SetParam;
    float               m_mesuredParam;
    float               m_preError;
    float               m_integral;
    float               m_calculatedError;
    bool                m_isReady;
    int                 SamplesFilterSize;
    PID_Config_Params   m_params;
}HeadBlowerConfig;
HeadBlowerConfig HeadBlowerControl[2] = {0,0};

/******************** FUNCTIONS  ********************************************/
int HeadBlowersGetPWM(uint8_t blowerId);

/********************************************************************************
 * Arc Head Pressure sensors
 *******************************************************************************/

void HeadBlowersInit()
{
    Trigger_Head_Fan_Control(HEAD_FAN_RIGHT, 0);
    Trigger_Head_Fan_Control(HEAD_FAN_LEFT, 0);

    //PID parameters init
    HeadBlowerControl[0].m_params.MAX = 200;
    HeadBlowerControl[0].m_params.MIN = -200;
    HeadBlowerControl[0].m_params.Kd = 0;
    HeadBlowerControl[0].m_params.Kp = 800;
    HeadBlowerControl[0].m_params.Ki = 80;
    HeadBlowerControl[0].m_params.IntegralErrorMultiplier = 10;
    HeadBlowerControl[0].m_params.ProportionalErrorMultiplier = 10;
    HeadBlowerControl[0].m_params.epsilon = 0;
    HeadBlowerControl[0].m_params.dt = 2;
    //HeadBlowerControl[0].m_ingnoreValue =  PID_Request->sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
    HeadBlowerControl[0].m_calculatedError = 0;
    HeadBlowerControl[0].m_integral = 0;
    HeadBlowerControl[0].m_isEnabled = true;
    HeadBlowerControl[0].m_isReady = true;
    HeadBlowerControl[0].m_mesuredParam = 0;
    HeadBlowerControl[0].m_preError = 0;
    HeadBlowerControl[0].m_SetParam = 5.0;//need to update SetParams on presegment stage

    HeadBlowerControl[1].m_params.MAX = 200;
    HeadBlowerControl[1].m_params.MIN = -200;
    HeadBlowerControl[1].m_params.Kd = 0;
    HeadBlowerControl[1].m_params.Kp = 800;
    HeadBlowerControl[1].m_params.Ki = 80;
    HeadBlowerControl[1].m_params.IntegralErrorMultiplier = 10;
    HeadBlowerControl[1].m_params.ProportionalErrorMultiplier = 10;
    HeadBlowerControl[1].m_params.epsilon = 0;
    HeadBlowerControl[1].m_params.dt = 2;
    //HeadBlowerControl[0].m_ingnoreValue =  PID_Request->sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
    HeadBlowerControl[1].m_calculatedError = 0;
    HeadBlowerControl[1].m_integral = 0;
    HeadBlowerControl[1].m_isEnabled = true;
    HeadBlowerControl[1].m_isReady = true;
    HeadBlowerControl[1].m_mesuredParam = 0;
    HeadBlowerControl[1].m_preError = 0;
    HeadBlowerControl[1].m_SetParam = 5.0;//need to update SetParams on presegment stage
}

uint32_t HeadBlowerPidRequestMessage(void* request, int BlowerId)
{
    HardwarePidControl * PID_Request = request;

    HeadBlowerControl[BlowerId].m_params.MAX = 200;
    HeadBlowerControl[BlowerId].m_params.MIN = -200;
    HeadBlowerControl[BlowerId].m_params.Kd = PID_Request->derivativetime;
    HeadBlowerControl[BlowerId].m_params.Kp = PID_Request->proportionalgain;
    HeadBlowerControl[BlowerId].m_params.Ki = PID_Request->integraltime;
    HeadBlowerControl[BlowerId].m_params.IntegralErrorMultiplier = PID_Request->setpointramprateorsoftstartramp;
    HeadBlowerControl[BlowerId].m_params.ProportionalErrorMultiplier = PID_Request->outputonoffhysteresisvalue;
    HeadBlowerControl[BlowerId].m_params.epsilon = PID_Request->epsilon;
    HeadBlowerControl[BlowerId].m_params.dt = PID_Request->controloutputtype;
    //HeadBlowerControl.m_ingnoreValue =  PID_Request->sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
    HeadBlowerControl[BlowerId].m_calculatedError = 0;
    HeadBlowerControl[BlowerId].m_integral = 0;
    HeadBlowerControl[BlowerId].m_isEnabled = true;
    HeadBlowerControl[BlowerId].m_isReady = true;
    HeadBlowerControl[BlowerId].m_mesuredParam = 0;
    HeadBlowerControl[BlowerId].m_preError = 0;
    HeadBlowerControl[BlowerId].m_SetParam = PID_Request->outputproportionalcycletime;//need to update SetParams on presegment stage
    return OK;
}

uint32_t HeadBlowerPidFunc(double setParam,double measuredParam, int blowerId)
{
    int calculatedPwm;

    HeadBlowerControl[blowerId].m_mesuredParam = measuredParam;
    HeadBlowerControl[blowerId].m_SetParam = setParam;
    HeadBlowerControl[blowerId].m_calculatedError = PIDAlgorithmCalculation((float)HeadBlowerControl[blowerId].m_SetParam , (float)HeadBlowerControl[blowerId].m_mesuredParam,
                                                      &HeadBlowerControl[blowerId].m_params,   &HeadBlowerControl[blowerId].m_preError, &HeadBlowerControl[blowerId].m_integral);
    calculatedPwm = HeadBlowersGetPWM(blowerId) + HeadBlowerControl[blowerId].m_calculatedError;
    HeadBlowersCloseLoopTime[blowerId] = HeadBlowerControl[blowerId].m_params.dt;

    if (calculatedPwm < MIN_ALLOWED_PWM) {
        //ReportWithPackageFilter(HeatersFilter,"calculatedPwm < MIN_ALLOWED_PWM",__FILE__,__LINE__,calculatedPwm,RpError, MIN_ALLOWED_PWM,0);
        HeadBlowerControl[blowerId].m_integral = 0;
        calculatedPwm = MIN_ALLOWED_PWM;
    }
    if (calculatedPwm > MAX_ALLOWED_PWM) {
        //ReportWithPackageFilter(HeatersFilter,"calculatedPwm > MAX_ALLOWED_PWM",__FILE__,__LINE__,calculatedPwm,RpError, MAX_ALLOWED_PWM,0);
        HeadBlowerControl[blowerId].m_integral = 0;
        calculatedPwm = MAX_ALLOWED_PWM;
    }
    Trigger_Head_Fan_Control(blowerId, calculatedPwm);
    return OK;
}

int HeadBlowerSensorIdToFanId(uint8_t sensorId)
{
    if (sensorId == HEAD_PT100_ZONE_5_0X84_0)
        return HEAD_FAN_RIGHT;
    else
        return HEAD_FAN_LEFT;
}

int HeadBlowerFanIdToBlowerId(uint8_t FanId)
{
    return BlowerIdToSensorId[FanId];
}

bool HeadBlowerFlowControl(double Q_value, uint8_t blowerId)
{
    double currentFlow = 0.0;

    currentFlow = PressureSensorGetPressure(blowerId);
    HeadBlowerPidFunc(Q_value,currentFlow, blowerId);
    return OK;
}

void HeadBlowersOff(int off)
{
    if (off == 1)
        HeadBlowersEnable = 0;
    else
        HeadBlowersEnable = 1;
    ReportWithPackageFilter(HeatersFilter,"HeadBlowersOff",__FILE__,__LINE__,off,RpError, 0,0);
    return;
}

uint32_t HeadBlowersOffGet()
{
    ReportWithPackageFilter(HeatersFilter,"HeadBlowersOffGet",__FILE__,__LINE__,HeadBlowersEnable,RpError, 0,0);
    return HeadBlowersEnable;
}
static int8_t count1 = 0, count2 = 0, count3 = 0, count4 = 0, count5 = 0;
static int8_t count6 = 0, count7 = 0, count8 = 0, count9 = 0, count10 = 0;
void setArcHeadAlarms(uint32_t AlarmId, bool value)
{
    AlarmHandlingSetAlarm(AlarmId, value);
    if (value == true)
    {
        switch(AlarmId)
        {
            /*case EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_STOPPED:
                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count9,RpError, 0,0);
                break;
            case EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_STOPPED:
                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count9,RpError, 0,0);
                break;*/
            case EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_RPM_TOO_LOW:
                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count3,RpError, HeadBlowersGetRPM(HEAD_FAN_RIGHT),0);
                break;
            case EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_RPM_TOO_LOW:
                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count4,RpError, HeadBlowersGetRPM(HEAD_FAN_LEFT),0);
                break;
            case EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_HIGH:
                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count5,RpError, PressureSensorGetPressure(HEAD_FAN_RIGHT),0);
                break;
            case EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_HIGH:
                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count7,RpError, PressureSensorGetPressure(HEAD_FAN_LEFT),0);
                break;
            case EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_LOW:
                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count6,RpError, PressureSensorGetPressure(HEAD_FAN_RIGHT),0);
                break;
            case EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_LOW:
                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count8,RpError, PressureSensorGetPressure(HEAD_FAN_LEFT),0);
                break;
                /*case EVENT_TYPE__DYEING_HEAD_ARC_LID_IS_OPEN:
                    ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count9,RpError, 0,0);
                    break;
                case EVENT_TYPE__DYEING_HEAD_TUNNEL_LID_IS_OPEN:
                    ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count9,RpError, 0,0);
                    break;*/
            case EVENT_TYPE__DYEING_HEAD_COVER_IS_OPEN:
                ReportWithPackageFilter(HeatersFilter,"FlatHeadAlarms on",__FILE__,AlarmId,count9,RpError, 0,0);
                break;
            default:
                break;
        }
    }
}
void FlatHeadAlarms(void)
{
    if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp  [HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) == LIMIT) {
        if (++count9 == ARC_ALARM_LIMIT)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_COVER_IS_OPEN, true);
        count9 = (count9 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count9);
    } else {
        if (--count9 == 0)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_COVER_IS_OPEN, false);
        count9 = (count9 < 0)?(0):(count9);
    }

}
void ArcHeadAlarms(void)
{
    double currentFlow = 0.0;

    if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_ARC) != LIMIT) {
        if (++count9 == ARC_ALARM_LIMIT)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_ARC_LID_IS_OPEN, true);
        count9 = (count9 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count9);
    } else {
        if (--count9 == 0)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_ARC_LID_IS_OPEN, false);
        count9 = (count9 < 0)?(0):(count9);
    }

    if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_TUNNEL_ARC) != LIMIT) {
        if (++count10 == ARC_ALARM_LIMIT)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_TUNNEL_LID_IS_OPEN, true);
        count10 = (count10 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count10);
    } else {
        if (--count10 == 0)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_TUNNEL_LID_IS_OPEN, false);
        count10 = (count10 < 0)?(0):(count10);
    }

    if (Head_Fan_Tach[0] == 0x1FFE) {
        if (++count1 == ARC_ALARM_LIMIT)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_STOPPED, true);
        count1 = (count1 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count1);
    } else {
        if (--count1 == 0)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_STOPPED, false);
        count1 = (count1 < 0)?(0):(count1);
    }
    if (Head_Fan_Tach[1] == 0x1FFE) {
        if (++count2 == ARC_ALARM_LIMIT)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_STOPPED, true);
        count2 = (count2 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count2);
    } else {
        if (--count2 == 0)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_STOPPED, false);
        count2 = (count2 < 0)?(0):(count2);
    }

    if (HeadBlowersGetRPM(HEAD_FAN_RIGHT) < 2000) {
        if (++count3 == ARC_ALARM_LIMIT)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_RPM_TOO_LOW, true);
        count3 = (count3 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count3);
    } else {
        if (--count3 == 0)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_RPM_TOO_LOW, false);
        count3 = (count3 < 0)?(0):(count3);
    }
    if (HeadBlowersGetRPM(HEAD_FAN_LEFT) < 2000) {
        if (++count4 == ARC_ALARM_LIMIT)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_RPM_TOO_LOW, true);
        count4 = (count4 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count4);
    } else {
        if (--count4 == 0)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_RPM_TOO_LOW, false);
        count4 = (count4 < 0)?(0):(count4);
    }

    currentFlow = PressureSensorGetPressure(HEAD_FAN_RIGHT);
    if (currentFlow > 5.0) {
        if (++count5 == ARC_ALARM_LIMIT)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_HIGH, true);
        count5 = (count5 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count5);
    } else {
        if (--count5 == 0)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_HIGH, false);
        count5 = (count5 < 0)?(0):(count5);
    }
    if (currentFlow < 1.0) {
        if (!HeadBlowersV0InitInProgress) {
            if (++count6 == ARC_ALARM_LIMIT)
                setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_LOW, true);
            count6 = (count6 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count6);
        }
    } else {
        if (--count6 == 0)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_LOW, false);
        count6 = (count6 < 0)?(0):(count6);
    }

    currentFlow = PressureSensorGetPressure(HEAD_FAN_LEFT);
    if (currentFlow > 5.0) {
        if (++count7 == ARC_ALARM_LIMIT)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_HIGH, true);
        count7 = (count7 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count7);
    } else {
        if (--count7 == 0)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_HIGH, false);
        count7 = (count7 < 0)?(0):(count7);
    }
    if (currentFlow < 1.0) {
        if (!HeadBlowersV0InitInProgress) {
            if (++count8 == ARC_ALARM_LIMIT)
                setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_LOW, true);
            count8 = (count8 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count8);
        }
    } else {
        if (--count8 == 0)
            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_LOW, false);
        count8 = (count8 < 0)?(0):(count8);
    }
}

void HeadBlowersControlLoop ()
{
    int blowerId;
    static int count[2] = {0, 0};

    if (GetMachineState() < MACHINE_STATE_WAIT_FOR_COOLER)
        return; //do not start before controller is initialized and running

    if (!HeadBlowersEnable) {
        return;
    }

    ArcHeadAlarms();

    for (blowerId = 0; blowerId <= 1; blowerId++) {
        if (count[blowerId] == HeadBlowersCloseLoopTime[blowerId]) {
            count[blowerId] = 0;
            HeadBlowerFlowControl(headBlowersFlow[blowerId], blowerId);
        } else {
            count[blowerId] += 1;
        }
    }
}

void HeadBlowersCfg()
{
    if (HeadBlowerCfg[0].enabled) {
        Trigger_Head_Fan_Control(HEAD_FAN_RIGHT, HeadBlowerCfg[0].voltage);
    }
    if (HeadBlowerCfg[1].enabled) {
        Trigger_Head_Fan_Control(HEAD_FAN_LEFT, HeadBlowerCfg[1].voltage);
    }
}

uint32_t PressureSensorInit(int blowerId)
{
    uint32_t rpm, V0 = 0;

    rpm = HeadBlowersGetRPM(blowerId);
    if (rpm < FAN_MIN_RPM_THRESHOLD) {
        V0 = MillisecGetTemperatures(BlowerIdToSensorId[blowerId]);
        V0 /= 10;
    }
    return V0;
}

int HeadBlowersGetRPM(uint8_t blowerId)
{
    Trigger_Head_Read_Tacho(blowerId);

    if (Head_Fan_Tach[blowerId] == 0x1FFE) {
        return 0;
    } else {
        return 7864320/Head_Fan_Tach[blowerId];
    }
}

int HeadBlowersGetPWM(uint8_t blowerId)
{
    return Head_Fan_PWM_Command[blowerId];
}

double PressureSensorGetPressure(uint8_t FanId)
{
    double V0, Vm, Q;
    int SensorId;

    SensorId = BlowerIdToSensorId[FanId];
    V0 = PressureSensorV0[FanId];
    Vm = (double)(MillisecGetTemperatures(SensorId));
    Vm /= 10.0;
    if ((Vm - V0 + 22)<0)
        return 0.0;
    Q = sqrt(Vm - V0 + 22) * PressureSensor_CP + PressureSensor_B;
    return Q;
}

uint32_t  HeadBlowerCommandRequestMessage(int blowerId, float flow)
{
    if ((blowerId != HEAD_FAN_RIGHT) && (blowerId != HEAD_FAN_LEFT)) {
        ReportWithPackageFilter(HeatersFilter,"blowerId not in range",__FILE__,__LINE__,blowerId,RpError, 0,0);
        return ERROR;
    }
/*
    if (HeadBlowerCfg[blowerId].enabled == false) {
        ReportWithPackageFilter(HeatersFilter,"HeadBlower not configured",__FILE__,__LINE__,blowerId,RpError, 0,0);
        return ERROR;
    }
*/
    if ((flow < 0.0) || (flow > 5.0)) {
        ReportWithPackageFilter(HeatersFilter,"flow not in range",__FILE__, flow, blowerId,RpError, 0,0);
        return ERROR;
    }
    ReportWithPackageFilter(HeatersFilter,"HeadBlower configured",__FILE__,blowerId,(int)(flow*100),RpError, PressureSensorV0[blowerId],0);
    headBlowersFlow[blowerId] = flow;
    return OK;
}

uint32_t HeadBlowerAirflowCalibration(void)
{
    uint32_t V0[2];

    V0[0] = PressureSensorInit(0);
    V0[1] = PressureSensorInit(1);
    ReportWithPackageFilter(HeatersFilter,"read pressure sensor v0[0],v0[1]", __FILE__,__LINE__, (int)V0[0], RpMessage, (int)V0[1], 0);
    if ((V0[0] > 1000) && (V0[0] < 1200)) {
        MCU_E2PromProgram(EEPROM_PRESSURE_SENSOR_V0_0, V0[0]);
        ReportWithPackageFilter(InitFilter,"store pressure sensor v0[0]", __FILE__,__LINE__, (int)V0[0], RpMessage, 0, 0);
    } else {
        MCU_E2PromRead(EEPROM_PRESSURE_SENSOR_V0_0, &V0[0]);
        ReportWithPackageFilter(HeatersFilter,"load from EEPROM", __FILE__,__LINE__, (int)V0[0], RpMessage, 0, 0);
    }
    if ((V0[1] > 1000) && (V0[1] < 1200)) {
        MCU_E2PromProgram(EEPROM_PRESSURE_SENSOR_V0_1, V0[1]);
        ReportWithPackageFilter(HeatersFilter,"store pressure sensor v0[1]", __FILE__,__LINE__, (int)V0[1], RpMessage, 0, 0);
    } else {
        MCU_E2PromRead(EEPROM_PRESSURE_SENSOR_V0_1, &V0[1]);
        ReportWithPackageFilter(HeatersFilter,"load from EEPROM", __FILE__,__LINE__, (int)V0[1], RpMessage, 0, 0);
    }

    if ((V0[0] > 1000) && (V0[0] < 1200)) {
        PressureSensorV0[0] = V0[0];
    }
    if ((V0[1] > 1000) && (V0[1] < 1200)) {
        PressureSensorV0[1] = V0[1];
    }
    return OK;
}

uint32_t HeadBlowersInitCallbak(uint32_t IfIndex, uint32_t ReadValue)
{

    if (HeadBlowersV0InitCount++ >= V0_INIT_STOP_FAN_TIME)
    {
        SafeRemoveControlCallback(HeadBlowersInitControlId,HeadBlowersInitCallbak);
        HeadBlowersInitControlId = 0xFF;
        HeadBlowerAirflowCalibration();
        HeadBlowersOff(0);
        HeadBlowersV0InitInProgress = 0;
        Report("Head Blowers V0 calculated", __FILE__, __LINE__, PressureSensorV0[0], RpMessage, PressureSensorV0[1], 0);
        Waste_Check_Flows();
    }
    Report("Head Blowers calculate V0", __FILE__, __LINE__, 0, RpMessage, 0, 0);
    return OK;
}

uint32_t HeadBlowersV0Init(void)
{
    if (Head_Type != HEAD_TYPE_ARC)
    {
        Waste_Check_Flows();
        return OK;
    }
    if (HeadBlowersInitControlId != 0xFF)
        RemoveControlCallback(HeadBlowersInitControlId,HeadBlowersInitCallbak);

    //set Head Blowers to minimum rpm
    HeadBlowersOff(1);
    Trigger_Head_Fan_Control(HEAD_FAN_RIGHT, 0);
    Trigger_Head_Fan_Control(HEAD_FAN_LEFT, 0);

    //init Head Blowers V0
    HeadBlowersV0InitCount = 0;
    HeadBlowersV0InitInProgress = 1;
    HeadBlowersInitControlId = AddControlCallback("Head Blowers V0 init", HeadBlowersInitCallbak, eOneSecond,  TemplateDataReadCBFunction, 0, 0, 0);
    Report("Head Blowers V0 init", __FILE__, __LINE__, Head_Type, RpMessage, 0, 0);
    return OK;
}

void Set_Head_Blowers_Parameters(float cp,float b)
{
    if (cp)
    {
        PressureSensor_CP = cp;
        Report("Head Blowers CP Coefficient", __FILE__, __LINE__, (int)(PressureSensor_CP * 1000), RpMessage, 0, 0);
    }
    if (b)
    {
        PressureSensor_B = b;
        Report("Head Blowers B Coefficient", __FILE__, __LINE__, (int)(PressureSensor_B * 1000), RpMessage, 0, 0);
    }
}