/************************************************************************************************************************
 * 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"


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 2.10
#define INITIAL_DISPENSER_TIMEOUT_LIMIT 60000
#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 IDS_StopHomeDispenserBuildPressure (uint32_t deviceID);

AutoHoming_Config_enum AutoHoming_Config = AutoHoming_off;
void IDS_Dispenser_SetAutoHoming_Config(AutoHoming_Config_enum Config)
{
    Report("IDS_Dispenser_SetAutoHoming_Config ",__FILE__,__LINE__,Config,RpWarning,AutoHoming_Config,0);
    AutoHoming_Config = Config;
}

void IDS_Dispenser_SetBackLashValues(double   initialdispenserpressure,   uint32_t initialdispensertimeout,   uint32_t initialdispensertimelag,   uint32_t initialdispenserspeed)
{
    InitialDispenserPressure = initialdispenserpressure;
    InitialDispenserTimeout = initialdispensertimeout;
    InitialDispenserTimeLag = initialdispensertimelag;
    InitialDispenserSpeed = INITIAL_DISPENSER_SPEED;

    Report("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)||(JobStartStopBackLash[DispenserId] == true))
    {
        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);

        if (JobStartStopBackLash[DispenserId] == true)
            JobStartStopBackLash[DispenserId] = false;
        Report("End backlash",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)DispenserHomingTime[DispenserId],0);
        if (SafeRemoveControlCallback(DispenserHomingControlId[DispenserId], IDS_HomeDispenserBackMoveCallback )==OK)
            DispenserHomingControlId[DispenserId] = 0xFF;
        else
            Report("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_HomeDispenserCallback(uint32_t motorId, uint32_t ReadValue)
{
    uint8_t DispenserId = motorId-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;

    MotorStop(motorId,Hard_Hiz);
    CurrentDispenserSpeed[DispenserId] = 0;
    if (Extended_Motor_Param == true)
        Power_Step_01_Dispenser_Mode(DispenserId,Voltage);
    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]);
    HomingActive[DispenserId]= false;
    PrimingActive[DispenserId]= false;
    Control_TCA9534ByPass(DispenserId,DISABLE);// use ENABLE or DISABLE
    if (HomingRequestCallback[DispenserId])
    {
        HomingRequestCallback[DispenserId](DispenserId,0);
        HomingRequestCallback[DispenserId] = NULL;
    }
//    HomingActive[DispenserId]= false;
    IDS_Dispenser_RefillEnded ( DispenserId, MotorsCfg[motorId].microstep);
    Report("End homing no backlash backlash",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)DispenserHomingTime[DispenserId],0);
    /*Report("Start backlash",__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] = InitialDispenserSpeed;*/
    return OK;
}

uint32_t IDS_HomeDispenser (uint32_t DispenserId, uint32_t speed , callback_fptr callback)
{
    assert(DispenserId < MAX_SYSTEM_DISPENSERS);
    //if (DispensersAlarmState[DispenserId] == true)
    //    return ERROR;

    double midTankContent = 0.0;
    if (FPGA_Read_limit_Switches(Dispenser_Id_to_LS_Id[DispenserId]) == LIMIT)
    {
        Report("IDS_HomeDispenser - Disp is full",__FILE__,Dispenser_Id_to_LS_Id[DispenserId],(int)DispenserId,RpWarning,LIMIT,0);
        return OK;
    }
    if (Get_MidTank_Pressure_Sensor(DispenserId)<0.2)
    {
        Report("IDS_HomeDispenser - Midtank is empty",__FILE__,Get_MidTank_Int1000_Sensor(DispenserId),(int)DispenserId,RpWarning,LIMIT,0);
        return ERROR;
    }
    //if Safety is active
        //if safety is upper position or
    if ((HomingActive[DispenserId] == true)||(PrimingActive[DispenserId] == true))
    {
        Report("IDS_HomeDispenser - Homing/priming already active",__FILE__,HomingActive[DispenserId],(int)DispenserId,RpWarning,PrimingActive[DispenserId],0);
        return ERROR;
    }
    else
        HomingActive[DispenserId] = true;

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

    TimerMotors_t MotorId = HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1 + DispenserId;
    if ( Dispenser_Id_to_LS_Id[DispenserId] != MAX_GPI)
    {
        //open dispenser valve dispenser to midtank direction
        Control3WayValvesWithCallback ((Valves_t)DispenserId, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
        KeepMicrostep[DispenserId] = MotorsCfg[MotorId].microstep;
        if (Extended_Motor_Param == true)
            Power_Step_01_Dispenser_Mode(DispenserId,Current);
        MotorSetMicroStep(MotorId, 1);
        SysCtlDelay(180000);
        //open dry air valve in the dispenser
        Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_ON);
        Enable_MidTank_Pressure_Reading(DispenserId);
        SysCtlDelay(1200000);
        midTankContent = Read_MidTank_Pressure_Sensor(DispenserId);
        if (midTankContent < 0.2) //midtank is empty - abort!!!!
        {
            Report("IDS_HomeDispenser - Mid Tank Empty!!! ",__FILE__,__LINE__,(int)(midTankContent * 1000),RpWarning,DispenserId,0);
            Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF);
            STATUS_GREEN_LED_OFF;
            STATUS_RED_LED_ON;

            return ERROR;
        }

        IDS_Dispenser_RefillStarted(DispenserId,1);
        MotorMovetoLimitSwitch (MotorId,1-MotorsCfg[MotorId].directionthreadwize, speed,  Dispenser_Id_to_LS_Id[DispenserId], IDS_HomeDispenserCallback,0);
        CurrentDispenserSpeed[DispenserId] = speed;
        CurrentDispenserSpeed[DispenserId] = (-1*CurrentDispenserSpeed[DispenserId]);
        Report("IDS_HomeDispenser ",__FILE__,__LINE__,(int)DispenserId,RpWarning,speed,0);
        return OK;
    }
    return ERROR;
}
uint32_t IDS_Dispenser_Alarm_On (uint8_t DispenserId)
{
    uint32_t status = OK;
    Report("IDS_Dispenser_Alarm_On",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,0,0);
    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);
    Enable_MidTank_Pressure_Reading(DispenserId);
    if (Extended_Motor_Param == true)
        Power_Step_01_Dispenser_Mode(DispenserId,Voltage);
    status |= MotorSetMicroStep(MotorId, KeepMicrostep[DispenserId]);
    status |= MotorStop(DispenserId, Hard_Hiz);
    CurrentDispenserSpeed[DispenserId] = 0;
    JobEndReason = JOB_OUT_OF_DYE;
    return status;
}
uint32_t IDS_Dispenser_Alarm_Off (uint8_t DispenserId)
{
    uint32_t status = OK;
    DispensersAlarmState[DispenserId] = false;
    Report("IDS_Dispenser_Alarm_Off",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,0,0);
    return status;
}

uint32_t IDS_StopHomeDispenser (uint32_t DispenserId)
{
    assert(DispenserId < MAX_SYSTEM_DISPENSERS);

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

    TimerMotors_t MotorId = (DispenserId)+HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
    MotorAbortMovetoLimitSwitch(MotorId);
    //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 (PrimingActive[DispenserId] == true)
    {
        if (DispenserId == LUBRICANT_DISPENSER)
        {
            Lubricant_2Way_Valve (STOP);
        }
        PrimingActive[DispenserId] = false;
        Control_TCA9534ByPass(DispenserId,DISABLE);// use ENABLE or DISABLE
        if (Extended_Motor_Param == true)
            Power_Step_01_Dispenser_Mode(DispenserId,Voltage);
        MotorSetMicroStep(MotorId, KeepMicrostep[DispenserId]);
        CurrentDispenserSpeed[DispenserId] = 0;
        CurrentDispenserSpeed[DispenserId] = 0;
    }
    return OK;
}
uint32_t IDS_StopHomeDispenserBuildPressure (uint32_t DispenserId)
{
    assert(DispenserId < MAX_SYSTEM_DISPENSERS);

    Report("IDS_StopHomeDispenser and build pressure",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)HomingActive[DispenserId],0);
    if (HomingActive[DispenserId] != true)
    {
        LOG_ERROR (DispenserId,"Homing not active");
        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);
    CurrentDispenserSpeed[DispenserId] = 0;
    if (Extended_Motor_Param == true)
        Power_Step_01_Dispenser_Mode(DispenserId,Voltage);
    MotorSetMicroStep(motorId, KeepMicrostep[DispenserId]);

    if (HomingRequestCallback[DispenserId])
    {
        HomingRequestCallback[DispenserId](DispenserId,0);
        HomingRequestCallback[DispenserId] = NULL;
    }
//    HomingActive[DispenserId]= false;
    JobStartStopBackLash[DispenserId] = false;
    Report("End homing no backlash",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)DispenserHomingTime[DispenserId],0);
    /*if (JobStartStopBackLash[DispenserId] == true)
    {
            JobStartStopBackLash[DispenserId] = false;
            Report("End homing for job start",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)DispenserHomingTime[DispenserId],0);
    }
    else
    {
        Report("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 == 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);
    }
    Report("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))
        return ERROR;
    else
        PrimingActive[DispenserId] = true;
    HomingRequestCallback[DispenserId] = callback;
    if (Check_Disp_Safety_Stop_Indication(DispenserId) == true)
    {
        if ((Get_Notification_Disp_Down(DispenserId) == false)||(Get_Notification_Over_Press(DispenserId) == false))
        {
            Report("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)
    {
        //open dispenser valve dispenser to midtank direction
        Control3WayValvesWithCallback ((Valves_t)DispenserId, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
        if (Extended_Motor_Param == 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;
}