/*
 * InitSequence.c
 *
 *  Created on: Feb 12, 2019
 *      Author: shlomo
 */
#include <Container.h>
#include <DataDef.h>
#include "include.h"

#include  <PMR/Diagnostics/EventType.pb-c.h>
#include "PMR/MachineStatus/MachineStatus.pb-c.h"
#include "Modules/General/MachineStatus.h"
#include "PMR/Power/StartPowerUpRequest.pb-c.h"
#include "PMR/Power/AbortPowerUpRequest.pb-c.h"
#include "PMR/Power/StartPowerUpResponse.pb-c.h"
#include "PMR/Power/AbortPowerUpResponse.pb-c.h"
#include "PMR/Power/PowerUpState.pb-c.h"

#include "modules/General/GeneralHardware.h"
#include "modules/General/Safety.h"
#include "modules/thread/thread.h"
#include "modules/ids/ids.h"
#include "modules/control/control.h"
#include "Modules/Control/MillisecTask.h"
#include "modules/AlarmHandling/AlarmHandling.h"
#include "modules/heaters/heaters_ex.h"
#include "modules/Diagnostics/Diagnostics.h"
#include "Modules/General/process.h"
#include "modules/General/process.h"

#include "StateMachines/Printing/PrintingSTM.h"
#include "StateMachines/Initialization/PowerIdle.h"
#include "InitSequence.h"

#include "drivers/I2C_Communication/DAC/Blower.h"
#include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
#include "drivers/adc_sampling/adc.h"
#include "drivers/Valves/Valve.h"
#include "drivers/Flash_ram/MCU_E2Prom.h"

#include <Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/MAX11614_Driver.h>
#include <Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/WHS_MAX11614_A2D.h>
#include <Drivers/I2C_Communication/WHS_Card/D_EMC2302_Fan/WHS_Fan.h>
#include <Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.h>
#include <Drivers/I2C_Communication/WHS_Card/D_AD5272_Rheostat/WHS_Rheostat.h>

#include "drivers/Heater/TemperatureSensor.h"

#include "heaters/heaters_ex.h"
#include <Drivers/I2C_Communication/I2C_Task.h>

    typedef enum
    {
        INIT_SEQUENCE_INIT,
        INIT_SEQUENCE_RESET_REASON,
        INIT_SEQUENCE_BUILT_IN_TEST,
        INIT_SEQUENCE_INITIAL_BLOWER_ACTIVATION,
        //INIT_SEQUENCE_DISPENSER_PRESSURE_BUILDUP_TEST,
        INIT_SEQUENCE_POWER_MANAGEMENT_INIT,
        INIT_SEQUENCE_WAIT_FOR_COOLER,
        INIT_SEQUENCE_THREAD_DETECTION,
        INIT_SEQUENCE_START_HEATING,
        INIT_SEQUENCE_MACHINE_READY_TO_DYE,
        INIT_SEQUENCE_END,
    }INIT_SEQUENCE_STAGES_ENUM;
    INIT_SEQUENCE_STAGES_ENUM InitStages = INIT_SEQUENCE_INIT, StoredInitStages = INIT_SEQUENCE_INIT;
    MACHINE_STATE_STAGES_ENUM MachineStateDetail = MACHINE_STATE_INIT;


uint32_t HWControlId,InitSchedulerControlId;
uint32_t MidTankControlId;

uint32_t RESET_Cause = 0;

void InitSequenceResetReason(void);
uint32_t InitSequenceBuiltInTest(void);
uint32_t InitSequenceInitialBlowerActivation(void);
uint32_t InitSequenceDispenserPressureBuildUpTest(void);
uint32_t InitSequenceThreadDetection(void);
uint32_t InitSequenceStartHeating(void);
uint32_t InitSequenceMachineReadyToDye(void);

uint32_t InitSequenceStateMachine( INIT_SEQUENCE_STAGES_ENUM ReadValue);
uint32_t PowerUpUpdate(MACHINE_STATE_STAGES_ENUM stage);
int InitFailures = 0;

MACHINE_STATE_STAGES_ENUM GetMachineState(void)
{
    return MachineStateDetail;
}
void SetMachineState(MACHINE_STATE_STAGES_ENUM NewState)
{
    if (MachineStateDetail != NewState)
    {
        MachineStateDetail = NewState;
        PowerUpUpdate(NewState);
    }
}
void StopInitSequence(void)
{
    InitStages = INIT_SEQUENCE_END;
    InitSequenceStateMachine(InitStages);
}
uint32_t InitSequenceCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
{
    MACHINE_STATE_STAGES_ENUM status;
    if (SafeRemoveControlCallback(HWControlId, InitSequenceCallBackFunction )==OK)
        HWControlId = 0xFF;
    else
        Report("Remove control callback failed",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)InitSequenceCallBackFunction,0);

    MCU_E2PromProgram(EEPROM_INIT_FAILURE_COUNTER,InitFailures+1);

    /*if (InitFailures >= REPEATED_INIT_FAILURE_LIMIT+3)
    {
        InitFailures = 0;
        MCU_E2PromProgram(EEPROM_INIT_FAILURE_COUNTER,0);
    }*/
    //if (InitFailures > REPEATED_INIT_FAILURE_LIMIT)
    //{
        Report("HWConfigurationInit repeated failure",__FILE__,__LINE__,(int)InitFailures,RpWarning,(int)EEPROM_INIT_FAILURE_COUNTER,0);
        /*SetMachineState(MACHINE_STATE_HW_CONFIG_FAILED);
        AlarmHandlingSetAlarm (EVENT_TYPE__MACHINE_STATE_HW_CONFIG_FAILED,ON);
        SetMachineStatus(MACHINE_STATE__Error);
        return ERROR;*/
    //}
    status = (MACHINE_STATE_STAGES_ENUM)HWConfigurationInit();
    if (status == OK)
    {
        //ActivateHeadMagnet();
        InitStages++;
        //InitSequenceStateMachine(InitStages);
    }
    else
    {
        Report("HWConfigurationInit failed",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)InitSequenceCallBackFunction,0);
        SetMachineState(status);
        AlarmHandlingSetAlarm (EVENT_TYPE__MACHINE_STATE_HW_CONFIG_FAILED,ON);
        //SetMachineStatus(MACHINE_STATE__Error);
    }
        return OK;
}
/*******************************************************************************************************/
uint32_t InitScheduler(uint32_t IfIndex, uint32_t BusyFlag)
{
    if (InitStages > StoredInitStages)
    {
        StoredInitStages = InitStages;
        InitSequenceStateMachine(InitStages);
    }
    //PowerUpUpdate(MachineStateDetail);
    return OK;
}
/*******************************************************************************************************/

uint32_t Start_InitSequence(void)
{
    SetMachineState(MACHINE_STATE_HW_CONFIG);
    SetMachineStatus(MACHINE_STATE__PowerUp);
    HWControlId = AddControlCallback("Init HW Init", InitSequenceCallBackFunction, 4* eOneSecond, TemplateDataReadCBFunction,0,0, 0 );
    Report("AddControlCallback INIT",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)InitStages,0);
    InitSchedulerControlId = AddControlCallback("InitScheduler", InitScheduler, eOneSecond, TemplateDataReadCBFunction,0,0, 0 );
    return OK;
}
uint32_t ResetReason = 0;
void InitSequenceResetReason(void)
{
    ResetReason = ReportResetReason();
    InitStages++;
    //InitSequenceStateMachine(InitStages);

    //return OK;
}
void InitSequenceBuiltInTestCallBack(uint32_t IfIndex, uint32_t BusyFlag)
{
    if (1)//BIT OK
    {
        InitStages++;
        //InitSequenceStateMachine(InitStages);
        AlarmHandlingSetAlarm(EVENT_TYPE__POWER_UP_BIT_FAILURE,false);
    }
    else
    {
        SetMachineState(MACHINE_STATE_BUILT_IN_FAILED);
        AlarmHandlingSetAlarm(EVENT_TYPE__POWER_UP_BIT_FAILURE,true);
    }
    //return OK;
}
uint32_t InitSequenceBuiltInTest(void)
{
    SetMachineState(MACHINE_STATE_BUILT_IN_TEST);
    InitSequenceBuiltInTestCallBack(0,0);
    return OK;
}
int MidTankOperationCounter = 0;
uint32_t InitSequenceMidTankCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
{
    int MidTankOpenAir = 8;
    int MidTankReadPressure = 16;
    int MidTankCloseAir = 40;
    int MidTankEnd = 48;
    int portId;

    if (MidTankOperationCounter >= MidTankEnd)
    {
        SafeRemoveControlCallback(MidTankControlId, InitSequenceMidTankCallBackFunction);
    }
    else if (MidTankOperationCounter >= MidTankCloseAir)
    {
        //close air valve for midtank (MidTankOperationCounter-MidTankCloseAir)
        portId = (MidTankOperationCounter - MidTankCloseAir) ; //0-7
        Disable_MidTank_Pressure_Reading(portId);
        Valve_Set(IDS_Id_to_AirValve[portId], Atm_MidTank_OFF );   //Atm_MidTank_OFF/ON
        //Control3WayValvesWithCallback ((Valves_t)portId, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
    }
    else if (MidTankOperationCounter >= MidTankReadPressure)
    {
        //read pressure for midtank (MidTankOperationCounter-MidTankCloseAir)
        portId = MidTankOperationCounter%MAX_SYSTEM_DISPENSERS;
        if((IfIndex==0)||(Get_MidTank_Pressure_Sensor(portId)<0.5))
        {
            Read_MidTank_Pressure_Sensor(portId);
        }
    }
    else if (MidTankOperationCounter >= MidTankOpenAir)
    {
        //open air valve for midtank (MidTankOperationCounter-MidTankOpenAir)
        portId = (MidTankOperationCounter - MidTankOpenAir) ; //0-7
        if((IfIndex==0)||(Get_MidTank_Pressure_Sensor(portId)<0.5))
        {
            Enable_MidTank_Pressure_Reading(portId);
            Valve_Set(IDS_Id_to_AirValve[portId], Atm_MidTank_ON );   //Atm_MidTank_OFF/ON
            Control3WayValvesWithCallback ((Valves_t)portId, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
        }
    }
    MidTankOperationCounter++;
    return OK;
}
uint32_t InitSequenceBlowerCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
{

    if (SafeRemoveControlCallback(HWControlId, InitSequenceBlowerCallBackFunction )==OK)
        HWControlId = 0xFF;
    else
        Report("Remove control callback failed",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)InitSequenceBlowerCallBackFunction,0);


    Turn_the_Blower_On();//Turn on with the Default_Voltage
    if (BlowerCfg.heatingvoltage)
        Control_Voltage_To_Blower(BlowerCfg.heatingvoltage);
    else
        Control_Voltage_To_Blower(3000);

    WHS_enable_control_loop(true);
    WHS_Set_SetPoint_Q_value(headairflow);
    //AlarmHandlingSetAlarm (EVENT_TYPE__MACHINE_STATE_INITIAL_BLOWER_FAILED,OFF);  //handle alarm detection and operation

    waste_seq_step1();// include 1Sec delay           <- to open !!!!


    Safety_Init();
    InitStages++;
    //InitSequenceStateMachine(InitStages);
    return OK;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////
#define MAX_CURRENT_READING 5
#define CURRENT_READING_ERROR_UNSTABLE 100
double Heater_Current[MAX_CURRENT_READING] ;
bool initial_wait = false;
int count_Heater_Current = 0;
int Maxcount_Heater_Current = 0;
bool MainHeaterStable = false,SecondaryHeaterStable = false;
double MainCurrent,SecondaryCurrent,StableCurrent;
double InitDrierAcVoltage = 0.0;
int InitDrierAcVoltageCount = 0;
double Zone2Resistance = 0.0;
double Zone1Resistance = 0.0;
int WaitForCurrentRaise = 0;//let the heater current raise
void InitCurrentReadingStable(void)
{
    memset(Heater_Current,0,sizeof(Heater_Current));
    initial_wait = false;
    count_Heater_Current = 0;
    Maxcount_Heater_Current = 0;
    StableCurrent = 0.0;
    WaitForCurrentRaise = 0;
}
#define MaxStabilization 10
bool DetectIfCurrentReadingStable(double HeaterCurrent)
{
    bool ret = false;
    int i=0;
    double sum = 0;
    double average = 0;

    Maxcount_Heater_Current++;
    //ReportWithPackageFilter(InitFilter,"DetectIfCurrentReadingStable", __FILE__,GetHeaterState(HEATER_TYPE__DryerMainHeater),(int)(HeaterCurrent*100), RpMessage, Maxcount_Heater_Current, 0);
    Heater_Current[count_Heater_Current++] = HeaterCurrent;
    //if (HeaterCurrent<1)
    //    return false;
    if ( count_Heater_Current >= MAX_CURRENT_READING)
        {
            count_Heater_Current = 0;
            initial_wait = true;
        }
    if (initial_wait == false)
        return false;
    for (i=0;i<MAX_CURRENT_READING;i++)
        sum += Heater_Current[i];
    average = sum/MAX_CURRENT_READING;
    if (fabs(average - HeaterCurrent)<0.2)
    {
        ret = true;
        StableCurrent = average;
    }
    return ret;
}

double GetZone1RMSCurrent(double VAC)
{
    return (VAC/Zone1Resistance);
}
double GetZone2RMSCurrent(double VAC)
{
    return (VAC/Zone2Resistance);
}
uint32_t PowerManagementCallBack(uint32_t IfIndex, uint32_t BusyFlag)
{
    if (WaitForCurrentRaise++ < MaxStabilization)
        return OK;
    ReportWithPackageFilter(InitFilter,"PowerManagementCallBack 1", __FILE__,MainHeaterStable,(int)(SecondaryHeaterStable), RpMessage, Maxcount_Heater_Current, 0);
    ReportWithPackageFilter(InitFilter,"PowerManagementCallBack 2", __FILE__,GetHeaterState(HEATER_TYPE__DryerMainHeater),(int)(GetHeaterState(HEATER_TYPE__DryerSecondaryHeater)), RpMessage, Maxcount_Heater_Current, 0);

    if ((MainHeaterStable == false)&&(Maxcount_Heater_Current<CURRENT_READING_ERROR_UNSTABLE))
    {
        InitDrierAcVoltage += ReadVAC();
        InitDrierAcVoltageCount++;
        ReportWithPackageFilter(InitFilter,"main heater current not stable", __FILE__,GetHeaterState(HEATER_TYPE__DryerMainHeater),(int)(Get_Heaters_Current(HEATER_DRYER_CURRENT_1)*100), RpMessage, Maxcount_Heater_Current, 0);
        MainHeaterStable = DetectIfCurrentReadingStable(Get_Heaters_Current(HEATER_DRYER_CURRENT_1));
        Read_Heaters_Current(HEATER_DRYER_CURRENT_1);
        if (MainHeaterStable == true)
        {
            ReportWithPackageFilter(InitFilter,"main heater current stable", __FILE__,GetHeaterState(HEATER_TYPE__DryerMainHeater),(int)(StableCurrent*100), RpMessage, Maxcount_Heater_Current, 0);
            InitDrierAcVoltage /= InitDrierAcVoltageCount;
            MainCurrent = StableCurrent;
            InitCurrentReadingStable();
            Zone1Resistance = InitDrierAcVoltage/MainCurrent;
            DeActivateHeater(HEATER_TYPE__DryerMainHeater);
            ActivateHeater(HEATER_TYPE__DryerSecondaryHeater);
            ReportWithPackageFilter(InitFilter,"starting secondary heater ", __FILE__,__LINE__,(int)(StableCurrent*100), RpMessage, Maxcount_Heater_Current, 0);
        }
    }
    else
    {
        DeActivateHeater(HEATER_TYPE__DryerMainHeater);
        if (Maxcount_Heater_Current>=CURRENT_READING_ERROR_UNSTABLE)
        {
            ReportWithPackageFilter(InitFilter,"drier heaters current not stable", __FILE__,GetHeaterState(HEATER_TYPE__DryerSecondaryHeater),(int)(Get_Heaters_Current(HEATER_DRYER_CURRENT_2)*100), RpMessage, Maxcount_Heater_Current, 0);
            if (SafeRemoveControlCallback(HWControlId, PowerManagementCallBack )==OK)
                    HWControlId = 0xFF;
            else
                    Report("Remove control callback failed",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)PowerManagementCallBack,0);
            AlarmHandlingSetAlarm(EVENT_TYPE__DRYER_HEATERS_ZONE_1_CURRENT_OUT_OF_RANGE,ON);
            DeActivateHeater(HEATER_TYPE__DryerSecondaryHeater);
            DeActivateHeater(HEATER_TYPE__DryerMainHeater);
            InitStages++;
        }
        else //go to secondary
        {
            if ((SecondaryHeaterStable == false)&&(Maxcount_Heater_Current<CURRENT_READING_ERROR_UNSTABLE))
            {
                SecondaryHeaterStable = DetectIfCurrentReadingStable(Get_Heaters_Current(HEATER_DRYER_CURRENT_2));
                Read_Heaters_Current(HEATER_DRYER_CURRENT_2);
                if (SecondaryHeaterStable == true)
                {
                    ReportWithPackageFilter(InitFilter,"secondary heater current stable", __FILE__,__LINE__,(int)(StableCurrent*100), RpMessage, Maxcount_Heater_Current, 0);
                    SecondaryCurrent = StableCurrent;
                    InitCurrentReadingStable();
                    DeActivateHeater(HEATER_TYPE__DryerSecondaryHeater);
                    //do stuff for RMS 2
                    Zone2Resistance = InitDrierAcVoltage/SecondaryCurrent;
                }
            }
            else
            {
                if (SafeRemoveControlCallback(HWControlId, PowerManagementCallBack )==OK)
                        HWControlId = 0xFF;
                else
                        Report("Remove control callback failed",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)PowerManagementCallBack,0);
                DeActivateHeater(HEATER_TYPE__DryerSecondaryHeater);
                DeActivateHeater(HEATER_TYPE__DryerMainHeater);
                if (Maxcount_Heater_Current>=CURRENT_READING_ERROR_UNSTABLE)
                    AlarmHandlingSetAlarm(EVENT_TYPE__DRYER_HEATERS_ZONE_2_CURRENT_OUT_OF_RANGE,ON);
                InitStages++;
           }
        }
    }
return OK;
}
uint32_t InitSequencePowerManagementInit(void)
{
    //if ((Head_Type >= HEAD_TYPE_SYLKO)&&(WHS_Type == WHS_TYPE_NEW))
    {
        if (HWControlId == 0xFF)
        {
            InitCurrentReadingStable();
            ActivateHeater(HEATER_TYPE__DryerMainHeater);
            Report("AddControlCallback INIT",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)InitStages,0);
            HWControlId = AddControlCallback("Init Power", PowerManagementCallBack, eHundredMillisecond, TemplateDataReadCBFunction,0,0, 0 );
            ReportWithPackageFilter(InitFilter,"starting main heater ", __FILE__,__LINE__,(int)(StableCurrent*100), RpMessage, Maxcount_Heater_Current, 0);
        }
        else
            ReportWithPackageFilter(InitFilter,"cannot start InitSequencePowerManagementInit", __FILE__,__LINE__,(int)(StableCurrent*100), RpMessage, Maxcount_Heater_Current, 0);
    }
   // else
   //     InitStages++;
    return OK;
}
uint32_t InitSequenceWHSAirFlowCalibration(void)
{
    int i,total1=0,total2=0,total3=0;
    uint32_t ZeroValue;
    uint16_t value = 0;

    if (ResetReason  & SYSCTL_CAUSE_POR)
    {
        for (i=0;i<8;i++)
        {
            Trigger_WHS_MAX11614_Read_allADC();
            Task_sleep (100);
            get_orf_1(&value);
            total1+=value;
            get_orf_2(&value);
            total2+=value;
            get_orf_3(&value);
            total3+=value;
            ReportWithPackageFilter(InitFilter,"load initial orifice value", __FILE__,total1,total2, RpMessage, total3, 0);
            Task_sleep(100);
        }
        total1/=8;
        total2/=8;
        total3/=8;
        MCU_E2PromRead(EEPROM_ORIFICE1_ZERO_VALUE,&ZeroValue);
        if ((total1>800)&&(total1<2000))
        {
            MCU_E2PromProgram(EEPROM_ORIFICE1_ZERO_VALUE,total1);
        }
        else
            total1 = ZeroValue;
        ReportWithPackageFilter(InitFilter,"store initial orifice 1 value", __FILE__,EEPROM_ORIFICE1_ZERO_VALUE,total1, RpMessage, ZeroValue, 0);
        MCU_E2PromRead(EEPROM_ORIFICE3_ZERO_VALUE,&ZeroValue);
        if ((total3>1000)&&(total3<1500))
        {
            MCU_E2PromProgram(EEPROM_ORIFICE3_ZERO_VALUE,total3);
        }
        else
            total3 = ZeroValue;
        ReportWithPackageFilter(InitFilter,"store initial orifice 3 value", __FILE__,EEPROM_ORIFICE3_ZERO_VALUE,total3, RpMessage, ZeroValue, 0);
        WHS_MAX11614_Load_OrificeZeroValue(total1,total2,total3);
    }
    else
    {
        MCU_E2PromRead(EEPROM_ORIFICE1_ZERO_VALUE,&total1);
        MCU_E2PromRead(EEPROM_ORIFICE3_ZERO_VALUE,&total3);
        ReportWithPackageFilter(InitFilter,"load initial values from EEPROM", __FILE__,0,total3, RpMessage, total1, 0);
        WHS_MAX11614_Load_OrificeZeroValue(total1,total1,total3);
    }
    return OK;

}

uint32_t InitSequenceInitialBlowerActivation(void)
{
    WHS_enable_control_loop(false);
    Turn_the_Blower_On();//Turn on with the Default_Voltage
    Control_Voltage_To_Blower(1); //stop the blower
    HeadBlowersInit();
    PowerIdleInit();

	Task_sleep (1000);
	
    if (WHS_Type == WHS_TYPE_NEW)
    {
        InitSequenceWHSAirFlowCalibration();
    }

    Voc_Sensor_Zero_Calibration();
    SetMachineState(MACHINE_STATE_INITIAL_BLOWER_ACTIVATION);

    Control_Voltage_To_Blower(4200);
    Set_All_WHS_Fans(200);
    Report("AddControlCallback INIT",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)InitStages,0);
    HWControlId = AddControlCallback("Init Blower", InitSequenceBlowerCallBackFunction, 10* eOneSecond, TemplateDataReadCBFunction,0,0, 0 );
    /*if (RdInkCartridgeSensor()) //if there is a cartridge in the ink slot skip the valves procedure
    {
        MidTankOperationCounter = 32;
        Report("There is a cartridge in the ink slot. skipping the valves procedure",__FILE__,__LINE__,(int)MidTankOperationCounter,RpWarning,(int)InitStages,0);
    }*/
    MidTankControlId = AddControlCallback("Init Midtank", InitSequenceMidTankCallBackFunction, 300/*eHundredMillisecond*/, TemplateDataReadCBFunction,0,0, 0 );

    return OK;
}
int NumOfCheckedDispnsers = 0;
uint32_t InitSequenceDispenserPressureBuildUpTestCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
{
    NumOfCheckedDispnsers--;
    if(NumOfCheckedDispnsers==0)
    {
        InitStages++;
        //InitSequenceStateMachine(InitStages);
        SetMachineState(MACHINE_STATE_DISPENSER_PRESSURE_BUILDUP_TEST);

    }
    return OK;
}
uint32_t InitSequenceDispenserPressureBuildUpTest(void)
{
    int Dispenser_i;
    Report("InitSequenceDispenserPressureBuildUpTest",__FILE__,__LINE__,(int)0,RpWarning,(int)0,0);

    for (Dispenser_i = 0;Dispenser_i<MAX_SYSTEM_DISPENSERS;Dispenser_i++)
    {
        if (isMotorConfigured(Dispenser_i + HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1)==true)
        {
            //IDS_Dispenser_Build_Pressure(Dispenser_i,InitSequenceDispenserPressureBuildUpTestCallBackFunction);
            Report("IDS_Dispenser_Build_Pressure",__FILE__,__LINE__,(int)Dispenser_i,RpWarning,(int)Dispenser_i,0);
            NumOfCheckedDispnsers++;
        }
    }


   return OK;
}
uint32_t InitSequenceWaitForCoolerCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
{
    Report("InitSequenceWaitForCoolerCallBackFunction",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)MachineStateDetail,0);

    if (SafeRemoveControlCallback(HWControlId, InitSequenceWaitForCoolerCallBackFunction )==OK)
            HWControlId = 0xFF;
    else
            Report("Remove control callback failed",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)InitSequenceWaitForCoolerCallBackFunction,0);
    if (InitStages == INIT_SEQUENCE_WAIT_FOR_COOLER)
    {
        InitStages++;
        SetMachineState(MACHINE_STATE_WAIT_FOR_COOLER);
        //SetMachineStatus(MACHINE_STATE__Ready); //prevent job while waiting for the cooler
    }
    else
        Report("InitSequenceWaitForCoolerCallBackFunction called too late",__FILE__,__LINE__,(int)InitStages,RpWarning,(int)MachineStateDetail,0);
    MidTankControlId = AddControlCallback("Init Midtank", InitSequenceMidTankCallBackFunction, 300/*eHundredMillisecond*/, TemplateDataReadCBFunction,1,0, 0 );

    return OK;
}
uint32_t InitSequenceWaitForCooler(void)
{
    Trigger_SetWHSFanSpeed(FAN1, SmallFansCfg);
    Trigger_SetWHSFanSpeed(FAN2, SmallFansCfg);
    Trigger_SetWHSFanSpeed(FAN3, SmallFansCfg);
    Trigger_SetWHSFanSpeed(FAN4, SmallFansCfg);
    Trigger_SetWHSFanSpeed(FAN5, LargeFansCfg);
    Trigger_SetWHSFanSpeed(FAN6, LargeFansCfg);
    if (Head_Type == HEAD_TYPE_ARC) {
        HeadBlowersCfg();
    }
    if (WHS_Type == WHS_TYPE_UNKNOWN)
    {
        InitStages++;
        SetMachineState(MACHINE_STATE_WAIT_FOR_COOLER);
        return OK;
    }
    if (/*(ResetReason  & SYSCTL_CAUSE_SW)||*/(ResetReason  & SYSCTL_CAUSE_WDOG1)||(ResetReason  & SYSCTL_CAUSE_WDOG0))
    {
        //soft-warm reset
        InitStages++;
        SetMachineState(MACHINE_STATE_WAIT_FOR_COOLER);
    }
    else
    {
        SetMachineState(MACHINE_STATE_WAIT_FOR_COOLER);
        //SetMachineStatus(MACHINE_STATE__Error); //prevent job while waiting for the cooler
        //cold reset, wait 2 minutes for cooler start
//        HWControlId = AddControlCallback("Init Cooler", InitSequenceWaitForCoolerCallBackFunction, 2*eOneMinute, TemplateDataReadCBFunction,0,0, 0 );
          HWControlId = AddControlCallback("Init Cooler", InitSequenceWaitForCoolerCallBackFunction, eOneMinute, TemplateDataReadCBFunction,0,0, 0 );
          Report("AddControlCallback INIT",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)InitStages,0);
    }
    //InitSequenceStateMachine(InitStages);
    return OK;
}
uint32_t InitSequenceThreadDetection(void)
{
    InitStages++;
    SetMachineState(MACHINE_STATE_THREAD_DETECTION);
    //InitSequenceStateMachine(InitStages);
    return OK;
}

uint32_t InitSequenceStartHeatingCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
{
    if (HeaterCheckReady()==true)
    {
        SetMachineState(MACHINE_STATE_MACHINE_READY_TO_DYE);
        if (SafeRemoveControlCallback(HWControlId, InitSequenceStartHeatingCallBackFunction )==OK)
            HWControlId = 0xFF;
        else
            Report("Remove control callback failed",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)InitSequenceStartHeatingCallBackFunction,0);
        InitStages++;
        //InitSequenceStateMachine(InitStages);
    }
    return OK;
}
bool InitialHeating = false;
void InitSequenceSetStartHeating(bool StartHeating)
{
    REPORT_MSG(StartHeating,"InitSequence StateMachine InitialHeating set");
    InitialHeating = StartHeating;
}
uint32_t InitSequenceStartHeating(void)
{
    TimerMotors_t MotorId = HARDWARE_MOTOR_TYPE__MOTO_DH_LID;

    SetMachineReadyForHeating(true);
    SetMachineState(INIT_SEQUENCE_START_HEATING);

    if(Head_Type == HEAD_TYPE_ARC) {
        MotorMovetoLimitSwitch(MotorId,1-MotorsCfg[MotorId].directionthreadwize, 30,  Motor_Id_to_LS_IdDown[MotorId], cleaningMotorCBFunction,30000);
    } else if(Head_Type == HEAD_TYPE_FLAT){
        HeadCard_Actuators_Relocate();
    }
    //SetMachineStatus(MACHINE_STATE__Ready); //prevent job while waiting for the cooler
    waste_seq_step1();
    if (InitialHeating)
    {
        if (ProcessParamsInit() ==OK)
        {
            SetMachineState(MACHINE_STATE_HEATING_STARTED);
            Report("AddControlCallback INIT",__FILE__,__LINE__,(int)HWControlId,RpWarning,(int)InitStages,0);
            HWControlId = AddControlCallback("Init Heating", InitSequenceStartHeatingCallBackFunction, 2*eOneSecond, TemplateDataReadCBFunction,0,0, 0 );
            //start heaters with predefined / store process parameters
        }
        else
        {
            SetMachineState(MACHINE_STATE_NO_PROCESS_PARAMS);
		    SetMachineStatus(MACHINE_STATE__Ready);
	        REPORT_MSG(MachineStateDetail,"InitSequence StateMachine no process parameters file");
        }
    }
    else
    {
        REPORT_MSG(InitialHeating,"InitSequence StateMachine InitialHeating is OFF");
        SetMachineStatus(MACHINE_STATE__Ready);
        SetMachineState(MACHINE_STATE_MACHINE_READY_TO_DYE);
        InitStages++;
    }
    if (AutoHoming_Config >= AutoHoming_PowerOn_off )
    {
        int Dispenser_i;
        for ( Dispenser_i = 0;Dispenser_i < MAX_SYSTEM_DISPENSERS;Dispenser_i++)
        {
            if (Dispenser_i!=LUBRICANT_DISPENSER)
                    IDS_HomeDispenser (Dispenser_i, 1000 , NULL);
        }
    }
    //activate second pump for 10 minutes
    PumpActivation(600);
    MCU_E2PromProgram(EEPROM_INIT_FAILURE_COUNTER,0);
    return OK;
}
uint32_t InitSequenceMachineReadyToDye(void)
{
    SetMachineStatus(MACHINE_STATE__Ready);
    InitStages++;
    return OK;
}
uint32_t InitSequenceInitEnd(void)
{
    RemoveControlCallback( InitSchedulerControlId,InitScheduler);
    return OK;
}

uint32_t InitSequenceStateMachine( INIT_SEQUENCE_STAGES_ENUM ReadValue)
{
    REPORT_MSG(ReadValue,"InitSequenceStateMachine");

    switch (ReadValue)
    {
        case INIT_SEQUENCE_INIT:
            SetMachineState(MACHINE_STATE_INIT);
            SetMachineState(MACHINE_STATE_HW_CONFIG);
            Start_InitSequence();
            break;
        case INIT_SEQUENCE_RESET_REASON:
            InitSequenceResetReason();
            break;
        case INIT_SEQUENCE_BUILT_IN_TEST:
            InitSequenceBuiltInTest();
            break;
        case INIT_SEQUENCE_INITIAL_BLOWER_ACTIVATION:
            InitSequenceInitialBlowerActivation();
            break;
        case INIT_SEQUENCE_POWER_MANAGEMENT_INIT:
            InitSequencePowerManagementInit();
            break;
        /*case INIT_SEQUENCE_DISPENSER_PRESSURE_BUILDUP_TEST:
            InitSequenceDispenserPressureBuildUpTest();
            break;*/
        case INIT_SEQUENCE_WAIT_FOR_COOLER:
            InitSequenceWaitForCooler();
            break;
        case INIT_SEQUENCE_THREAD_DETECTION:
            InitSequenceThreadDetection();
            break;
        case INIT_SEQUENCE_START_HEATING:
            InitSequenceStartHeating();
            break;
        case INIT_SEQUENCE_MACHINE_READY_TO_DYE:
            InitSequenceMachineReadyToDye();
            break;
        case INIT_SEQUENCE_END:
            InitSequenceInitEnd();
            break;
        default:
            LOG_ERROR(ReadValue,"ERROR IN INIT STATE MACHINE");
            RemoveControlCallback( InitSchedulerControlId,InitScheduler);
            break;
    }
return OK;
}
char PowerUpToken[36+1] = {0};
//char init_string[100];
uint32_t PowerUpLoadControlId = 0xFF;
uint32_t PowerUpUpdateCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
{

    PowerUpUpdate(MachineStateDetail);
    return OK;
}
uint32_t PowerUpUpdate(MACHINE_STATE_STAGES_ENUM stage)
{
    uint32_t status = NOT_SUPPORTED;

    double calculate;
    int result = 0;
    MessageContainer responseContainer;
    ErrorCode Error = ERROR_CODE__NONE;

    StartPowerUpResponse response = START_POWER_UP_RESPONSE__INIT;
    bool last = false;

    if (PowerUpToken[0] == 0)
        return OK;
    //Report("PowerUpUpdate",__FILE__,last,InitStages,RpWarning,(int)stage,0);
    response.has_state = true;
    response.has_progresspercentage = true;

    switch (stage)
    {
        case MACHINE_STATE_INIT:
            response.state = POWER_UP_STATE__Init;
            response.progresspercentage = 0.0;
            response.message = "Initializing";
            break;
        case MACHINE_STATE_NO_CFG_FILE:
            response.state = POWER_UP_STATE__HwConfig;
            response.progresspercentage = 0.0;
            response.message = "Initializing - Waiting for HW Version";
            Error = ERROR_CODE__POWER_UP_NO_CFG_FILE;
            last = true;
            break;
        case MACHINE_STATE_HW_CONFIG:
            response.state = POWER_UP_STATE__HwConfig;
            response.progresspercentage = 10.0;
            response.message = "Hardware Initialization";
            break;
        case MACHINE_STATE_HW_CONFIG_FAILED:
            response.state = POWER_UP_STATE__HwConfig;
            response.progresspercentage = 10.0;
            response.message = "Hardware Initialization failed";
            Error = ERROR_CODE__POWER_UP_HW_CONFIG_FAILED;
            last = true;
            break;
        case MACHINE_STATE_BUILT_IN_FAILED:
            response.state = POWER_UP_STATE__BuiltInTest;
            response.progresspercentage = 15.0;
            response.message = "Built In Test failed";
            Error = ERROR_CODE__POWER_UP_BUILT_IN_FAILED;
            break;
        case MACHINE_STATE_BUILT_IN_TEST:
            response.state = POWER_UP_STATE__BuiltInTest;
            response.progresspercentage = 15.0;
            response.message = "Built In Test";
            break;
        case MACHINE_STATE_INITIAL_BLOWER_ACTIVATION:
            response.state = POWER_UP_STATE__InitialBlowerActivation;
            response.progresspercentage = 18.0;
            response.message = "Initial blower activation";
            break;
        case MACHINE_STATE_INITIAL_BLOWER_FAILED:
            response.state = POWER_UP_STATE__InitialBlowerActivation;
            response.progresspercentage = 18.0;
            response.message = "Initial blower activation failed";
            Error = ERROR_CODE__POWER_UP_BUILT_IN_FAILED;
            break;
        case MACHINE_STATE_WAIT_FOR_COOLER:
            response.state = POWER_UP_STATE__WaitForCooler;
            response.progresspercentage = 25.0;
            response.message = "wait for cooler";
            break;
        case MACHINE_STATE_DISPENSER_PRESSURE_BUILDUP_TEST:
            response.state = POWER_UP_STATE__DispenserPressureBuildupTest;
            response.progresspercentage = 28.0;
            response.message = "pressure buildup";
            break;
        case MACHINE_STATE_DISPENSER_PRESSURE_BUILDUP_FAILED:
            response.state = POWER_UP_STATE__DispenserPressureBuildupTest;
            response.progresspercentage = 28.0;
            response.message = "pressure buildup failed";
            Error = ERROR_CODE__POWER_UP_BUILT_IN_FAILED;
            break;
        case MACHINE_STATE_THREAD_DETECTION:
            response.state = POWER_UP_STATE__ThreadDetection;
            response.progresspercentage = 30.0;
            response.message = "thread detection";
            break;
        case MACHINE_STATE_NO_THREAD_DETECTED:
            response.state = POWER_UP_STATE__ThreadDetection;
            response.progresspercentage = 30.0;
            response.message = "thread detection failed";
            break;
        case MACHINE_STATE_NO_PROCESS_PARAMS:
            response.state = POWER_UP_STATE__MachineReadyToDye;
            response.progresspercentage = 100.0;
            response.message = "Waiting for heating data";
            Error = ERROR_CODE__POWER_UP_NO_PROCESS_PARAMS;
            last = true;
            break;
        case MACHINE_STATE_HEATING_STARTED:
            response.state = POWER_UP_STATE__HeatingStarted;
            if (dryerzone1temp < 1)
                response.progresspercentage = 30.0;
            else
            {
                calculate = (double)((MillisecGetTemperatures(TEMP_SENSE_ANALOG_DRYER_TEMP1)/100)/(dryerzone1temp))*70.0;
                if (calculate > 0)
                    result = (int)calculate;
                if (result > 70.00)
                    result = 70.0;
                if (HeaterCheckReady()==false)
                    result-=2;
                response.progresspercentage = 30.0 + result;
            }
            //Report("Heating calculation",__FILE__,(int)dryerzone1temp,(int)response.progresspercentage,RpWarning,(int)result,0);
            response.message = "Heating up";
            break;
        case MACHINE_STATE_MACHINE_READY_TO_DYE:
            response.state = POWER_UP_STATE__MachineReadyToDye;
            response.progresspercentage = 100.0;
            response.message = "Ready to dye";
            last = true;
            break;
        default:
            response.state = POWER_UP_STATE__Cancelled;
            response.progresspercentage = 0.0;
            response.message = "unknown state";
            Error = ERROR_CODE__POWER_UP_NO_CFG_FILE;
            break;
    }
    //usnprintf(init_string, 80, "PowerUpUpdate token %s",PowerUpToken);
    Report("PowerUpUpdate",__FILE__,InitStages,(int)response.progresspercentage,RpWarning,(int)stage,0);
    responseContainer = createContainer(MESSAGE_TYPE__StartPowerUpResponse, PowerUpToken, last, &response, &start_power_up_response__pack, &start_power_up_response__get_packed_size);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    if (Error != ERROR_CODE__NONE)
    {
        responseContainer.has_error = true;
        responseContainer.error = Error;
        responseContainer.errormessage = response.message;
    }
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);

    SendChars((char*)container_buffer, container_size);
    //MessageContainer responseContainer;

    if (last == true)
    {
        if (PowerUpLoadControlId!= 0xFF)
        {
            RemoveControlCallback(PowerUpLoadControlId, PowerUpUpdateCallBackFunction);
            PowerUpLoadControlId = 0xFF;
            PowerUpStopReporting();
        }
    }
    return status;
}


uint32_t PowerUpUpdateFunc(MessageContainer* requestContainer)
{
    uint32_t status = NOT_SUPPORTED;
    //MessageContainer responseContainer;

//    MachineUpdateInitParams InitParams;

    StartPowerUpRequest* request = start_power_up_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
    ustrncpy (PowerUpToken, requestContainer->token,36);
    //usnprintf(init_string, 80, "PowerUpUpdateFunc token %s",PowerUpToken);
    Report("PowerUpUpdateFunc",__FILE__,__LINE__,(int)0,RpWarning,(int)0,0);
    PowerUpLoadControlId = AddControlCallback("PowerUpLoad", PowerUpUpdateCallBackFunction, 2* eOneSecond, TemplateDataReadCBFunction,0,0, 0 );

    //PowerUpUpdate(MachineStateDetail);
    start_power_up_request__free_unpacked(request,NULL);
    return status;
}
void PowerUpStopReporting(void)
{
    PowerUpToken[0] = 0;
}
uint32_t ctlId;
uint32_t AbortPowerUpResetCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
{
    SafeRemoveControlCallback(ctlId, AbortPowerUpResetCallBackFunction );
    /*volatile short *ptr = (volatile short *)(0x60000800 | 0x3D0);
    *ptr =  0;*/
    Power_Reset();
    return OK;
}

uint32_t AbortPowerUpFunc(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    AbortPowerUpRequest* request = abort_power_up_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    AbortPowerUpResponse response = ABORT_POWER_UP_RESPONSE__INIT;

//TODO Handle the request!!!!
    //PowerUpStopReporting();
    responseContainer = createContainer(MESSAGE_TYPE__AbortPowerUpResponse, requestContainer->token, false, &response, &abort_power_up_response__pack, &abort_power_up_response__get_packed_size);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);

    SendChars((char*)container_buffer, container_size);
    //PowerOffMachineState = POWER_OFF_CANCELLED;
    //PowerUpUpdate(POWER_OFF_CANCELLED);

    ctlId = AddControlCallback("Abort", AbortPowerUpResetCallBackFunction, 2* eOneSecond, TemplateDataReadCBFunction,0,0, 0 );
    Report("HWSystemResetRequest", __FILE__,__LINE__,0, RpMessage, 2, 0);


return OK;
}