merge

10 files changed, 403 insertions, 360 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 4ad5336d9..a56c3dae1 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -766,8 +766,12 @@ uint32_t MillisecLowLoop(uint32_t tick)
             }
             */
         }
+
         //call waste state machine
         Waste_StateMachine_OneSecond_Call();
+
+        //call IFS state machine
+        midTankStateMachine();
     }
     if (Tensecond_Tick)
     {
@@ -796,6 +800,11 @@ uint32_t MillisecLowLoop(uint32_t tick)
 #ifdef CONTROL_DEBUG
         ResetControlTime();
 #endif
+        if (WHS_Type == WHS_TYPE_NEW)
+        {
+            waste_seq_step1();// include 1Sec delay           <- to open !!!!
+        }
+
     }
     if (OneHourTick)
     {
@@ -807,10 +816,10 @@ uint32_t MillisecLowLoop(uint32_t tick)
             PumpCounter = 0;
         }
         MidTankReading();
-        if (WHS_Type == WHS_TYPE_NEW)
+        /*if (WHS_Type == WHS_TYPE_NEW)
         {
             waste_seq_step1();// include 1Sec delay           <- to open !!!!
-        }
+        }*/
         //Trigger_WHS_MAX11614_Read_Gas_Sensor();
     }
     //ROM_IntMasterEnable();
diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index 27ed8d0e9..bec8fc53f 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -51,7 +51,7 @@
 #include "Drivers/I2C_Communication/Dispenser_Card/EEPROM/Dispenser_EEPROM.h"
 #include <Drivers/I2C_Communication/Dispenser_Card/IO_Ports/Dispenser_IO.h>
 #include "Modules/General/buttons.h"
-#include "Modules/Waste/Waste.h"
+#include "Modules/Waste/Waste_ex.h"
 #include "Drivers/I2C_Communication/Main_Board_EEPROM/Main_EEPROM.h"
 #include <Modules/Waste/newWHS_init.h>
 #include <Drivers/I2C_Communication/I2C_Task.h>
@@ -165,12 +165,8 @@ uint32_t HWConfigurationInit(void)
 #ifndef DISPESER_TEST
     if (WHS_Type == WHS_TYPE_NEW)
         newWHS_init();
-    //WHS_init(); // remove call to old WHS
 #endif
-
-    // Waste Init (WHS)
     Waste_Init();
-
     ADC_MUX_Init();
     GeneralHwReady = true;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
index 8f10a0e5d..0be1e7ebe 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
@@ -55,17 +55,20 @@ uint32_t IDS_Cleaning_Move_Actuators()
        return OK;
    if (CleaningStageCounter == 1)
    {
-       Trigger_Head_Actuators_Control(ACTIN, LOW,true);
+       Trigger_Head_Actuators_Stub(ACTIN, true, true);
+       //Trigger_Head_Actuators_Control(ACTIN, LOW,true);
        Report("IDS_Cleaning_Move_ActuatorUp", __FILE__, __LINE__, 1, RpWarning,   CleaningStageCounter, 0);
    }
    else if (CleaningStageCounter == Tup)
    {
-       Trigger_Head_Actuators_Disable();
+       Trigger_Head_Actuators_Stub(ACTIN, false, false);
+       //Trigger_Head_Actuators_Disable();
        Report("IDS_Cleaning_Stop_Actuator", __FILE__, __LINE__, Tup, RpWarning,   CleaningStageCounter, 0);
    }
    else if (CleaningStageCounter == Tdelay1)
    {
-       Trigger_Head_Actuators_Control(ACTIN, LOW,false);
+       Trigger_Head_Actuators_Stub(ACTIN, true, false);
+       //Trigger_Head_Actuators_Control(ACTIN, LOW,false);
        Report("IDS_Cleaning_MoveDown", __FILE__, __LINE__, Tdelay1, RpWarning,   CleaningStageCounter, 0);
    }
    else if (CleaningStageCounter == Tdelay2)
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index b4442c996..c444c7017 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -590,7 +590,7 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue)
              Report("pressureReady = true  or timeout;",__FILE__,DispenserBuildTimeCounter,NumofReadyDispensers,RpWarning,(int)NumOfActiveDispensers,0);
          }
      }
-     else if (endOfPrepareWCF == false)
+     else if ((endOfPrepareWCF == false)&&(NumOfActiveDispensers))
      {
          if ((DispenserBuildTimeCounter == PRESSURE_READ_TIME_GAP)&&(FirstBrushStop))
          {
@@ -683,6 +683,13 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue)
 
          }
      }
+     else if (NumOfActiveDispensers == 0)
+     {
+         endOfPrepareWCF = true;
+         FreeBrushStopFileData(FirstBrushStop);
+         FirstBrushStop =NULL;
+         Report("endOfPrepareWCF = true;",__FILE__,DispenserBuildTimeCounter,NumofReadyDispensers,RpWarning,(int)NumOfActiveDispensers,0);
+     }
      //start new stage
      if (endOfPrepareWCF == true)
      {
@@ -1027,7 +1034,7 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue)
 
 
 //********************************************************************************************************************
-
+#define IDS_PRESEGMENT_TIME_STEP 50
  uint32_t IDSPreSegmentStateCallbackRunner(uint32_t IfIndex, uint32_t ReadValue)
  {
      JobDispenser **Dispensers;
@@ -1065,7 +1072,7 @@ uint32_t IDS_Cleaning_Spray_Cleaning_Solution (int dispenserSpeed,callback_fptr
 uint32_t IDS_Cleaning_Stop_Cleaning_Solution (callback_fptr callback);
 */
      //InterSegmentStepsLimit = lInterSegmentLength*10;//100 millisec steps
-     InterSegmentStepsCount+=100;
+     InterSegmentStepsCount+=IDS_PRESEGMENT_TIME_STEP;
      if (InterSegmentStepsCount >= lInterSegmentLength)
      {
          //IDS_Valve_PresegmentReady(1,0);
@@ -1259,7 +1266,7 @@ uint32_t IDSPreSegmentState(void *SegmentDetails, int SegmentId)
             lInterSegmentLength = ((IntersegmentLength*100)*1000/dyeingspeed);
             lInterSegmentLength-=(lInterSegmentLength%100);   //round to a 100 multiplication
             InterSegmentStepsCount = 0;
-            DispenserPreSegmentControlId = AddControlCallback(NULL,IDSPreSegmentStateCallbackRunner, 100,TemplateDataReadCBFunction ,0, 0, 0 );
+            DispenserPreSegmentControlId = AddControlCallback(NULL,IDSPreSegmentStateCallbackRunner, IDS_PRESEGMENT_TIME_STEP,TemplateDataReadCBFunction ,0, 0, 0 );
             if (DispenserPreSegmentControlId == 0xFF)
             {
                 Report("Add control callback failed",__FILE__,__LINE__,(int)100,RpWarning,(int)0,0);
@@ -1292,7 +1299,7 @@ uint32_t IDSPreSegmentState(void *SegmentDetails, int SegmentId)
         lInterSegmentLength = (dryerbufferCentimeters*1000/dyeingspeed);
         lInterSegmentLength-=(lInterSegmentLength%100);   //round to a 100 multiplication
         InterSegmentStepsCount = 0;
-        DispenserPreSegmentControlId = AddControlCallback( IDSPreSegmentStateCallbackRunner, 100,TemplateDataReadCBFunction ,0, 0, 0 );
+        DispenserPreSegmentControlId = AddControlCallback( IDSPreSegmentStateCallbackRunner, IDS_PRESEGMENT_TIME_STEP,TemplateDataReadCBFunction ,0, 0, 0 );
         if (DispenserPreSegmentControlId == 0xFF)
         {
             Report("Add control callback failed",__FILE__,__LINE__,(int)100,RpWarning,(int)0,0);
@@ -1695,7 +1702,7 @@ uint32_t IDSSegmentState(void *SegmentDetails, int SegmentId)
          lInterSegmentLength = (dryerbufferCentimeters*1000/dyeingspeed);
          lInterSegmentLength-=(lInterSegmentLength%100);   //round to a 100 multiplication
          InterSegmentStepsCount = 0;
-         DispenserPreSegmentControlId = AddControlCallback("IDS DTS", IDSPreSegmentStateCallbackRunner, 100,TemplateDataReadCBFunction ,0, 0, 0 );
+         DispenserPreSegmentControlId = AddControlCallback("IDS DTS", IDSPreSegmentStateCallbackRunner, IDS_PRESEGMENT_TIME_STEP,TemplateDataReadCBFunction ,0, 0, 0 );
          if (DispenserPreSegmentControlId == 0xFF)
          {
              Report("Add control callback failed",__FILE__,__LINE__,(int)100,RpWarning,(int)0,0);
diff --git a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
index 5af90c3d1..974b022d7 100644
--- a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
+++ b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
@@ -8,14 +8,15 @@
  *  @author Shai
  */
 
+#include <DataDef.h>
 #include "PMR/Diagnostics/CartridgeSlot.pb-c.h"
 #include "PMR/Diagnostics/CartridgeTagContent.pb-c.h"
 #include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
-
-#include <DataDef.h>
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"//#include "FPGA_GPIO.h"  //  use for FPGA IO
+#include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
+#include "StateMachines/Initialization/PowerOffSequence.h"
 #include "include.h"
 
-#include "Modules/Waste/Waste.h"
 #include "Modules/IDS/ids_ex.h"
 #include "Modules/IFS/ifs.h"
 #include "Modules/AlarmHandling/AlarmHandling.h"
@@ -23,256 +24,147 @@
 #include "CartridgeValidationResponse.pb-c.h"
 #include  <PMR/Diagnostics/EventType.pb-c.h>
 #include "drivers/Valves/Valve.h"
-#include "Modules/Waste/Waste.h"
 #include "../control/control.h"
+#include "Modules/General/buttons.h"
+
+#define CARTRIDGE_EMPTY_QUANTITY_LIMIT 10
+#define CARTRIDGE_EMPTY_CALCULATION_TIME 12
+#define FIVE_SECONDS 5
+
+//#define IFS_DEBUG
+
+int CartridgeInkTimeout = CARTRIDGE_INK_TIMEOUT;
+int CartridgeCheckTime = FIVE_SECONDS;
 
-bool IFS_TimeOutAlarm(bool status);
-bool ColorMatch();
-bool MidTankValvesAction(bool action);
+double midTankCapacity;
+MidTank_t midTankCartColor;
+bool prevDoorState;
+bool doorState;
+uint8_t inkAuthenticationPass;
+uint32_t midTankTimeoutCounter = 0;
+double MidTankEmptyLimit = MidTank_Pressure_EMPTY;
+MidTankStateEnum midTankState = 0;
+
+char CartridgeRequestToken[36+1];
+int  CartridgeRequestId = 0x01010101;
+
+bool RdInkCartridgeSensor()
+{
+    return Is_Cartridge_Present(CART_1);
+}
 
 bool IFS_CartridgeLowerPresent()
 {
-    return IFS_info.IFS_Ink_precense_sensor;
+    return RdInkCartridgeSensor();
 }
 
 bool IFS_MidTankFilling(void)
 {
-    return (IFS_info.Ink.time_out > 0);
+    return (midTankTimeoutCounter > 0);
 }
+
 MidTank_t IFS_MidTankIsActive(void)
 {
-    return (IFS_info.Ink.cart_color);
+    return midTankCartColor;
 }
 
-NFC_Tag NFCTag[MAX_CARTRIDGES];
+uint32_t MidTankReading(void)
+{
+    if (midTankTimeoutCounter > 0)
+        return ERROR;
+    return OK;
+}
 
-void IFS_Init(void)
+bool MidTankValvesAction(bool action)
 {
-    int i;
-    for (i=0;i<MAX_CARTRIDGES;i++)
-    {
-        //TagInitialize(&TagContent[i]);
-        memset(NFCTag[i].Buf,0,sizeof(NFCTag[i]));
+    bool ret =  true;
+    if (action == Cartridge_MidTank_ON) {
+        Enable_MidTank_Pressure_Reading(midTankCartColor);
+    } else {
+        Disable_MidTank_Pressure_Reading(midTankCartColor);
+    }
+    if (IDS_IsHomingActive(midTankCartColor)==false) {
+        Valve_Set(IDS_Id_to_AirValve[midTankCartColor], action);
     }
+    Valve_Set(IDS_Id_to_CartrideValve[midTankCartColor], action);
+    return ret;
 }
 
-/**
- *
- * @brief detect cartridge INK Empty
- *  this function detect changes in IFS INK cartridge
- *  when MidTank average is bigger the 98% of midtank pressure
- *  the INK cartridge is empty
- * @return bool ret OK = 0 , CartridgeFinished = 1
- */
-#define CARTRIDGE_EMPTY_QUANTITY_LIMIT 10
-#define CARTRIDGE_EMPTY_CALCULATION_TIME 12
 bool DetectIfCartridgeFinished(int int_MidTank_Pressure_1000)
 {
-
-#define CartridgeFinished 1
-
-    bool ret = OK;
+    bool ret = false;
     static int measure_midtank[CARTRIDGE_EMPTY_CALCULATION_TIME] ;
     static int count_measure_midtank = 0;
     int i=0;
     int sum = 0;
     int average = 0;
 
-    if ( count_measure_midtank == CARTRIDGE_EMPTY_CALCULATION_TIME)
+    if (count_measure_midtank == CARTRIDGE_EMPTY_CALCULATION_TIME)
         count_measure_midtank = 0;
     measure_midtank[count_measure_midtank] = int_MidTank_Pressure_1000;
     count_measure_midtank += 1;
     for (i=0;i<CARTRIDGE_EMPTY_CALCULATION_TIME;i++)
         sum += measure_midtank[i];
     average = sum/CARTRIDGE_EMPTY_CALCULATION_TIME;
-    if (abs(average - int_MidTank_Pressure_1000)<CARTRIDGE_EMPTY_QUANTITY_LIMIT)  ret = CartridgeFinished;
-    //if (average > (int_MidTank_Pressure_1000 * 0.98) )  ret = CartridgeFinished;
-
+    if (abs(average - int_MidTank_Pressure_1000)<CARTRIDGE_EMPTY_QUANTITY_LIMIT)
+        ret = true;
     return ret;
 }
 
-
-
-/**
- *
- * @brief detect changes in cartridge INK presence
- *  this function detect changes in IFS presence cartridge
- *  when changes are happens it update the presence status
- *  and the correct event
- * @return IFS_INK_presence_sensor or WHS_no_event event
- */
-
-WHS_sensor CartridgeInkCallBackFunction()
+#ifdef IFS_DEBUG
+bool CartridgeInkFull()
 {
-     WHS_sensor ret = WHS_no_event;
-
-    if (IFS_info.IFS_Ink_precense_sensor != RdInkCartridgeSensor())
-    {
-        WHS_info.event = IFS_INK_presence_sensor;
-        IFS_info.IFS_Ink_precense_sensor = !IFS_info.IFS_Ink_precense_sensor;
-        Report("------------ find  : IFS_Ink_presence_sensor event  -----------------", __FILE__,__LINE__, IFS_info.IFS_Ink_precense_sensor, RpMessage, 0, 0);
-        ret = IFS_INK_presence_sensor;
+    static int count = 1;
+    if((count++ % 60) == 0) {
+        return true;
+    } else {
+        return false;
     }
-    return ret;
 }
+#else
+int CartridgeFinishedCount = 0;
 
-/**
- *
- * @brief detect TimeOut
- * we have 2 way to stop filling:
- * 1. time out
- * 2. detect Midtank is full.
- *
- * 1. time out process is:
- *      1. detect timeout after 5 minutes
- *      2. close and open the valves
- *      3. count 5 minutes again
- *2. measure MidTank capacity
- *  if MidTank is full stop count
- *
- * @return IFS_INK_presence_sensor or WHS_no_event event
- */
-WHS_sensor CartridgeInkTimeOutCallBackFunction()
+bool CartridgeInkFull()
 {
-    WHS_sensor ret = WHS_no_event;
     double MidTank_Pressure = 0;
     int int_MidTank_Pressure_1000 = 0;
 
-
-    //Enable_MidTank_Pressure_Reading(IFS_info.Ink.cart_color);
-    MidTank_Pressure = Read_MidTank_Pressure_Sensor(IFS_info.Ink.cart_color);// Get_MidTank_Pressure_Sensor(IFS_info.Ink.cart_color);
-    //Disable_MidTank_Pressure_Reading(IFS_info.Ink.cart_color);
+    MidTank_Pressure = Read_MidTank_Pressure_Sensor(midTankCartColor);
     int_MidTank_Pressure_1000 =  ((int)(MidTank_Pressure*1000));
-    Report("------------ IFS_info.Ink.time_out and Pressure  -----------------", __FILE__,__LINE__,(IFS_info.Ink.time_out * SECONDS_5), RpMessage, (int_MidTank_Pressure_1000), 0);
-
-    if (IFS_info.Ink.time_out) IFS_info.Ink.time_out += 1; /*** counter  ***/
-
 
     /*** detect the MidTank capacity is full  ***/
-    //Read_MidTank_Pressure_Sensor(IFS_info.Ink.cart_color);
-    if (MidTank_Pressure >= IFS_info.MidTank_capacity  + CARTRIDGE_CAPATICY)
-    {
-        IFS_info.Ink.time_out = 0;
-        WHS_info.event = IFS_MidTankFull;
-        return IFS_MidTankFull;
-    }
-
-    /**** detect if the MidTank_Pressure is the same, 5 times   ****/
-    if (DetectIfCartridgeFinished(int_MidTank_Pressure_1000))
-    {
-        Report("------------ INK Cartrigde is empty (by average)  -----------------", __FILE__,__LINE__,(IFS_info.Ink.time_out * SECONDS_5), RpMessage, (int_MidTank_Pressure_1000), 0);
-        IFS_info.Ink.time_out = 0;
-        WHS_info.event = IFS_MidTankFull;
-        return IFS_MidTankFull;
-    }
-
-
-    int timeout = CARTRIDGE_INK_TIMEOUT;
-    int secondtimeout = CARTRIDGE_INK_TIMEOUTx2;
-    if (IFS_info.Ink.cart_color == MIDTANK_8)//lubricant
-    {
-        timeout = CARTRIDGE_INK_TIMEOUT*3;
-        secondtimeout = CARTRIDGE_INK_TIMEOUTx2 * 3;
-    }
-    /*** first TimeOut  ***/
-    if (IFS_info.Ink.time_out == timeout)
-    {
-        Report("------------ first IFS_info.Ink.time_out, close valves  -----------------", __FILE__, __LINE__, (IFS_info.Ink.time_out * SECONDS_5), RpMessage, (int_MidTank_Pressure_1000), 0);
-        //IFS_info.Ink.time_out = 0;
-        //return IFS_info.event = IFS_Timeout;
+    //Read_MidTank_Pressure_Sensor(IFS_info.Ink.midTankCartColor);
+    if (MidTank_Pressure >= midTankCapacity  + CARTRIDGE_CAPATICY)
+        return true;
 
-        // try again :
-        // start timeout
-        // close and open the correct valves
-
-        IFS_info.Ink.time_out = 0;
-        MidTankValvesAction(Cartridge_MidTank_OFF);
-        IFS_info.Ink.time_out = timeout;
+    if (CartridgeFinishedCount++>=CartridgeCheckTime ) {
+        CartridgeFinishedCount = 0;
+        Report("IFS_filling ", __FILE__,__LINE__,int_MidTank_Pressure_1000, RpMessage, int_MidTank_Pressure_1000, 0);
+        /**** detect if the MidTank_Pressure is the same, 5 times   ****/
+        if (DetectIfCartridgeFinished(int_MidTank_Pressure_1000))
+            return true;
     }
-
-    if (IFS_info.Ink.time_out == (timeout + 1)) //need to open the valve for second chance
-        {
-        Report("------------ IFS_Timeout open valves for second timeout  -----------------", __FILE__, __LINE__, (IFS_info.Ink.time_out * SECONDS_5), RpMessage, (int_MidTank_Pressure_1000), 0);
-        MidTankValvesAction(Cartridge_MidTank_ON);
-        IFS_info.Ink.time_out = timeout;
-        }
-
-    if (IFS_info.Ink.time_out >= (secondtimeout + 1))
-        {
-            Report("------------ IFS_Timeout_Second_Time  -----------------", __FILE__, __LINE__, (IFS_info.Ink.time_out * SECONDS_5), RpMessage, (int_MidTank_Pressure_1000), 0);
-            IFS_info.Ink.time_out = 0;
-            MidTankValvesAction(Cartridge_MidTank_OFF);
-            WHS_info.event = IFS_Timeout_Second;
-            return IFS_Timeout_Second;
-        }
-    return ret;
+    return false;
 }
+#endif
 
-/**
- *
- * @brief IFS Time Out Alarm
- *
- * @return OK
- */
-bool IFS_TimeOutAlarm(bool status)
+bool midTankCart_door()
 {
-    bool ret =  OK;
-
-        AlarmHandlingSetAlarm( EVENT_TYPE__MID_TANK_1_FILL_TIMEOUT+IFS_info.Ink.cart_color,  status);
-
-
-        return ret;
-}
+    bool door;
 
-/**
- *
- * @brief MidTank Valves Action
- * the MidTank have 2 Valves:
- * 1. Ink Color Valve to get INK from the INK Cartridge to Midtank
- * 2. Air valve to get INK in the Midtank we have to open the Air valve so the air can go out
- *
- * @return action to open the Valve use: Cartridge_MidTank_ON/Atm_MidTank_ON
- *                to close the Valve use : Cartridge_MidTank_OFF/Atm_MidTank_OFF
- */
-bool MidTankValvesAction(bool action) //Cartridge_MidTank_ON of Cartridge_MidTank_OFF
-{
-    bool ret =  false;
-    if (action == Cartridge_MidTank_ON)
+    if(Get_COVER_1_State(CartridgesDoor))//to check the polarity
     {
-        Enable_MidTank_Pressure_Reading(IFS_info.Ink.cart_color);
+        door = OPEN;
     }
     else
     {
-        Disable_MidTank_Pressure_Reading(IFS_info.Ink.cart_color);
+        door = CLOSE;
     }
-    if (IDS_IsHomingActive(IFS_info.Ink.cart_color)==false)
-        Valve_Set(IDS_Id_to_AirValve[IFS_info.Ink.cart_color], action );   //Atm_MidTank_OFF/ON
-    Valve_Set(IDS_Id_to_CartrideValve[IFS_info.Ink.cart_color], action );   //Atm_MidTank_OFF/ON
-
-    ret = true;
-    return ret;
-}
-
-bool ColorMatch()
-{
-    bool ret =  false;
-
-    ret = true;
-    return ret;
+    return door;
 }
 
-char CartridgeRequestToken[36+1];
-int  CartridgeRequestId = 0x01010101;
-
 
-/**
- *
- * @brief create message to PPC
- *  temporary message RFID will replace that message.
- *  send a request to PPC to know the MidTank number
- * @return OK
- */
-uint32_t ReadCartridgeData(cartridge_name cart_name)
+uint32_t ReadInkCartridgeData()
 {
     //this function triggers reading and validation of the cartridge information
     // for now it sends a request to the tablet to inquire for the ink color/midtank id
@@ -306,16 +198,9 @@ uint32_t ReadCartridgeData(cartridge_name cart_name)
     return OK;
 }
 
-/**
- *
- * @brief PPC answers the MidTank number and call WasteTankCBFunction() function.
- *  temporary message RFID will replace that message.
- *
- * @return OK
- */
 bool CartridgeValidationResponseFunc(MessageContainer* requestContainer)
 {
-    bool ret = notOK;
+    bool ret = ERROR;
 
     CartridgeValidationResponse* response = cartridge_validation_response__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
 
@@ -324,118 +209,150 @@ bool CartridgeValidationResponseFunc(MessageContainer* requestContainer)
     // 2. the index is out of rang
     if (response == NULL)
     {
-        ReportWithPackageFilter(WHS_filling,"------------ IFS_filling message from PPC is null!!! -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
+        Report("IFS_filling message from PPC is null!!!", __FILE__,__LINE__,0, RpMessage, 0, 0);
         return ret;
     }
 
     if (response->index < NUM_OF_MIDTANKS)
     {
-        IFS_info.Ink.cart_color = response->index; // midtank 1-8
-        ReportWithPackageFilter(WHS_filling,"------------ IFS get message response->index is  -----------------", __FILE__, __LINE__, response->index, RpMessage, 0, 0);
-        WHS_info.event = IFS_INK_authentication_pass;
+        midTankCartColor = response->index; // midtank 1-8
+        Report("IFS get message response->index is", __FILE__, __LINE__, response->index, RpMessage, 0, 0);
+        inkAuthenticationPass = true;
     }
     else
     {
-        ReportWithPackageFilter(WHS_filling,"------------ IFS get message response->index is out of rang -----------------", __FILE__, __LINE__, response->index, RpMessage, 0, 0);
-        WHS_info.event = IFS_INK_authentication_fail;
+        Report("IFS get message response->index is out of rang", __FILE__, __LINE__, response->index, RpMessage, 0, 0);
+        inkAuthenticationPass = false;
     }
-
-    WasteTankCBFunction();//call cart is ok
-    ret = OK;
-
-return ret;
+    return OK;
 }
 
-
-/**
- *
- * @brief that response is from STUB. replace the PPC
- *  use stub : Poweroff_progress.cs from P:\Users - Public\Shai_Frieder
- *  ProgressRequest progressRequest = new ProgressRequest();
- *  progressRequest.Amount = 0xCC; //porgress id is 0xCC
- *  progressRequest.Delay = 1; //midtank number from 0-7
- *
- *
- * @return OK
- */
 void ResponseDemo(int MidtankId)
 {
-    WHS_info.event = IFS_INK_authentication_pass;
-    //global mid tank id = MidtankId;
-    IFS_info.Ink.cart_color = MidtankId;
-    WasteTankCBFunction();//call cart is ok
+    inkAuthenticationPass = true;
+    midTankCartColor = MidtankId;
 }
-uint32_t MidTankReadControlId = 0xFF;
-int MidTankCounter = 0;
-uint32_t MidTankCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
+
+void midTankStateMachine(void)
 {
-    int MidTankOpenAir = 8;
-    int MidTankReadPressure = 16;
-    int MidTankCloseAir = 40;
-    int MidTankEnd = 48;
-    int portId;
+    static int init = 1;
+    static int authenticationTimeOutCounter = 0;
 
-    if (MidTankCounter >= MidTankEnd)
-    {
-        SafeRemoveControlCallback(MidTankReadControlId, MidTankCallBackFunction);
+    if (init) {
+        doorState = midTankCart_door();
+        prevDoorState = doorState;
+        init = 0;
+    } else {
+#ifndef IFS_DEBUG
+        doorState = midTankCart_door();
+#endif
     }
-    else if (MidTankCounter >= MidTankCloseAir)
-    {
-        //close air valve for midtank (MidTankCounter-MidTankCloseAir)
-        portId = (MidTankCounter - MidTankCloseAir) ; //0-7
-        if (IDS_IsHomingActive(portId) == false)
-        {
-            Disable_MidTank_Pressure_Reading(portId);
-            Valve_Set(IDS_Id_to_AirValve[portId], Atm_MidTank_OFF );   //Atm_MidTank_OFF/ON
+    if (!RdInkCartridgeSensor()) { // cartridge not presence
+        Pannel_Leds( CART_1,  MODE_OFF);
+        cart1.color = colorOFF;
+        if (IFS_MidTankFilling()) {
+            MidTankValvesAction(Cartridge_MidTank_OFF);
+            Report("Cartridge extracted during filling!!!", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
+            Report("Mid-tank # filling fail", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
+            midTankState = MidTankStateIdle;
         }
-    }
-    else if (MidTankCounter >= MidTankReadPressure)
-    {
-        //read pressure for midtank (MidTankCounter-MidTankCloseAir)
-        for (portId = 0;portId < MAX_SYSTEM_DISPENSERS;portId++)
-        {
-            Read_MidTank_Pressure_Sensor(portId);
+    } else { // cartridge presence
+        if (cart1.color == colorOFF) {
+            Pannel_Leds( CART_1,  MODE_ON);
+            cart1.color = colorON;
         }
     }
-    else if (MidTankCounter >= MidTankOpenAir)
+    switch (midTankState)
     {
-        //open air valve for midtank (MidTankCounter-MidTankOpenAir)
-        portId = (MidTankCounter - MidTankOpenAir) ; //0-7
-        Enable_MidTank_Pressure_Reading(portId);
-        Valve_Set(IDS_Id_to_AirValve[portId], Atm_MidTank_ON );   //Atm_MidTank_OFF/ON
-    }
-    MidTankCounter++;
-    return OK;
-}
+        case MidTankStateIdle:
+            if ((doorState == CLOSE) && (prevDoorState == OPEN)) {
+                //check if power down in process
+                if (PowerOffInProcessGetState()) {
+                    Report("Power-down in process", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+                    break;
+                }
 
-uint32_t MidTankReading(void)
-{
-    MidTankCounter = 0;
-    if (IFS_info.Ink.time_out > 0)
-        return ERROR;
-    MidTankReadControlId = AddControlCallback("MidTankReading", MidTankCallBackFunction, 300/*eHundredMillisecond*/, TemplateDataReadCBFunction,0,0, 0 );
+                //check RFID? (TBD)
 
-    return OK;
-}
+                // send request for validation to ppc
+                inkAuthenticationPass = 2; //not defined
+                authenticationTimeOutCounter = 0;
+                ReadInkCartridgeData();
 
-bool  INKCartridgeAuthentication(cartridge_name cart_name)
-{
-    bool ret = notOK;
-    // TBD;
-    switch (cart_name)
-    {
-        case INK_cartridge:
-            IFS_info.Ink.autheticate = PASSED;  //todo
-            //Pannel_Leds(CART_1, MODE_ON);
+                //move to next state
+                midTankState = MidTankStateWait;
+                Report("move to midTankStateWait", __FILE__, prevDoorState, doorState, RpMessage, midTankState, 0);
+            }
+            if (doorState != prevDoorState) {
+                prevDoorState = doorState;
+            }
             break;
+        case MidTankStateWait:
+            if (inkAuthenticationPass == 2) {
+                authenticationTimeOutCounter++;
+                Report("MidTank Cartridge authentication wait...", __FILE__, __LINE__, authenticationTimeOutCounter, RpMessage, 0, 0);
+                if (authenticationTimeOutCounter > 60) {
+                    midTankState = MidTankStateIdle;
+                }
+                break;
+            }
+            if (inkAuthenticationPass == false) {
+                Report("MidTank Cartridge authentication fail", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+                midTankState = MidTankStateIdle;
+                break;
+            }
+
+            midTankCapacity = Get_MidTank_Pressure_Sensor(midTankCartColor);
+            if (midTankCapacity > MidTankEmptyLimit)
+            {
+                Pannel_Leds(CART_1, MODE_ON);
+                cart1.color = fastBILNK;
+                Report("Mid-tank not empty", __FILE__, __LINE__, midTankCapacity, RpMessage, MidTankEmptyLimit, 0);
+                midTankState = MidTankStateIdle;
+                break;
+            }
+            Report("Mid-tank filling in process", __FILE__,__LINE__,(int)(MidTankEmptyLimit*1000), RpMessage, (int)(midTankCapacity*1000), 0);
+            Pannel_Leds(CART_1, MODE_ON);
+            cart1.color  = BLINK;
+
+            if (midTankCartColor == MIDTANK_8)
+            {
+                CartridgeInkTimeout = CARTRIDGE_INK_TIMEOUT*4;
+                CartridgeCheckTime = FIVE_SECONDS*3;
+            }
+            // set valve selection
+            MidTankValvesAction(Cartridge_MidTank_ON);
+            Report("midTankStateWait set valve", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+
+            // RFID change status - TBD
 
+            // set count down
+            midTankTimeoutCounter = 1;
+
+            //start  filling
+            midTankState = MidTankStateFilling;
+            break;
+        case MidTankStateFilling:
+            // if time out or emptying done - idle
+            if (midTankTimeoutCounter++ > CartridgeInkTimeout) {
+                midTankTimeoutCounter = 0;
+                Pannel_Leds(CART_1, MODE_ON);
+                cart1.color = fastBILNK;
+                MidTankValvesAction(Cartridge_MidTank_OFF);
+                Report("Mid-tank # filling fail", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
+                Report("Call customer support", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
+                midTankState = MidTankStateIdle;
+            } else if (CartridgeInkFull()) {
+                midTankTimeoutCounter = 0;
+                Pannel_Leds(CART_1, MODE_OFF);
+                cart1.color = colorOFF;
+                MidTankValvesAction(Cartridge_MidTank_OFF);
+                Report("Mid-tank # filling done", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
+                midTankState = MidTankStateIdle;
+            }
+            break;
         default:
-            //WHS_info.cartridge_1.autheticate = FAILED; //todo
-           // WHS_info.cartridge_2.autheticate = FAILED; //todo
-            //Pannel_Leds(CART_2, MODE_OFF);
-            //Pannel_Leds(CART_3, MODE_OFF);
+            Report("wrong state", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
             break;
     }
-    ret = OK;
-    return ret;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h
index 6225dc1d4..bf42c0e9a 100644
--- a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h
+++ b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.h
@@ -11,55 +11,17 @@
 #ifndef MODULES_IFS_IFS_H_
 #define MODULES_IFS_IFS_H_
 
-#include "Modules/Waste/Waste.h"
+#define CARTRIDGE_INK_TIMEOUT           420
+#define MidTank_Pressure_EMPTY          0.5
+#define CARTRIDGE_CAPATICY              1500
 
-//void IFS_Init(void)
-#define SECONDS_5                       5               /**<  5seconds  */
-#define SECONDS_300                     300             /**<  300 seconds = 5 minutes  */
-#define SECONDS_420                     (7*60)          /**<  420 seconds = 7 minutes  */
-#define SECONDS_600                     600             /**<  600 seconds = 10 minutes  */
-#define SECONDS_2_DLY                   0               /**< dly 0 seconds  */
-#define CARTRIDGE_INK_TIMEOUT           (SECONDS_420 / SECONDS_5)             /**< TimeOut (7 minutes) for finishing transfer the Ink from Ink cartridge to MidTank  */
-#define CARTRIDGE_INK_TIMEOUTx2         (CARTRIDGE_INK_TIMEOUT * 2)           /**< TimeOut (7 minutes) for second try finishing transfer the Ink from Ink cartridge to MidTank  */
-#define CartridgeInkTimeOutCallBackTime (eOneSecond * SECONDS_5)
-#define CartridgeInkCallBackTime        eOneSecond
-#define MidTank_Pressure_EMPTY          0.3  //todo get from Moti the correct number  (Oleg -> 300cc)
-#define MidTank_PP_Pressure_EMPTY       0.5  //todo get from Moti the correct number  (Oleg -> 300cc)
-#define CARTRIDGE_CAPATICY 1500 //todo get from Moti the correct number
-#define FULL 1000   //todo get from Moti the correct number
-
-
-
-/**
- *  A structure to represent IFS_information
- */
-struct IFS_information
-{
-    struct cartridge_params Ink;                                /**< cartridge_params */
-    CarteidgPrecenseSensorStatus IFS_Ink_precense_sensor;       /**< INK cartridge presence(1) or not_precense(0) */
-    uint32_t Cartridge_Ink_device_Id;                           /**< INK device_Id number for callback */
-    uint32_t Cartridge_Ink_TimeOut_device_Id;                   /**< INK device_Id number for TimeOut callback */
-    uint32_t INK_isOK_flag ;                                    /**< INK cartridge pass authentication (from PPC) */
-    double MidTank_capacity;                                    /**< MidTank current capacity  */
-};
-
-struct IFS_information IFS_info;
-
-WHS_sensor CartridgeInkCallBackFunction();
-WHS_sensor CartridgeInkTimeOutCallBackFunction();
-uint32_t ReadCartridgeData(cartridge_name cart_name);
-bool INKCartridgeAuthentication(cartridge_name cart_name);
-bool MidTankValvesAction(bool action); //Cartridge_MidTank_ON of Cartridge_MidTank_OFF
-bool ColorMatch();
-bool IFS_TimeOutAlarm(bool status);
-void ResponseDemo(int MidtankId);
-bool IFS_MidTankFilling(void);
-MidTank_t IFS_MidTankIsActive(void);
-
-bool IFS_CartridgeLowerPresent();
-
-bool CartridgeValidationResponseFunc(MessageContainer* requestContainer);
+typedef enum{
+    MidTankStateIdle,
+    MidTankStateWait,
+    MidTankStateFilling
+}MidTankStateEnum;
 
+void midTankStateMachine(void);
 uint32_t MidTankReading(void);
-
+void ResponseDemo(int MidtankId);
 #endif /* MODULES_IFS_IFS_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
index 4a7008a7a..e5afba3b9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
@@ -63,6 +63,7 @@ extern uint8_t Test_Read_Buf[4];
 
 extern uint8_t Head_Fan_PWM_Command[2];
 extern uint16_t Head_Fan_Tach[2];
+extern bool doorState;
 
 bool RestartNeeded = false;
 
@@ -1051,6 +1052,16 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
             response.has_progress = true;
         }
     else
+    if(request->amount == 0xB9)  //fast refresh for pressure
+    {
+        if ((request->delay == 0) || (request->delay == 1)) {
+            doorState = request->delay;
+            LOG_ERROR(request->delay,"door");
+        }
+        response.progress = request->delay;
+        response.has_progress = true;
+    }
+    else
     if(request->amount == 0xC3)  //suspend I2C task
     {
         if (request->delay == 0)
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
index 2fdd3a479..7d6837018 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_ex.h
@@ -1,5 +1,5 @@
-#ifndef WASTE_MAINT_H
-#define WASTE_MAINT_H
+#ifndef WASTE_EX_H
+#define WASTE_EX_H
 
 typedef enum {
     CartridgeStateOUT,
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c
index 03ee73daf..870463321 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_init.c
@@ -1,4 +1,5 @@
 /*   ----  WHS      */
+#if 0
 #include <Container.h>
 #include <DataDef.h>
 #include "include.h"
@@ -1446,7 +1447,7 @@ U8 WHS_HW_test()
     return ret;
 }
 
-
+#endif
 /*------------------------------------------------------------------------------------------------*/
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
index 099fc0c2e..dc76a3390 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
@@ -32,6 +32,10 @@ uint32_t Cartridge_Cover_Control;
 button *CartLed[MaxWasteCartridges] = {0, &cart2, &cart3};
 WasteTankStateEnum WasteTankState = WasteTankStateIdle;
 
+int init = 1;
+
+bool setWasteValve (WasteCartridgeEnum WasteCartridge);
+
 #define WASTE_CARTRIDGE_SIZE 1500
 #define WASTE_LEVEL_OVERFLOW 2700
 #define WASTE_LEVEL_FULL 2300
@@ -65,6 +69,8 @@ void cartCART_EXTRACTED(WasteCartridgeEnum CartId) //{called from polling functi
     Report("Cartridges Cart extracted", __FILE__, __LINE__, CartId, RpMessage, CartState[CartId], 0);
     CartState[CartId] = CartridgeStateOUT;
     CartLed[CartId]->color = colorOFF;
+    if (init==0)
+        WHS_Set_IFS_Clearing_Suction(NUM_OF_MIDTANKS);
 
 }
 WasteCartridgeEnum cartSELECT_CART(void) //{ if there is no SELECTED cartridge select the first cartridge in IN state. If there is none, return -1}
@@ -113,6 +119,8 @@ void cartFILLING_END(void)
     } else {
         Report("Cartridges end with no active cartridge", __FILE__, __LINE__, CartState[WasteCartridge_middle], RpMessage, CartState[WasteCartridge_lower], 0);
     }
+    setWasteValve(WasteCartridge_middle);
+
 }
 
 CartridgeStateEnum cartGetState(WasteCartridgeEnum CartId)
@@ -163,6 +171,119 @@ WasteCartridgeEnum cartNotAllFull()
     return false;
 }
 
+bool SetWastePump( bool power)
+{
+    if ( power == OPEN ) {
+        if (WHS_Type == WHS_TYPE_UNKNOWN) {
+            Pumps_Control(WHS_WTANKPUMP2, OPEN); //waste_pump_power_on();
+        } else { //new WHS
+            Trigger_SetWHSPump(OPEN);
+        }
+        return OPEN;
+    } else {
+        if (WHS_Type == WHS_TYPE_UNKNOWN) {
+            Pumps_Control(WHS_WTANKPUMP2, CLOSE); //waste_pump_power_on();
+        } else { //new WHS
+            Trigger_SetWHSPump(CLOSE);
+        }
+        return CLOSE;
+    }
+}
+
+bool setWasteValve (WasteCartridgeEnum WasteCartridge)
+{
+    bool direction;
+
+    if (WasteCartridge == WasteCartridge_lower) {
+        direction = true;
+    } else {
+        direction = false;
+    }
+
+    if (WHS_Type == WHS_TYPE_UNKNOWN) {
+        Valve_Set(VALVE_WASTE_TANK,direction);
+    } else { //new WHS
+        Trigger_SetWHSValveWatseCartridge(direction);
+    }
+    return direction;
+}
+
+bool RdWasteTankEmptySensor()
+{
+    //double WasteLevel = 0.0;
+
+    if (WHS_Type == WHS_TYPE_UNKNOWN) {
+        return WHS_GPI_WCONTAINER_WARN();
+    } else { //new WHS
+        //WasteLevel = GetWHSWasteTankLevelMiliLiter();
+        if (wasteLevel < wasteLevelEmpty)
+            return 1; //Empty
+    }
+    return 0;
+}
+
+bool RdWasteTankFullSensor()
+{
+    //double WasteLevel = 0.0;
+
+    if (WHS_Type == WHS_TYPE_UNKNOWN) {
+        return WHS_GPI_WCONTAINER_FULL();
+    } else { //new WHS
+        //WasteLevel = GetWHSWasteTankLevelMiliLiter();
+        if (wasteLevel >= wasteLevelFull)
+            return 1; //full
+    }
+    return 0;
+}
+
+bool RdWasteTankOverFlowSensor()
+{
+    //double WasteLevel = 0.0;
+
+    if (WHS_Type == WHS_TYPE_UNKNOWN) {
+        return WHS_GPI_WASTE_OVERFULL();
+    } else {//new WHS
+        //WasteLevel = GetWHSWasteTankLevelMiliLiter();
+        if (wasteLevel > wasteLevelOverflow)
+            return 1; //overflow
+    }
+    return 0;
+}
+
+bool WHS_IsContainerFull()
+{
+    return RdWasteTankFullSensor();
+}
+
+bool WHS_IsContainerEmpty()
+{
+    return RdWasteTankEmptySensor();
+}
+
+bool WHS_IsContainerOverflow()
+{
+    return RdWasteTankOverFlowSensor();
+}
+
+bool WHS_WasteCartridgeLowerPresent()
+{
+    return Is_Cartridge_Present(CART_3);
+}
+
+bool WHS_WasteCartridgeMiddlePresent()
+{
+    return Is_Cartridge_Present(CART_2);
+}
+
+bool WHS_IsEmptying()
+{
+    if (WasteTankState == WasteTankStateEmptying) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
 void Waste_Init()
 {
     CartState[WasteCartridge_middle] = CartridgeStateOUT;
@@ -186,7 +307,6 @@ bool cartCart_door()//Polled by polling function. Notify waste. Poll each 1 seco
     return DoorState;
 }
 
-//Waste_DoorOpenDuringEmptying
 uint32_t Waste_DoorOpenDuringEmptying(uint32_t IfIndex, uint32_t ReadValue)
 {
     cartCart_door();
@@ -212,7 +332,6 @@ uint32_t Waste_DoorOpenDuringEmptying(uint32_t IfIndex, uint32_t ReadValue)
 
 void Waste_StateMachine_OneSecond_Call(void)
 {
-    static int init = 1;
     static WasteTankStateEnum prev_state = 0;
     static int prev_wasteLevel = 0;
 
@@ -263,7 +382,7 @@ void Waste_StateMachine(void)
     switch (WasteTankState)
     {
         case WasteTankStateIdle:
-            if (wasteLevel > wasteLevelOverFlow) {
+            if(WHS_IsContainerOverflow()) {
                 AlarmHandlingSetAlarm( EVENT_TYPE__WASTE_CONTAINER_OVERFLOW,  true);
             } else {
                 AlarmHandlingSetAlarm( EVENT_TYPE__WASTE_CONTAINER_OVERFLOW,  false);
@@ -271,7 +390,7 @@ void Waste_StateMachine(void)
             if (cartNotAllFull() == true) {
                 AlarmHandlingSetAlarm( EVENT_TYPE__ALL_WASTE_CARTRIDGES_FULL,  false);
             }
-            if (wasteLevel > wasteLevelFull) {
+            if(WHS_IsContainerFull()) {
                 //check if power down in process
                 if ( PowerOffInProcessGetState() ) {
                     Report("Power-down in process", __FILE__, __LINE__, 0, RpMessage, 0, 0);
@@ -307,9 +426,8 @@ void Waste_StateMachine(void)
 
             // set valve selection
             activeCart = cartGetActiveCart();
-            Valve_Set(VALVE_WASTE_TANK, activeCart);
-            Report("WasteTankStateEmptying set valve", __FILE__, __LINE__, activeCart, RpMessage, 0, 0);
-
+            //Report("WasteTankStateEmptying set valve", __FILE__, __LINE__, activeCart, RpMessage, 0, 0);
+            setWasteValve(activeCart);
             // RFID change status - TBD
 
             // set count down
@@ -317,6 +435,7 @@ void Waste_StateMachine(void)
 
             // set emptying limit
             wasteLevelEmpty = wasteLevel - WASTE_CARTRIDGE_SIZE;
+            waste_seq_step1_cont();
 
             // start a timer to observe door opening
             Cartridge_Cover_Control = AddControlCallback("Cartridge Cover Door", Waste_DoorOpenDuringEmptying, 100,  cartCart_door, 0, 0, 0);
@@ -338,15 +457,18 @@ void Waste_StateMachine(void)
                 Report("WasteTankStateEmptying TimeOut", __FILE__, __LINE__, 0, RpMessage, 0, 0);
                 AlarmHandlingSetAlarm( EVENT_TYPE__WASTE_CONTAINER_EMPTYING_TIMEOUT,  true);
                 RemoveControlCallback(Cartridge_Cover_Control, Waste_DoorOpenDuringEmptying);
-            } else if (wasteLevel < wasteLevelEmpty) {
+                timeout_counter = 0;
+            } else if (WHS_IsContainerEmpty()) {
                 SetWastePump(CLOSE);
                 cartFILLING_END();
+                waste_seq_cont_stop();
                 // RFID change status - TBD
                 WasteTankState = WasteTankStateIdle;
                 RemoveControlCallback(Cartridge_Cover_Control, Waste_DoorOpenDuringEmptying);
                 if (cartNotAllFull() == false)
                     AlarmHandlingSetAlarm( EVENT_TYPE__ALL_WASTE_CARTRIDGES_FULL,  true);
                 Report("WasteTankStateEmptying done", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+                timeout_counter = 0;
             }
 #ifdef WHS_DEBUG
             wasteLevel -= 100;
@@ -355,6 +477,21 @@ void Waste_StateMachine(void)
             break;
         case WasteTankStatePaused:
             Report("WasteTankStatePaused...", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+            activeCart = cartGetActiveCart();
+            if (activeCart == MaxWasteCartridges)
+            {
+                SetWastePump(CLOSE);
+                cartFILLING_END();
+                waste_seq_cont_stop();
+                // RFID change status - TBD
+                WasteTankState = WasteTankStateIdle;
+                RemoveControlCallback(Cartridge_Cover_Control, Waste_DoorOpenDuringEmptying);
+                if (cartNotAllFull() == false)
+                    AlarmHandlingSetAlarm( EVENT_TYPE__ALL_WASTE_CARTRIDGES_FULL,  true);
+                Report("WasteTankStatePaused done", __FILE__, __LINE__, 0, RpMessage, 0, 0);
+                timeout_counter = 0;
+
+            }
             break;
         default:
             Report("wrong state", __FILE__, WasteTankState, 0, RpMessage, SelectedCard, 0);