/************************************************************************************************************************
 * Ids_maint.c
 * Printing module is responsible for :
 * operating the dispensers according to predefined dispensing rate from the UI
 **************************************************************************************************************************/
#include "include.h"
#include "ids.h"
#include "ids_ex.h"
#include "../control/control.h"
#include "../control/pidalgo.h"
#include "../thread/thread.h"
#include "PMR/Hardware/Hardwaremotor.pb-c.h"
#include "PMR/Hardware/HardwareDispenser.pb-c.h"
#include "StateMachines/Printing/printingSTM.h"

#include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
#include "drivers/I2C_Communication/Dispenser_Card/IO_Ports/Dispenser_IO.h"

#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
#include "drivers/FPGA/FPGA_SPI_Comm.h"
#include "drivers/Motors/Motor.h"
#include "drivers/Valves/Valve.h"
#include "Modules/AlarmHandling/AlarmHandling.h"


FPGA_GPI_ENUM     Dispenser_Id_to_LS_Id[MAX_SYSTEM_DISPENSERS] = {
      GPI_LS_DISPENSER_DOWN_1, //MOTO_DISPENSER_1 = 6,
      GPI_LS_DISPENSER_DOWN_2, //MOTO_DISPENSER_2 = 7,
      GPI_LS_DISPENSER_DOWN_3, //MOTO_DISPENSER_3 = 8,
      GPI_LS_DISPENSER_DOWN_4, //MOTO_DISPENSER_4 = 9,
      GPI_LS_DISPENSER_DOWN_5, //MOTO_DISPENSER_5 = 10,
      GPI_LS_DISPENSER_DOWN_6, //MOTO_DISPENSER_6 = 11,
      GPI_LS_DISPENSER_DOWN_7, //MOTO_DISPENSER_7 = 12,
      GPI_LS_DISPENSER_DOWN_8, //MOTO_DISPENSER_8 = 13,
};
FPGA_GPI_ENUM     Dispenser_Id_to_LS_Empty_Id[MAX_SYSTEM_DISPENSERS] = {
      GPI_LS_DISPENSER_UP_1, //MOTO_DISPENSER_1 = 6,
      GPI_LS_DISPENSER_UP_2, //MOTO_DISPENSER_2 = 7,
      GPI_LS_DISPENSER_UP_3, //MOTO_DISPENSER_3 = 8,
      GPI_LS_DISPENSER_UP_4, //MOTO_DISPENSER_4 = 9,
      GPI_LS_DISPENSER_UP_5, //MOTO_DISPENSER_5 = 10,
      GPI_LS_DISPENSER_UP_6, //MOTO_DISPENSER_6 = 11,
      GPI_LS_DISPENSER_UP_7, //MOTO_DISPENSER_7 = 12,
      GPI_LS_DISPENSER_UP_8, //MOTO_DISPENSER_8 = 13,
};
FPGA_GPI_ENUM     Dispenser_Id_to_Alarm_LS_Id[MAX_SYSTEM_DISPENSERS*2] = {
      GPI_LS_DISPENSER_UP_1, //MOTO_DISPENSER_1 = 6,
      GPI_LS_DISPENSER_UP_2, //MOTO_DISPENSER_2 = 7,
      GPI_LS_DISPENSER_UP_3, //MOTO_DISPENSER_3 = 8,
      GPI_LS_DISPENSER_UP_4, //MOTO_DISPENSER_4 = 9,
      GPI_LS_DISPENSER_UP_5, //MOTO_DISPENSER_5 = 10,
      GPI_LS_DISPENSER_UP_6, //MOTO_DISPENSER_6 = 11,
      GPI_LS_DISPENSER_UP_7, //MOTO_DISPENSER_7 = 12,
      GPI_LS_DISPENSER_UP_8, //MOTO_DISPENSER_8 = 13,
      GPI_LS_DISPENSER_25_1, //MOTO_DISPENSER_1 = 6,
      GPI_LS_DISPENSER_25_2, //MOTO_DISPENSER_2 = 7,
      GPI_LS_DISPENSER_25_3, //MOTO_DISPENSER_3 = 8,
      GPI_LS_DISPENSER_25_4, //MOTO_DISPENSER_4 = 9,
      GPI_LS_DISPENSER_25_5, //MOTO_DISPENSER_5 = 10,
      GPI_LS_DISPENSER_25_6, //MOTO_DISPENSER_6 = 11,
      GPI_LS_DISPENSER_25_7, //MOTO_DISPENSER_7 = 12,
      GPI_LS_DISPENSER_25_8, //MOTO_DISPENSER_8 = 13,
};
callback_fptr HomingRequestCallback[MAX_SYSTEM_DISPENSERS]={0,0,0,0,0,0,0,0};
callback_fptr HomingBacklashCallback[MAX_SYSTEM_DISPENSERS]={0,0,0,0,0,0,0,0};
uint32_t HomingBacklashTimeout[MAX_SYSTEM_DISPENSERS];
uint32_t HomingBacklashTime[MAX_SYSTEM_DISPENSERS];

int32_t KeepMicrostep[MAX_SYSTEM_DISPENSERS];

bool HomingActive[MAX_SYSTEM_DISPENSERS] = {false,false,false,false,false,false,false,false};
bool PrimingActive[MAX_SYSTEM_DISPENSERS] = {false,false,false,false,false,false,false,false};
uint32_t DispenserHomingControlId[MAX_SYSTEM_DISPENSERS] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
uint32_t DispenserBacklashControlId[MAX_SYSTEM_DISPENSERS] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
uint32_t DispenserHomingTime[MAX_SYSTEM_DISPENSERS] = {0,0,0,0,0,0,0,0};
#define INITIAL_DISPENSER_PRESSURE 3.85
#define INITIAL_DISPENSER_TIMEOUT_LIMIT 120000
#define INITIAL_DISPENSER_TIMEOUT 100
#define INITIAL_DISPENSER_SPEED 1000

double   InitialDispenserPressure = INITIAL_DISPENSER_PRESSURE;
uint32_t InitialDispenserTimeout = INITIAL_DISPENSER_TIMEOUT_LIMIT;
uint32_t InitialDispenserTimeLag = INITIAL_DISPENSER_TIMEOUT;
uint32_t InitialDispenserSpeed = INITIAL_DISPENSER_SPEED;

uint32_t DispenserHomingTimeoutControlId [MAX_SYSTEM_DISPENSERS] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
uint32_t    ControlIdtoInactiveDispenserId [MAX_SYSTEM_DISPENSERS] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};

//uint32_t IDS_StopHomeDispenserBuildPressure (uint32_t deviceID);

AutoHoming_Config_enum AutoHoming_Config = AutoHoming_off;
void IDS_Dispenser_SetAutoHoming_Config(AutoHoming_Config_enum Config)
{
    ReportWithPackageFilter(IDSFilter,"IDS_Dispenser_SetAutoHoming_Config ",__FILE__,__LINE__,Config,RpWarning,AutoHoming_Config,0);
    AutoHoming_Config = Config;
#ifdef DISPESER_TEST
    AutoHoming_Config = AutoHoming_off;
#endif
}

void IDS_Dispenser_SetBackLashValues(double   initialdispenserpressure,   uint32_t initialdispensertimeout,   uint32_t initialdispensertimelag,   uint32_t initialdispenserspeed)
{
    //if (initialdispenserpressure>2)
        InitialDispenserPressure = initialdispenserpressure;
    //if (initialdispensertimeout>70000)
        InitialDispenserTimeout = initialdispensertimeout;
    InitialDispenserTimeLag = initialdispensertimelag;
    InitialDispenserSpeed = initialdispenserspeed;

    ReportWithPackageFilter(IDSFilter,"IDS_Dispenser_SetBackLashValues ",__FILE__,InitialDispenserPressure*100,InitialDispenserTimeout,RpWarning,(int)InitialDispenserTimeLag,0);
}
uint32_t IDS_HomeDispenserWaitForHomingEndCallback(uint32_t DispenserId, uint32_t ReadValue)
{
    HomingBacklashTime[DispenserId]+=eOneSecond;
    if (( HomingActive[DispenserId]== false) ||((HomingBacklashTime[DispenserId]>=HomingBacklashTimeout[DispenserId])&&(HomingBacklashTimeout[DispenserId]>0)))
       {
           //stop this control loop
           SafeRemoveControlCallback(DispenserBacklashControlId[DispenserId], IDS_HomeDispenserWaitForHomingEndCallback );
           DispenserBacklashControlId[DispenserId] = 0xFF;

           if (HomingBacklashCallback[DispenserId])
           {
               if ( HomingActive[DispenserId]== false)
                   HomingBacklashCallback[DispenserId](DispenserId,OK);
               else
                   HomingBacklashCallback[DispenserId](DispenserId,ERROR);
           }
           HomingBacklashCallback[DispenserId] = 0;
       }
    return OK;
}
uint32_t IDS_HomeDispenserWaitForHomingEnd(uint32_t DispenserId,  uint32_t timeout , callback_fptr callback)
{
    assert(DispenserId < MAX_SYSTEM_DISPENSERS);

    if (HomingBacklashCallback[DispenserId]!=NULL)
        return ERROR;
    HomingBacklashCallback[DispenserId] = callback;
    HomingBacklashTimeout[DispenserId] = timeout;
    HomingBacklashTime[DispenserId] = 0;

    if (DispenserBacklashControlId[DispenserId] != 0xFF)
    {
        RemoveControlCallback(DispenserBacklashControlId[DispenserId], IDS_HomeDispenserWaitForHomingEndCallback );
        DispenserBacklashControlId[DispenserId] = 0xFF;
        //return ERROR;
    }
    DispenserBacklashControlId[DispenserId] = AddControlCallback("Wait for Homing", IDS_HomeDispenserWaitForHomingEnd,eOneSecond , TemplateDataReadCBFunction,DispenserId,0, 0 );
    if ( DispenserBacklashControlId[DispenserId] == 0xFF)
        return ERROR;

    return OK;
}

/*uint32_t IDS_HomeDispenserBackMoveCallback(uint32_t motorId, uint32_t ReadValue)
{
    uint8_t DispenserId = motorId-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
    if ((GetDispenserPressure(DispenserId)>=InitialDispenserPressure)||(DispenserHomingTime[DispenserId]>InitialDispenserTimeout))
    {
        MotorStop(motorId,Hard_Hiz);
        CurrentDispenserSpeed[DispenserId] = 0;
        if (Extended_Motor_Param == true)
            Power_Step_01_Dispenser_Mode(DispenserId,Voltage);
        //MotorSetMicroStep(motorId, KeepMicrostep[DispenserId]);
        HomingActive[DispenserId]= false;
        PrimingActive[DispenserId]= false;
        Control_TCA9534ByPass(DispenserId,DISABLE);// use ENABLE or DISABLE

        //close dry air valve in the dispenser
        Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF);
        Disable_MidTank_Pressure_Reading(DispenserId);

        ReportWithPackageFilter(IDSFilter,"End backlash",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)DispenserHomingTime[DispenserId],0);
        if (SafeRemoveControlCallback(DispenserHomingControlId[DispenserId], IDS_HomeDispenserBackMoveCallback )==OK)
            DispenserHomingControlId[DispenserId] = 0xFF;
        else
            ReportWithPackageFilter(IDSFilter,"Remove control callback failed",__FILE__,__LINE__,(int)DispenserId,RpWarning,(int)DispenserHomingControlId[DispenserId],0);
        DispenserHomingTime[DispenserId]=0;
    }
    else
    {
        DispenserHomingTime[DispenserId]+=InitialDispenserTimeLag;
    }
    return OK;
}*/
uint32_t IDS_Homing_Timeout_Callback(uint32_t DispenserId, uint32_t ReadValue)
{
    ReportWithPackageFilter(IDSFilter,"IDS_Homing_Timeout_Callback",__FILE__,DispenserId,(int)(GetDispenserPressure(DispenserId)*100),RpWarning,2000,0);
    SafeRemoveControlCallback(DispenserHomingTimeoutControlId[DispenserId], IDS_Homing_Timeout_Callback );
    AlarmHandlingSetAlarm (DispenserId+EVENT_TYPE__DISPENSER_1_REFILL_FAILURE,true);
    return OK;
}

uint32_t IDS_HomeDispenserCallback(uint32_t motorId, uint32_t ReadValue)
{
    uint8_t DispenserId = motorId-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;

    Read_MidTank_Pressure_Sensor(DispenserId);
    //close dry air valve in the dispenser
    //Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF);
    //Disable_MidTank_Pressure_Reading(DispenserId);
    //    MotorSetMicroStep(motorId, KeepMicrostep[DispenserId]);
    if (HomingRequestCallback[DispenserId])
    {
        HomingRequestCallback[DispenserId](DispenserId,0);
        HomingRequestCallback[DispenserId] = NULL;
    }
    RemoveControlCallback(DispenserHomingTimeoutControlId[DispenserId], IDS_Homing_Timeout_Callback );
    //    HomingActive[DispenserId]= false;
    IDS_Dispenser_RefillEnded ( DispenserId, MotorsCfg[motorId].microstep);
    ReportWithPackageFilter(IDSFilter,"Homing ended",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)DispenserHomingTime[DispenserId],0);
//    DispenserHomingControlId[DispenserId] = AddControlCallback( IDS_HomeDispenserBackMoveCallback, InitialDispenserTimeLag, GetDispenserPressure,motorId, motorId, 0 );
    MotorSetDirection(motorId,MotorsCfg[motorId].directionthreadwize);
    Task_sleep(10);
//    MotorSetSpeed(motorId, InitialDispenserSpeed);
    CurrentDispenserSpeed[DispenserId] = 0;
/////////////////////////////////////////////////////////////
    if (Extended_Motor_Param[motorId] == true)
        Power_Step_01_Dispenser_Mode(DispenserId,Voltage);
    //MotorSetMicroStep(motorId, KeepMicrostep[DispenserId]);
    HomingActive[DispenserId]= false;
    PrimingActive[DispenserId]= false;
    Control_TCA9534ByPass(DispenserId,DISABLE);// use ENABLE or DISABLE
#ifdef DISPENSER_VALVES_OPEN
    if (JobIsActive()==false)
        Control3WayValvesWithCallback (DispenserId, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
#endif

    //close dry air valve in the dispenser
    Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF);
    Disable_MidTank_Pressure_Reading(DispenserId);

    ////////////////////////////////////////////////////////
    return OK;
}
bool     IDS_IsHomingActive(uint32_t DispenserId)
{
    return HomingActive[DispenserId];
}
uint32_t IDS_HomeDispenser (uint32_t DispenserId, uint32_t speed , callback_fptr callback)
{
    assert(DispenserId < MAX_SYSTEM_DISPENSERS);
    TimerMotors_t MotorId = HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1 + DispenserId;
    //if (DispensersAlarmState[DispenserId] == true)
    //    return ERROR;

    double midTankContent = 0.0;
    if (isMotorConfigured(MotorId)==false)
    {
        ReportWithPackageFilter(IDSFilter,"IDS_HomeDispenser - Disp not in config",__FILE__,Dispenser_Id_to_LS_Id[DispenserId],(int)DispenserId,RpWarning,LIMIT,0);
        return ERROR;
    }
    if (FPGA_Read_limit_Switches(Dispenser_Id_to_LS_Id[DispenserId]) == LIMIT)
    {
        ReportWithPackageFilter(IDSFilter,"IDS_HomeDispenser - Disp is full",__FILE__,Dispenser_Id_to_LS_Id[DispenserId],(int)DispenserId,RpWarning,LIMIT,0);
        return ERROR;
    }
    if (Get_MidTank_Pressure_Sensor(DispenserId)<0.2)
    {
        ReportWithPackageFilter(IDSFilter,"IDS_HomeDispenser - Midtank is empty",__FILE__,Get_MidTank_Int1000_Sensor(DispenserId),(int)DispenserId,RpWarning,LIMIT,0);
        return ERROR;
    }
    midTankContent = Read_MidTank_Pressure_Sensor(DispenserId);
    if (midTankContent < 0.2) //midtank is empty - abort!!!!
    {
        ReportWithPackageFilter(IDSFilter,"IDS_HomeDispenser - Mid Tank Empty!!! ",__FILE__,__LINE__,(int)(midTankContent * 1000),RpWarning,DispenserId,0);
        return ERROR;
    }
    //if Safety is active
        //if safety is upper position or
    if ((HomingActive[DispenserId] == true)||(PrimingActive[DispenserId] == true))
    {
        ReportWithPackageFilter(IDSFilter,"IDS_HomeDispenser - Homing/priming already active",__FILE__,HomingActive[DispenserId],(int)DispenserId,RpWarning,PrimingActive[DispenserId],0);
        return ERROR;
    }

    if (Check_Disp_Safety_Stop_Indication(DispenserId) == true)
    {
        if ((Get_Notification_Disp_UP(DispenserId) == false)||(Get_Notification_Over_Press(DispenserId) == false))
        {
            ReportWithPackageFilter(IDSFilter,"IDS_HomeDispenser - Bypassing safety",__FILE__,HomingActive[DispenserId],(int)DispenserId,RpWarning,PrimingActive[DispenserId],0);
            Control_TCA9534ByPass(DispenserId,ENABLE);// use ENABLE or DISABLE
        }
    }
    HomingRequestCallback[DispenserId] = callback;

    if ( Dispenser_Id_to_LS_Id[DispenserId] != MAX_GPI)
    {
        HomingActive[DispenserId] = true;
        //open dispenser valve dispenser to midtank direction
        Control3WayValvesWithCallback ((Valves_t)DispenserId, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
        if (Extended_Motor_Param[MotorId] == true)
            Power_Step_01_Dispenser_Mode(DispenserId,Current);
        KeepMicrostep[DispenserId] = MotorsCfg[MotorId].microstep;
        //MotorSetMicroStep(MotorId, 1);
        //open dry air valve in the dispenser
        Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_ON);
        Enable_MidTank_Pressure_Reading(DispenserId);

        IDS_Dispenser_RefillStarted(DispenserId,1);
        AlarmHandlingSetAlarm (DispenserId+EVENT_TYPE__DISPENSER_1_REFILL_FAILURE,false);
        if (DispenserId== LUBRICANT_DISPENSER)
        {
            MotorMovetoLimitSwitch (MotorId,1-MotorsCfg[MotorId].directionthreadwize, speed/4,  Dispenser_Id_to_LS_Id[DispenserId], IDS_HomeDispenserCallback,0);
            CurrentDispenserSpeed[DispenserId] = speed/4;
            ReportWithPackageFilter(IDSFilter,"IDS_HomeDispenser ",__FILE__,(int)(midTankContent*1000),(int)DispenserId,RpWarning,speed/4,0);
            if (DispenserHomingTimeoutControlId[DispenserId] == 0xFF)
                DispenserHomingTimeoutControlId[DispenserId] = AddControlCallback("IDS Homing Timeout", IDS_Homing_Timeout_Callback, eOneHour*4,TemplateDataReadCBFunction ,DispenserId, 0, 0 );

        }
        else
        {
            MotorMovetoLimitSwitch (MotorId,1-MotorsCfg[MotorId].directionthreadwize, speed,  Dispenser_Id_to_LS_Id[DispenserId], IDS_HomeDispenserCallback,0);
            CurrentDispenserSpeed[DispenserId] = speed;
            ReportWithPackageFilter(IDSFilter,"IDS_HomeDispenser ",__FILE__,(int)(midTankContent*1000),(int)DispenserId,RpWarning,speed,0);
            if (DispenserHomingTimeoutControlId[DispenserId] == 0xFF)
                DispenserHomingTimeoutControlId[DispenserId] = AddControlCallback("IDS Homing Timeout", IDS_Homing_Timeout_Callback, eOneHour,TemplateDataReadCBFunction ,DispenserId, 0, 0 );
        }
        CurrentDispenserSpeed[DispenserId] = (-1*CurrentDispenserSpeed[DispenserId]);
        DispenserLastMovementDown[DispenserId] = true;

        return OK;
    }
    return ERROR;
}
uint32_t IDS_StopHomeDispenser (uint32_t DispenserId)
{
    assert(DispenserId < MAX_SYSTEM_DISPENSERS);

    ReportWithPackageFilter(IDSFilter,"IDS_StopHomeDispenser",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)HomingActive[DispenserId],0);
    if ((HomingActive[DispenserId] == false)&&( PrimingActive[DispenserId] == false))
    {
        ReportWithPackageFilter(IDSFilter,"Homing not active",__FILE__,__LINE__,(int)DispenserId,RpWarning,(int)HomingActive[DispenserId],0);
        return OK;
    }

    TimerMotors_t MotorId = (DispenserId)+HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
    MotorAbortMovetoLimitSwitch(MotorId);
    RemoveControlCallback(DispenserHomingTimeoutControlId[DispenserId], IDS_Homing_Timeout_Callback );
    //close dry air valve in the dispenser
    Disable_MidTank_Pressure_Reading(DispenserId);
    Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF);
    Control3WayValvesWithCallback ((Valves_t)DispenserId, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer

/*    if (HomingActive[DispenserId] == true)
    {
        IDS_StopHomeDispenserBuildPressure(DispenserId);
    }*/
    if (HomingActive[DispenserId] == true)
    {
        if (DispenserId == LUBRICANT_DISPENSER)
        {
            Lubricant_2Way_Valve (STOP);
        }
    }
    HomingActive[DispenserId] = false;
    PrimingActive[DispenserId] = false;
    Control_TCA9534ByPass(DispenserId,DISABLE);// use ENABLE or DISABLE
#ifdef DISPENSER_VALVES_OPEN
    if (JobIsActive()==false)
        Control3WayValvesWithCallback (DispenserId, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
#endif
    if (Extended_Motor_Param[MotorId] == true)
        Power_Step_01_Dispenser_Mode(DispenserId,Voltage);
    //MotorSetMicroStep(MotorId, KeepMicrostep[DispenserId]);
    CurrentDispenserSpeed[DispenserId] = 0;
    return OK;
}
/*
uint32_t IDS_StopHomeDispenserBuildPressure (uint32_t DispenserId)
{
    assert(DispenserId < MAX_SYSTEM_DISPENSERS);

    ReportWithPackageFilter(IDSFilter,"IDS_StopHomeDispenser and build pressure",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)HomingActive[DispenserId],0);
    if (HomingActive[DispenserId] != true)
    {
        ReportWithPackageFilter(IDSFilter,"Homing not active",__FILE__,__LINE__,(int)DispenserId,RpWarning,(int)HomingActive[DispenserId],0);
        return ERROR;
    }
    else
        HomingActive[DispenserId] = false;
    Control_TCA9534ByPass(DispenserId,DISABLE);// use ENABLE or DISABLE

    TimerMotors_t motorId = (DispenserId)+HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
    MotorAbortMovetoLimitSwitch(motorId);
    //close dry air valve in the dispenser
//////////////////////////////////////////////////
    Read_MidTank_Pressure_Sensor(DispenserId);
    //close dry air valve in the dispenser
    Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF);
    Disable_MidTank_Pressure_Reading(DispenserId);
    //    MotorSetMicroStep(motorId, KeepMicrostep[DispenserId]);
    if (HomingRequestCallback[DispenserId])
    {
        HomingRequestCallback[DispenserId](DispenserId,0);
        HomingRequestCallback[DispenserId] = NULL;
    }
//    HomingActive[DispenserId]= false;
      ReportWithPackageFilter(IDSFilter,"End homing for job start",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)DispenserHomingTime[DispenserId],0);
  /`*  else
    {
        ReportWithPackageFilter(IDSFilter,"homing aborted,start backlash",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)DispenserHomingTime[DispenserId],0);
        DispenserHomingControlId[DispenserId] = AddControlCallback( IDS_HomeDispenserBackMoveCallback, InitialDispenserTimeLag, GetDispenserPressure,motorId, motorId, 0 );
        Task_sleep(10);
        MotorSetDirection(motorId,MotorsCfg[motorId].directionthreadwize);
        Task_sleep(10);
        MotorSetSpeed(motorId, 1000);
        CurrentDispenserSpeed[DispenserId] = 1000;
    }*`/
    //IDS_Dispenser_RefillEnded ( DispenserId, MotorsCfg[motorId].microstep);

    return OK;
}
*/
uint32_t IDS_CheckDispenserLimitSwitch (LimitSwitchAlarms LS_Id)
{
    int DispenserId;
    if (LS_Id <= LimitSwitchAlarmEmpty_8)
    {
        DispenserId = LS_Id;
    }
    else
    {
        DispenserId = LS_Id-LimitSwitchAlarmEmpty_8;
    }
    if (isMotorConfigured(DispenserId + HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1)==false)
        return true;  //false is the alarm value
    else
        return FPGA_Read_limit_Switches(Dispenser_Id_to_Alarm_LS_Id[LS_Id]);
}
uint32_t IDS_EmptyDispenserCallback(uint32_t motorId, uint32_t ReadValue)
{
    uint8_t DispenserId = motorId-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;

    Read_MidTank_Pressure_Sensor(DispenserId);
    //close dry air valve in the dispenser
    Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF);
    Disable_MidTank_Pressure_Reading(DispenserId);

    MotorStop(motorId,Hard_Hiz);
    CurrentDispenserSpeed[DispenserId] = 0;
    if (Extended_Motor_Param[motorId] == true)
        Power_Step_01_Dispenser_Mode(DispenserId,Voltage);
    //MotorSetMicroStep(motorId, KeepMicrostep[DispenserId]);
    PrimingActive[DispenserId]= false;
    Control_TCA9534ByPass(DispenserId,DISABLE);// use ENABLE or DISABLE
    if (DispenserId == LUBRICANT_DISPENSER)
    {
        Lubricant_2Way_Valve (STOP);
    }
    ReportWithPackageFilter(IDSFilter,"End Priming",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)DispenserHomingTime[DispenserId],0);
    //    MotorSetMicroStep(motorId, KeepMicrostep[DispenserId]);
    if (HomingRequestCallback[DispenserId])
    {
        HomingRequestCallback[DispenserId](DispenserId,0);
        HomingRequestCallback[DispenserId] = NULL;
    }
    return OK;
}

uint32_t IDS_EmptyDispenser (uint32_t DispenserId, uint32_t speed , callback_fptr callback)
{
    assert(DispenserId < MAX_SYSTEM_DISPENSERS);

    //if (DispensersAlarmState[DispenserId] == true)
    //    return ERROR;
    if ((HomingActive[DispenserId] == true)||(PrimingActive[DispenserId] == true))
    {
        ReportWithPackageFilter(IDSFilter,"IDS_HomeDispenser - Homing/priming already active",__FILE__,HomingActive[DispenserId],(int)DispenserId,RpWarning,PrimingActive[DispenserId],0);
        return ERROR;
    }
    if (Check_Disp_Safety_Stop_Indication(DispenserId) == true)
    {
        if ((Get_Notification_Disp_Down(DispenserId) == false)||(Get_Notification_Over_Press(DispenserId) == false))
        {
            ReportWithPackageFilter(IDSFilter,"IDS_EmptyDispenser - Bypassing safety",__FILE__,HomingActive[DispenserId],(int)DispenserId,RpWarning,PrimingActive[DispenserId],0);
            Control_TCA9534ByPass(DispenserId,ENABLE);// use ENABLE or DISABLE
        }
    }
    TimerMotors_t MotorId = HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1 + DispenserId;
    if ( Dispenser_Id_to_LS_Empty_Id[DispenserId] != MAX_GPI)
    {
        PrimingActive[DispenserId] = true;
        HomingRequestCallback[DispenserId] = callback;
        //open dispenser valve dispenser to midtank direction
        Control3WayValvesWithCallback ((Valves_t)DispenserId, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
        if (Extended_Motor_Param[MotorId] == true)
            Power_Step_01_Dispenser_Mode(DispenserId,Current);
        //Valve_Set((Valves_t) request->index, Dispenser_Mixer);
        KeepMicrostep[DispenserId] = MotorsCfg[MotorId].microstep;
        //MotorSetMicroStep(MotorId, 1);
        Task_sleep(100);
        //open dry air valve in the dispenser
        //Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_ON);
        if (DispenserId == LUBRICANT_DISPENSER)
        {
            Lubricant_2Way_Valve (START);
        }
        MotorMovetoLimitSwitch (MotorId,MotorsCfg[MotorId].directionthreadwize, speed,  Dispenser_Id_to_LS_Empty_Id[DispenserId], IDS_EmptyDispenserCallback,0);
        CurrentDispenserSpeed[DispenserId] = speed;
        return OK;
    }
    return ERROR;
}
uint32_t IDS_Dispenser_Alarm_On (uint8_t DispenserId)
{
    uint32_t status = OK;
    ReportWithPackageFilter(IDSFilter,"IDS_Dispenser_Alarm_On",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,0,0);
    assert(DispenserId < MAX_SYSTEM_DISPENSERS);
    DispensersAlarmState[DispenserId] = true;
    TimerMotors_t MotorId = HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1 + DispenserId;
    status |= MotorAbortMovetoLimitSwitch(MotorId);
    Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF);
    Disable_MidTank_Pressure_Reading(DispenserId);
    if (Extended_Motor_Param[MotorId] == true)
        Power_Step_01_Dispenser_Mode(DispenserId,Voltage);
    //status |= MotorSetMicroStep(MotorId, KeepMicrostep[DispenserId]);
    status |= MotorStop(MotorId, Hard_Hiz);
    IDS_Dispenser_Init(DispenserId);
    JobEndReason = JOB_OUT_OF_DYE;
    usnprintf(AlarmReasonStr, 100, "IDS_Dispenser_Alarm_On %d ",DispenserId);
    return status;
}
uint32_t IDS_Dispenser_Alarm_Off (uint8_t DispenserId)
{
    uint32_t status = OK;
    assert(DispenserId < MAX_SYSTEM_DISPENSERS);
    DispensersAlarmState[DispenserId] = false;
    ReportWithPackageFilter(IDSFilter,"IDS_Dispenser_Alarm_Off",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,0,0);
    return status;
}
uint32_t IDS_ReCheck_Pressure_Callback(uint32_t DispenserId, uint32_t ReadValue)
{
    ReportWithPackageFilter(IDSFilter,"Pulling dispenser down result",__FILE__,DispenserId,(int)(GetDispenserPressure(DispenserId)*100),RpWarning,200,0);
    SafeRemoveControlCallback(GetControlLowDevice_i(), IDS_ReCheck_Pressure_Callback );

    return OK;
}
bool Check_Pressure_Bypass = false;
bool Set_Check_Pressure_Bypass (int value)
{
    if (value == 0)
        Check_Pressure_Bypass = false;
    else
        Check_Pressure_Bypass = true;
    return Check_Pressure_Bypass;
}
uint32_t IDS_Check_Pressure_Callback(uint32_t DispenserId, uint32_t ReadValue)
{
    TimerMotors_t  HW_Motor_Id = DispenserIdToMotorId[DispenserId];
    if (JobIsActive())
    {
#ifndef LIGHT_COLORS
            if ((DispenserId == CLEANER_DISPENSER)&&(DispenserPreSegmentControlId != 0xFF)) //not in presegment
            {
                return ERROR;
            }
            else if ((DispenserUsedInJob[DispenserId] == true)&&(DispenserId != CLEANER_DISPENSER))
            {
                return ERROR;
            }

#else
            if (DispenserUsedInJob[DispenserId] == true)
            {
                return ERROR;
            }
#endif
    }
    if (Check_Pressure_Bypass == true)
        return OK;

    if (GetDispenserPressure(DispenserId)>3.0)
    {
        //10/8/2020 pulling down 2000 steps
        ReportWithPackageFilter(IDSFilter,"Pulling dispenser down to reduce pressure",__FILE__,DispenserId,(int)(GetDispenserPressure(DispenserId)*100),RpWarning,2000,0);
        if ((HomingActive[DispenserId] == true)||(PrimingActive[DispenserId] == true))
        {
            ReportWithPackageFilter(IDSFilter,"Dispenser in pressure but homing active",__FILE__,DispenserId,(int)HomingActive[DispenserId],RpWarning,PrimingActive[DispenserId],0);
            return ERROR;
        }
        if (GetDispenserPressure(DispenserId)>5.5)
        {
            //stop job on pressure rise above 5.5 bar
            IDS_Dispenser_Alarm_On(DispenserId);
            //open valve toward mixer on pressure rise above 5.5 bar
            Task_sleep(100);
            Control3WayValvesWithCallback ((Valves_t)DispenserId, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
        }
        MotorMove(HW_Motor_Id,1-MotorsCfg[HW_Motor_Id].directionthreadwize,2000*MotorsCfg[HW_Motor_Id].microstep);
        AddControlCallback("IDS_ReCheck_Pressure", IDS_ReCheck_Pressure_Callback, eOneSecond*4,TemplateDataReadCBFunction ,DispenserId, 0, 0 );
    }
    return OK;
}


void IDS_Dispenser_Init(uint8_t DispenserId)
{
    assert(DispenserId < MAX_SYSTEM_DISPENSERS);
    ReportWithPackageFilter(IDSFilter,"IDS_Dispenser_Init",__FILE__,DispenserId,HomingActive[DispenserId],RpWarning,PrimingActive[DispenserId],0);
    CurrentDispenserSpeed[DispenserId] = 0;
    HomingActive[DispenserId]= false;
    PrimingActive[DispenserId]= false;
    Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF);
    if (ControlIdtoInactiveDispenserId[DispenserId] == 0xFF)
        ControlIdtoInactiveDispenserId[DispenserId] = AddControlCallback("IDS_Check_Pressure", IDS_Check_Pressure_Callback, eOneMinute,TemplateDataReadCBFunction ,DispenserId, 0, 0 );
}