Merge branch 'master' of https://twinetfs.visualstudio.com/Tango/_git/Tango

69 files changed, 3417 insertions, 392 deletions

diff --git a/Software/Embedded_SW/Embedded/Common/SWUpdate/FileSystem.c b/Software/Embedded_SW/Embedded/Common/SWUpdate/FileSystem.c
index 76c2fe9f2..77efa1cfd 100644
--- a/Software/Embedded_SW/Embedded/Common/SWUpdate/FileSystem.c
+++ b/Software/Embedded_SW/Embedded/Common/SWUpdate/FileSystem.c
@@ -653,7 +653,7 @@ uint32_t    GetFilesRequestFunc(MessageContainer* requestContainer)
     GetFilesRequest* request = get_files_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
 
     GetFilesResponse response = GET_FILES_RESPONSE__INIT;
-
+    //struct tm RTC_time;
     #define MAX_NUM_OF_FILES 20
     DIR dir;
     FILINFO* fno[MAX_NUM_OF_FILES];
@@ -727,6 +727,13 @@ uint32_t    GetFilesRequestFunc(MessageContainer* requestContainer)
               else
                usnprintf(&FullPath[i], 50,  "%s%s%s", request->path,"/", fno[i]->fname);
            Data[i].fullpath = &FullPath[i][0];
+           /*RTC_time.tm_year=(fno[i]->fdate>>9)+1980;//populate the time struct (FAT start==1980, RTC.year==0)
+           RTC_time.tm_mon=(fno[i]->fdate>>5)&0x000F;
+           RTC_time.tm_mday=fno[i]->fdate&0x001F;
+           RTC_time.tm_hour=(fno[i]->ftime>>11)&0x001F;
+           RTC_time.tm_min=(fno[i]->ftime>>5)&0x003F;
+           RTC_time.tm_sec=(fno[i]->ftime<<1)&0x003E;*/
+
 
         }
         response.n_items = NumOfFiles;
diff --git a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c
index fd6889185..8d2d9462a 100644
--- a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c
+++ b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c
@@ -20,7 +20,7 @@ typedef struct
 } TangoVersion_t;
 
 
-TangoVersion_t _gTangoVersion = {1,5,2,0};
+TangoVersion_t _gTangoVersion = {1,5,3,0};
 #define BUILD_DATE __DATE__
 char Dat[50] = BUILD_DATE; 
 
diff --git a/Software/Embedded_SW/Embedded/Common/Utilities/ustdlib.c b/Software/Embedded_SW/Embedded/Common/Utilities/ustdlib.c
index 420e1dbc3..749152b2e 100644
--- a/Software/Embedded_SW/Embedded/Common/Utilities/ustdlib.c
+++ b/Software/Embedded_SW/Embedded/Common/Utilities/ustdlib.c
@@ -819,7 +819,7 @@ usnprintf(char * restrict s, size_t n, const char * restrict format, ...)
     //
     // Call vsnprintf to perform the conversion.
     //
-    ret = uvsnprintf(s, n, format, arg);
+    ret = vsnprintf(s, n, format, arg);
 
     //
     // End the varargs processing.
diff --git a/Software/Embedded_SW/Embedded/Communication/Connection.c b/Software/Embedded_SW/Embedded/Communication/Connection.c
index af60541aa..5396938fd 100644
--- a/Software/Embedded_SW/Embedded/Communication/Connection.c
+++ b/Software/Embedded_SW/Embedded/Communication/Connection.c
@@ -53,9 +53,25 @@ void StopRecurringReports(void)
     PowerUpStopReporting();
     MachineUpdateStopReporting();
     PowerDownStopReporting();
+    HomingStopReporting();
 
     KeepAliveActive = false;
 }
+struct tm LocalTime;//Months since January - [0,11]
+
+TimeAScii LocalTimeInAScii;
+
+void LocalTimeToAScii()//'D''D''/''M''M''/''Y''Y' Nedded for RFID
+{
+    LocalTimeInAScii.Byte.YearL  = '0' + LocalTime.tm_year%10;
+    LocalTimeInAScii.Byte.YearH  = '0' + (LocalTime.tm_year/10)%10;
+    LocalTimeInAScii.Byte.SlashM = '/';
+    LocalTimeInAScii.Byte.MonthL = '0' + (LocalTime.tm_mon)%10;
+    LocalTimeInAScii.Byte.MonthH = '0' + ((LocalTime.tm_mon)/10)%10;
+    LocalTimeInAScii.Byte.SlashD = '/';
+    LocalTimeInAScii.Byte.DayL   = '0' + LocalTime.tm_mday%10;
+    LocalTimeInAScii.Byte.DayH   = '0' + (LocalTime.tm_mday/10)%10;
+}
 void ConnectionRequest(MessageContainer* requestContainer)
 {
     MessageContainer responseContainer;
@@ -83,6 +99,10 @@ void ConnectionRequest(MessageContainer* requestContainer)
 
     if (request->has_unixtime)
         utilsUpdateDateTime(request->unixtime);//(request->seconds);
+    ulocaltime(request->unixtime, &LocalTime);
+    LocalTime.tm_mon +=1;//since we get from ulocaltime Months since January - [0,11]
+
+    LocalTimeToAScii();
     StopRecurringReports();
     /*
     extern TangoVersion_t _gTangoVersion;
@@ -140,6 +160,7 @@ void DisconnectionRequest(MessageContainer* requestContainer)
     DiagnosticsStop();
     JobStopReporting();
     AlarmHandlingStop();
+    HomingStopReporting();
     //UART_ResetBuffers();
     //-------------------------------------------------------------------------------------------
     uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
diff --git a/Software/Embedded_SW/Embedded/Communication/Connection.h b/Software/Embedded_SW/Embedded/Communication/Connection.h
index c0a31344f..c4f8c1df2 100644
--- a/Software/Embedded_SW/Embedded/Communication/Connection.h
+++ b/Software/Embedded_SW/Embedded/Communication/Connection.h
@@ -9,6 +9,8 @@
 #ifndef COMMUNICATION_CONNECTION_H_
 #define COMMUNICATION_CONNECTION_H_
 
+extern TimeAScii LocalTimeInAScii;
+
 void ConnectionRequest(MessageContainer* requestContainer);
 void DisconnectionRequest(MessageContainer* requestContainer);
 void KeepAliveRequestFunc(MessageContainer* requestContainer);
@@ -17,5 +19,7 @@ void KeepAliveOneSecondCall(void);
 
 extern int KeepAliveOneSecondCounter;
 extern bool keepalivetest;
+extern struct tm LocalTime;
+extern TimeAScii LocalTimeInAScii;
 
 #endif /* COMMUNICATION_CONNECTION_H_ */
diff --git a/Software/Embedded_SW/Embedded/DataDef.h b/Software/Embedded_SW/Embedded/DataDef.h
index 755e5a962..91b3df6bf 100644
--- a/Software/Embedded_SW/Embedded/DataDef.h
+++ b/Software/Embedded_SW/Embedded/DataDef.h
@@ -29,8 +29,12 @@
 #define BTSR_NO_FEEDER_TFU
 #define BTSR_NO_PULLER_TFU
 #endif
-//#define USE_POWERSTEP01 -NA
 
+#define USE_RFID_STUB //stub only. to use it undef USE_RFID_LOGIC
+//#define RFID_READ_ONLY //
+#define USE_RFID_LOGIC //See more settings in RFID_Settings.h
+//#define USE_POWERSTEP01 -NA
+//#define LIGHT_COLORS
 //#define EMC_FORCE_MOT_CLOCK_INTERNAL
 
 //#define VAC_TEST
@@ -46,7 +50,7 @@ extern bool Special_Dispensers;
 
 #define USE_OLD_HEAD_EEPROM
 
-//#define USE_RFID
+
 //#define USE_VOC_BUZZER_ALARM
 
 #define I2C_2_Data_Transfer_Rate_400kbps
@@ -58,6 +62,7 @@ extern bool Special_Dispensers;
 
 #define NFC_HARDWARE_RESET 0x01
 
+#define REPEATED_INIT_FAILURE_LIMIT 10
 //#define RUN_AS_MAIN_JIG//(TEST_POWERSTEP01) send GATECFG parameters to work without sending parameters via machine studio + ADC + ADCINT
 
 #define ON  1
@@ -74,11 +79,12 @@ enum
     VERIFIED = 255
 };
 
-typedef enum Status_Enum
+typedef enum Status_Enum // don't change used for RFID
 {
-    S_NA,
-    S_SUCCEEDED,
-    S_FAILED
+    S_NA = 0,
+    S_SUCCEEDED = 1,
+    S_FAILED = -2,
+    S_NOTINUSE = -1
 }STATUS_ENUM;
 
 
@@ -304,6 +310,23 @@ typedef enum
 #define BIT30 0x40000000 //0x01 << 30
 #define BIT31 0x80000000 //0x01 << 31
 
+//LocalTimeInAScii
+typedef union
+{
+    struct
+    {
+        uint8_t DayH; //0
+        uint8_t DayL; //1
+        uint8_t SlashD; //2
+        uint8_t MonthH; //3
+        uint8_t MonthL; //4
+        uint8_t SlashM; //5
+        uint8_t YearH; //6
+        uint8_t YearL; //7
+    }Byte;
+    uint8_t Buf[8];
+    uint64_t Uint64;
+}TimeAScii;
 
 #define MAX_PWM_Command 100
 
@@ -319,6 +342,8 @@ typedef enum
     THREAD_LOAD ,
 }PANEL_BUTTON_OR_CRAT_ID;
 
+#define CART(readerID) ((PANEL_BUTTON_OR_CRAT_ID)(readerID+1))//readerID 0-2 , CRAT_ID 1-3
+
 typedef enum
 {
     MODE_OFF = 0,
@@ -635,9 +660,15 @@ Supports diagnostic <id> (--diag_suppress, - pds)
 #define HEAD6_PT100 TEMP_SENSE_AN_ENCLOSURETEMP3
 #define MIXER_PT100 TEMP_SENSE_ANALOG_MIXCHIP_TEMP
 
+#ifdef LIGHT_COLORS
+#define LUBRICANT_DISPENSER 9
+#define CLEANER_DISPENSER 9
+#define MAX_DYE_DISPENSERS MAX_SYSTEM_DISPENSERS
+#else
 #define LUBRICANT_DISPENSER 7
 #define CLEANER_DISPENSER 6
-//#define AUTO_HOME_DISPENSERS
+#define MAX_DYE_DISPENSERS 6
+#endif
 #define MAX_CARTRIDGES 3
 
 typedef enum
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
index 00502d3ba..080522d0d 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.c
@@ -1133,6 +1133,17 @@ uint8_t Init_Machine_Leds()
     return Status;
 }
 
+bool Is_AnyCartridge_presence()
+{
+    bool IsAnyCartPresent = true;
+
+    #ifndef EVALUATION_BOARD
+        if(F3_CARTx_PRES_02_Direct & (BIT5 | BIT6 | BIT7))
+            IsAnyCartPresent = false;
+    #endif
+
+    return IsAnyCartPresent;
+}
 
 bool Is_Cartridge_Present(PANEL_BUTTON_OR_CRAT_ID Cartridge)
 {
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h
index e707a9b39..7342f9998 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h
@@ -261,6 +261,7 @@ uint8_t Pannel_Leds(PANEL_BUTTON_OR_CRAT_ID Pannel_Led_Id, OPERATION_MODE LED_Mo
 uint8_t Init_Machine_Leds();
 
 bool Is_Cartridge_Present(PANEL_BUTTON_OR_CRAT_ID Cartridge);
+bool Is_AnyCartridge_presence();
 
 char Read_HW_Version(unsigned char *Brd_ID, unsigned char *Assy_ID);
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/Fan/Head_Fan.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/Fan/Head_Fan.h
index 394a69e1d..438d552ff 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/Fan/Head_Fan.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/Fan/Head_Fan.h
@@ -75,5 +75,6 @@ uint8_t Head_Fan_Read_Tacho(bool Fan);
 uint8_t Head_Read_Config_Reg();
 uint8_t Head_Fan_config();
 uint8_t Head_Fans_Init();
+uint8_t Head_Check_Fan_Controller_Type();
 
 #endif /* DRIVERS_I2C_COMMUNICATION_HEAD_CARD_FAN_HEAD_FAN_H_ */
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/Fan/fan_click.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/Fan/fan_click.c
index 728531488..ab68d6539 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/Fan/fan_click.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/Fan/fan_click.c
@@ -26,7 +26,7 @@ uint8_t readbuffer[2];
 uint8_t writebuffer[2];
 
 
-uint32_t I2C_HEAD_FAN_SLAVE_ADD = 0x5C;
+uint32_t I2C_HEAD_FAN_SLAVE_ADD = UNKNOWN;
 
 
 FAN_CLICK_INFO Fan_Click_Info;
@@ -195,6 +195,34 @@ uint8_t Head_fan_click_revision()
     return status;
 }
 
+uint8_t Head_Check_Fan_Controller_Type()
+{
+    uint8_t status = ERROR;
+
+    Select_Main_Head_Mux_Channel();
+
+    if(I2C_HEAD_FAN_SLAVE_ADD == UNKNOWN)
+    {
+        I2C_HEAD_FAN_SLAVE_ADD = 0x5C;
+        status = Head_fan_click_productID();
+
+        if((Fan_Click_Info.Product_ID != 0x36) || (status != OK))
+        {
+            I2C_HEAD_FAN_SLAVE_ADD = 0x5E;
+            status = Head_fan_click_productID();
+        }
+
+        if((Fan_Click_Info.Product_ID != 0x36) || (status != OK))
+        {
+            I2C_HEAD_FAN_SLAVE_ADD = 0x00;
+            ReportWithPackageFilter(GeneralFilter,"unable to recognize the Head fan controller", __FILE__,__LINE__, status, RpMessage, 0, 0);
+            status = ERROR;
+        }
+    }
+    return status;
+}
+
+
 uint8_t Head_Read_Config_Reg()
 {
     uint8_t status = OK;
@@ -296,6 +324,7 @@ uint8_t Head_Fans_Init()
 {
     uint8_t status = OK;
 
+    Head_Check_Fan_Controller_Type();
     Head_Read_Config_Reg();
     //Output is configured as a push-pull output
     EMC2302_Write.Reg.PWM_Output_Config |= 0x3;//Bit0 Output PWM1, Bit2 Output PWM2, 0 open drain, 1 push-pull
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.c
index 1bcf47fe8..c02683290 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.c
@@ -23,7 +23,9 @@
 #define I2C_DELAY 3000
 uint8_t Main_Mux_Flag = UNKNOWN;
 
-uint32_t I2C_BUSY_DELAY = 100;
+//uint32_t I2C_BUSY_DELAY = 100;
+uint32_t  I2C_2_3_BUSY_DELAY  = 3000;  //I2C2_BASE, I2C3_BASE  (3000 only for first communication changed to 100 in main)
+uint32_t  I2C_4_BUSY_DELAY    = 3000; //I2C4_BASE (RFID + Dispensers) (RFUD working only with 3000)
 
 void InitI2C2()
 {
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.h
index 56c386aa8..eb5ea03de 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.h
@@ -11,6 +11,10 @@
 extern uint32_t Arb_Loss_Count;
 
 extern uint32_t I2C_BUSY_DELAY;
+
+extern uint32_t I2C_2_3_BUSY_DELAY;  //I2C2_BASE, I2C3_BASE
+extern uint32_t  I2C_4_BUSY_DELAY; //I2C4_BASE (RFID + Dispensers)
+
 //#define I2C_BUSY_DELAY 100//15000
 
 uint32_t I2C_control(uint32_t I2C_ID, uint32_t Slave_Add, bool RW,uint32_t Byte2Write );
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Comm.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Comm.c
index 2dd26e98c..147778d05 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Comm.c
@@ -19,7 +19,7 @@
 #include "driverlib/sysctl.h"
 #include "driverlib/i2c.h"
 #include "inc/hw_i2c.h"
-//#include "i2c_fifo.h"
+#include "i2c_fifo.h"
 #include "i2c.h"
 
 uint32_t LastReadI2C_BASE; unsigned char LastReadaddr; unsigned char* LastReaddata; unsigned int LastReadlen;
@@ -44,6 +44,17 @@ uint32_t I2C_WriteBuff(uint32_t I2C_BASE, unsigned char addr, unsigned char* dat
 {
     uint32_t status = OK;
 
+    uint32_t I2C_BUSY_DELAY;
+
+    if(I2C_BASE == I2C4_BASE)
+    {
+        I2C_BUSY_DELAY = I2C_4_BUSY_DELAY;
+    }
+    else
+    {
+        I2C_BUSY_DELAY = I2C_2_3_BUSY_DELAY;
+    }
+
     if (I2CMasterBusy(I2C_BASE))
     {
         ReportWithPackageFilter(CommFilter,"I2C_WriteBuff busy", __FILE__,__LINE__,status, RpMessage, Task_self(), 0);
@@ -98,6 +109,17 @@ uint32_t I2C_ReadBuff(uint32_t I2C_BASE, unsigned char addr, unsigned char* data
 {
     uint32_t status = OK;
 
+    uint32_t I2C_BUSY_DELAY;
+
+    if(I2C_BASE == I2C4_BASE)
+    {
+        I2C_BUSY_DELAY = I2C_4_BUSY_DELAY;
+    }
+    else
+    {
+        I2C_BUSY_DELAY = I2C_2_3_BUSY_DELAY;
+    }
+
     if (I2CMasterBusy(I2C_BASE))
     {
         ReportWithPackageFilter(CommFilter,"I2C_ReadBuff busy", __FILE__,__LINE__, status, RpMessage, Task_self(), 0);
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_RFID_Dispenser_Task/I2C_RFID_Disp_Task.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_RFID_Dispenser_Task/I2C_RFID_Disp_Task.c
new file mode 100644
index 000000000..79d31393b
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_RFID_Dispenser_Task/I2C_RFID_Disp_Task.c
@@ -0,0 +1,445 @@
+/*
+ * I2C_RFID_Disp_Task.c
+ *
+ *  Created on: Oct 23, 2019
+ *      Author: shlomo
+ *
+ * Task for I2C4_BASE
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "include.h"
+#include "inc/hw_memmap.h"
+
+#include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
+#include <Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h>
+#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
+#include <Drivers/I2C_Communication/RFID_NFC/NFC.h>
+
+#include "drivers/I2C_Communication/Dispenser_Card/I2C_Dispenser_Card_Mux.h"
+#include "drivers/I2C_Communication/Dispenser_Card/EEPROM/Dispenser_EEPROM.h"
+
+
+Task_Handle DispRFID_Task_Handle;
+Mailbox_Handle          RFID_ReadingMsgQ = NULL;
+typedef enum
+{
+    //RFID
+//    RFID_ReaderAvailability,
+    RFID_Initialization,
+    RFID_PrepareForDiscovery,
+    RFID_Indetification,
+    RFID_GetUid,
+//    RFID_CheckPassword,
+    RFID_CheckValidity,
+    RFID_CheckUpdateCounter,
+    RFID_CheckHashValue,
+//    RFID_CheckUpdateBlocked,
+    RFID_ReadData,
+    RFID_CheckCRC,
+//    RFID_WriteValue,
+    RFID_UpdateData,
+    RFID_UpdateCheckSum,
+
+    RFID_Update_Ink_Fill,
+    RFID_Update_Ink_Used,
+    RFID_Update_Ink_Empty,
+    RFID_Update_Ink_Filled_MachineID,
+
+    RFID_Update_Waste_Fill,
+    RFID_Update_Waste_Full,
+    RFID_Update_Waste_Empty,
+    RFID_Update_Waste_Filled_MachineID,
+
+    RFID_UpdateWCounter,
+    RFID_UpdateCRC,
+
+    RFID_DisconnectComm,
+    RFID_TestTask,
+    RFID_End,
+    //-----------------------------
+    //Dispensers
+    DispenserReadData,
+    Dispensers_End,
+
+}RFID_ReadingMessages;
+
+typedef struct RFID_ReadingMessage{
+    RFID_ReadingMessages messageId;
+    uint32_t TargetID; //readerID / dispenserId
+    uint32_t parameter1;
+}RFID_ReadingMessageStruc;
+
+void Trigger_RFID_Init(/*RFID_READER_ID readerID*/)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_Initialization;
+    //RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_Prepare(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_PrepareForDiscovery;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_Discovery(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_Indetification;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_GetUid(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_GetUid;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_ReadData(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_ReadData;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_CheckCRC(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_CheckCRC;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_CheckHash(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_CheckHashValue;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_CheckValid(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_CheckValidity;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_CheckCounter(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_CheckUpdateCounter;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+
+
+void Trigger_RFID_UpdateInkFill(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_Update_Ink_Fill;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_UpdateInkUsed(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_Update_Ink_Used;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_UpdateInkEmpty(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_Update_Ink_Empty;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_UpdateInkMachineID(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_Update_Ink_Filled_MachineID;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_TestTask(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_TestTask;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_UpdateWasteFill(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_Update_Waste_Fill;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_UpdateWasteFull(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_Update_Waste_Full;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_UpdateWasteEmpty(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_Update_Waste_Empty;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_UpdateWasteMachineID(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_Update_Waste_Filled_MachineID;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_UpdateWCounter(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_UpdateWCounter;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+void Trigger_RFID_UpdateCRC(RFID_READER_ID readerID)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = RFID_UpdateCRC;
+    RFID_ReadingMessage.TargetID = readerID;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
+//----------------------------------------------------------------
+void Trigger_Dispenser_ReadData(int dispenserId)
+{
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+
+    RFID_ReadingMessage.messageId = DispenserReadData;
+    RFID_ReadingMessage.TargetID = dispenserId;
+    if (RFID_ReadingMsgQ != NULL)
+        Mailbox_post(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+//------------------------------------------------------------------
+void DispRFID_Task(UArg arg0, UArg arg1)
+{
+    RFID_READER_ID readerID;
+    uint8_t Dispenser_ID;
+
+    RFID_ReadingMessageStruc RFID_ReadingMessage;
+    //RFID_ReadingMsgQ = Mailbox_create(sizeof(RFID_ReadingMessageStruc), 10, NULL,NULL);
+    DispRFID_Task_Handle = Task_self();
+
+    while(1)
+    {
+        Mailbox_pend(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_WAIT_FOREVER);
+
+        /* select the correct I2C bus */
+        if (RFID_ReadingMessage.messageId < RFID_End)
+        {
+            readerID = (RFID_READER_ID)(RFID_ReadingMessage.TargetID);
+            SetReader(readerID);
+        }
+        else if (RFID_ReadingMessage.messageId < Dispensers_End)
+        {
+            Dispenser_ID = (uint8_t)(RFID_ReadingMessage.TargetID);
+            Select_Dispenser_Mux_Channel(Dispenser_ID);//
+        }
+        else
+        {
+            //TBWhen connecting to few targets in the message if used need  to open few channels in the mux
+        }
+
+        switch (RFID_ReadingMessage.messageId)
+        {
+            // --- RFID ---
+            case RFID_Initialization:
+                    Init_IFS();
+                break;
+            case RFID_PrepareForDiscovery:
+                    TagStage[readerID] = Tag_PrepareForDiscovery;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_Indetification:
+                    TagStage[readerID] = Tag_Indetification;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_GetUid:
+                    TagStage[readerID] = Tag_GetUid;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_ReadData:
+                    TagStage[readerID] = Tag_ReadValue;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_CheckCRC:
+                    TagStage[readerID] = Tag_CheckCRC;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_CheckHashValue:
+                    TagStage[readerID] = Tag_CheckHashValue;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_CheckValidity:
+                    TagStage[readerID] = Tag_CheckValidity;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_CheckUpdateCounter:
+                    TagStage[readerID] = Tag_CheckUpdateCounter;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_UpdateData:
+                    TagStage[readerID] = Tag_UpdateValue;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_Update_Ink_Fill:
+                    TagStage[readerID] = Tag_Update_Ink_Fill;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_Update_Ink_Used:
+                    TagStage[readerID] = Tag_Update_Ink_Used;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_Update_Ink_Empty:
+                    TagStage[readerID] = Tag_Update_Ink_Empty;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_Update_Ink_Filled_MachineID:
+                    TagStage[readerID] = Tag_UpdateInkMachineID;
+                    RFIDTagComm_StateMachine(readerID, OneStep);
+                break;
+            case RFID_Update_Waste_Fill:
+                     TagStage[readerID] = Tag_Update_Waste_Fill;
+                     RFIDTagComm_StateMachine(readerID, OneStep);
+                 break;
+             case RFID_Update_Waste_Full:
+                     TagStage[readerID] = Tag_Update_Waste_Full;
+                     RFIDTagComm_StateMachine(readerID, OneStep);
+                 break;
+             case RFID_Update_Waste_Empty:
+                     TagStage[readerID] = Tag_Update_Waste_Empty;
+                     RFIDTagComm_StateMachine(readerID, OneStep);
+                 break;
+             case RFID_Update_Waste_Filled_MachineID:
+                     TagStage[readerID] = Tag_UpdateWasteMachineID;
+                     RFIDTagComm_StateMachine(readerID, OneStep);
+                 break;
+             case RFID_UpdateWCounter:
+                     TagStage[readerID] = Tag_UpdateWCounter;
+                     RFIDTagComm_StateMachine(readerID, OneStep);
+                 break;
+            case RFID_UpdateCRC:
+                     TagStage[readerID] = Tag_UpdateCRC;
+                     RFIDTagComm_StateMachine(readerID, OneStep);
+                 break;
+            case RFID_TestTask:
+                TagStage[readerID] =  Tag_PrepareForDiscovery;
+                RFIDTagComm_StateMachine(readerID, Auto);
+                break;
+
+            // --- Dispensers ---
+            case DispenserReadData:
+                    Dispenser_EEPROM_Read_Data(Dispenser_ID);
+                break;
+            default:
+                break;
+        }
+    }
+}
+
+void I2C_DispRFIDTask_Init(void)
+{
+    Task_Params DispRFID_TaskParams;
+
+    RFID_ReadingMsgQ = Mailbox_create(sizeof(RFID_ReadingMessageStruc), 10, NULL,NULL);
+    Task_Params_init(&DispRFID_TaskParams);
+    DispRFID_TaskParams.instance->name = "RFID";
+    DispRFID_TaskParams.priority = 10;
+    DispRFID_TaskParams.stackSize = 4096;
+    DispRFID_Task_Handle = Task_create((Task_FuncPtr)DispRFID_Task, &DispRFID_TaskParams, NULL);
+}
+
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_RFID_Dispenser_Task/I2C_RFID_Disp_Task.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_RFID_Dispenser_Task/I2C_RFID_Disp_Task.h
new file mode 100644
index 000000000..5be55ce11
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_RFID_Dispenser_Task/I2C_RFID_Disp_Task.h
@@ -0,0 +1,41 @@
+/*
+ * I2C_RFID_Disp_Task.h
+ *
+ *  Created on: 2 Nov 2020
+ *      Author: avi
+ */
+
+#ifndef DRIVERS_RFID_TASK_RFID_TASK_H_
+#define DRIVERS_RFID_TASK_RFID_TASK_H_
+
+void Trigger_RFID_Init();
+void Trigger_RFID_Prepare(RFID_READER_ID readerID);
+void Trigger_RFID_Discovery(RFID_READER_ID readerID);
+void Trigger_RFID_GetUid(RFID_READER_ID readerID);
+void Trigger_RFID_ReadData(RFID_READER_ID readerID);
+void Trigger_RFID_CheckHash(RFID_READER_ID readerID);
+
+void Trigger_RFID_CheckValid(RFID_READER_ID readerID);
+void Trigger_RFID_UpdateInkFill(RFID_READER_ID readerID);
+void Trigger_RFID_UpdateInkUsed(RFID_READER_ID readerID);
+void Trigger_RFID_UpdateInkEmpty(RFID_READER_ID readerID);
+void Trigger_RFID_UpdateInkMachineID(RFID_READER_ID readerID);
+
+void Trigger_RFID_UpdateWasteFill(RFID_READER_ID readerID);
+void Trigger_RFID_UpdateWasteFull(RFID_READER_ID readerID);
+void Trigger_RFID_UpdateWasteEmpty(RFID_READER_ID readerID);
+void Trigger_RFID_UpdateWasteMachineID(RFID_READER_ID readerID);
+void Trigger_RFID_CheckCounter(RFID_READER_ID readerID);
+void Trigger_RFID_TestTask(RFID_READER_ID readerID);
+void I2C_DispRFIDTask_Init(void);
+
+
+void Trigger_RFID_UpdateWCounter(RFID_READER_ID readerID);
+void Trigger_RFID_UpdateCRC(RFID_READER_ID readerID);
+
+
+void Trigger_Dispenser_ReadData(int dispenserId);
+
+
+
+#endif /* DRIVERS_RFID_TASK_RFID_TASK_H_ */
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c
index ea804fc04..cb0b002fa 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c
@@ -56,7 +56,7 @@ typedef enum
     HeadArcPressureAction,
     HeadValveAction,
 
-    DispenserReadData,
+    //DispenserReadData, // I2C4  moved to RFID task
 
     /* enum for WHS :*/
     WHS_start,
@@ -271,17 +271,17 @@ void Trigger_Head_MixerValve(int direction)
     return;
 }
 
-void Trigger_Dispenser_ReadData(int dispenserId)
-{
-    I2C_ReadingMessageStruc I2C_ReadingMessage;
-    //uint8_t Midtank_i;
-
-    I2C_ReadingMessage.messageId = DispenserReadData;
-    I2C_ReadingMessage.parameter = dispenserId;
-    if (I2C_ReadingMsgQ != NULL)
-        Mailbox_post(I2C_ReadingMsgQ , &I2C_ReadingMessage, BIOS_NO_WAIT);
-    return;
-}
+//void Trigger_Dispenser_ReadData(int dispenserId) // I2C4  moved to RFID task
+//{
+//    I2C_ReadingMessageStruc I2C_ReadingMessage;
+//    //uint8_t Midtank_i;
+//
+//    I2C_ReadingMessage.messageId = DispenserReadData;
+//    I2C_ReadingMessage.parameter = dispenserId;
+//    if (I2C_ReadingMsgQ != NULL)
+//        Mailbox_post(I2C_ReadingMsgQ , &I2C_ReadingMessage, BIOS_NO_WAIT);
+//    return;
+//}
 
 void Trigger_HeaterWriting(void)
 {
@@ -742,9 +742,9 @@ void I2C_ReadingTask(UArg arg0, UArg arg1)
         case Head_ACT_ON_Writing:
             Head_Write_IO_Reg(0x46,HIGH);//ACT ON
             break;
-        case DispenserReadData:
-            Dispenser_EEPROM_Read_Data(I2C_ReadingMessage.parameter);
-            break;
+//        case DispenserReadData: // I2C4  moved to RFID task
+//            Dispenser_EEPROM_Read_Data(I2C_ReadingMessage.parameter);
+//            break;
         case WHS_PT100_Reading:
             for (Sensor_i = WHS_PT100_1_0X80_0; Sensor_i <= WHS_PT100_4_0X82_1; Sensor_i++)
             {
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
index 387a8f692..59f988b44 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
@@ -62,6 +62,6 @@ void Trigger_Head_Magnet(int direction);
 void Trigger_Head_MixerValve(int direction);
 void Trigger_Head_PressureArc(bool Enable_Disable);
 
-void Trigger_Dispenser_ReadData(int dispenserId);
+//void Trigger_Dispenser_ReadData(int dispenserId);
 
 #endif /* DRIVERS_I2C_COMMUNICATION_I2C_TASK_H_ */
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.c
index 4057b4912..c04cabc4a 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.c
@@ -10,12 +10,12 @@
 #include <stdbool.h>
 #include <string.h> //memset
 #include <DataDef.h>
+#include <Utilities/delay.h>
 #include "driverlib/sysctl.h"
 #include "driverlib/i2c.h"
 #include <Drivers/I2C_Communication/I2C.h>
 #include "drivers/FPGA/FPGA_Comm.h"
 #include <Drivers/I2C_Communication/RFID_NFC/PN7150/PN7150.h>
-#include <Utilities/delay.h>
 #include "I2C_IFS_Mux.h"
 #include "NFC_MainBaord.h"
 #include "Modules/Control/Control.h"
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC.c
index a1139591d..ef38f2b24 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC.c
@@ -10,17 +10,31 @@
 #include <string.h> //memset
 #include <DataDef.h>
 #include "datadef.h"
+#include <Utilities/delay.h>
 #include "drivers/I2C_Communication/I2C.h"
 #include "drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.h"
-#include "drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h"
 #include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
+#include "drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h"
+
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
+#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
 #include <Drivers/I2C_Communication/RFID_NFC/NFC.h>
-#include <Utilities/delay.h>
+#include <Drivers/I2C_Communication/RFID_NFC/RFID_Settings.h>
+#include <Drivers/I2C_Communication/I2C_RFID_Dispenser_Task/I2C_RFID_Disp_Task.h>
+#include <Drivers/I2C_Communication/RFID_NFC/RFID_IFS.h>
+#include <Drivers/I2C_Communication/RFID_NFC/RFID_Waste.h>
+#include <Drivers/I2C_Communication/I2C_RFID_Dispenser_Task/I2C_RFID_Disp_Task.h>
+
+uint8_t RFID_Discovery_TimeOut[3] = {RFID_Discovery_TimeOut_Limit,RFID_Discovery_TimeOut_Limit,RFID_Discovery_TimeOut_Limit};
+bool RequestForDiscovery[3] = {false,false,false};//request from WHS logic
+bool DiscoverRFIDTagEvery100mSec[3] = {false,false,false};
 
-RFID_TAG_State_Machine    TagStage = Tag_Idle;
+RFID_TAG_State_Machine    TagStage[3] = {Tag_Idle,Tag_Idle,Tag_Idle};
 
 NxpNci_RfIntf_t tagInfo;
 
+RFID_STATUS RFID_Status_Words[Max_Readers];
+
 bool  Init_RFID_Reader(RFID_READER_ID Reader_ID)//0-2
 {
     static bool firstTime = true;
@@ -45,7 +59,7 @@ bool  Init_RFID_Reader(RFID_READER_ID Reader_ID)//0-2
 
 }
 
-uint8_t RFID_Pint_Errors(uint8_t readerID, RFID_RETURN_CODE Err_Code)
+uint8_t RFID_Print_Errors(uint8_t readerID, RFID_RETURN_CODE Err_Code)
 {
     uint8_t Status = ERROR;
 
@@ -124,7 +138,7 @@ uint8_t Connect_to_RFID_Reder(uint8_t readerID)//2
         {
             RFIDRetValue = getUID( readerID, &tagInfo, uid );
             RFID_Status = RFIDRetValue;
-            Status |= RFID_Pint_Errors(readerID, RFIDRetValue);
+            Status |= RFID_Print_Errors(readerID, RFIDRetValue);
     /*
             //password authentication
             PwdRetVal = checkPwd( newPwd, newPACK );
@@ -163,106 +177,896 @@ uint8_t Connect_to_RFID_Reder(uint8_t readerID)//2
     return Status;
 }
 
-RFID_RETURN_CODE  TagWriteData()
+
+NFC_Tag NFC_Tag_R[Max_Readers];//read
+NFC_Tag NFC_Tag_W[Max_Readers];//write (for testing / stub)
+NFC_Tag NFC_Tag_U[Max_Readers];//read with updates
+
+//NFCTag_t NFC_Tag_W_Struct;
+
+
+RFID_RETURN_CODE  TagWriteData(RFID_READER_ID readerID)
 {
+    uint8_t index = 0;
+    uint32_t dummy = 0;
+    uint32_t Tuint32 = 0;
+    //uint16_t Tuint16 = 0;
+    //uint8_t Tuint8 = 0;
+    RFID_RETURN_CODE RFIDRetValue;
+
+    //NxpNci_RfIntf_t TagInfo;
+
+    uint8_t tagMemory[256];
+
+//    uint8_t Serial[] = {'1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', 'G'};
+
+    //RFID_READER_ID readerID = (RFID_READER_ID)(Active_RFID_Reader -1);
+
+//    for(index = 0; index < sizeof(Serial); index++)
+//    {
+//        Serial[index] = 'a'+index;
+//    }
+///*
+//    for(index = 0; index < sizeof(NFC_Tag_W_Struct.CartSN); index++)
+//    {
+//        NFC_Tag_W_Struct.CartSN[index] = 'M'+index;
+//    }
+//
+//    RFIDRetValue = WriteValue( readerID, &TagInfo, NFC_Tag_W_Struct.CartSN, 0, dummy );
+//    */
+//    /*
+//    for(index = 0; index < sizeof(NFC_Tag_W[readerID].Struct.CartSN); index++)
+//    {
+//        NFC_Tag_W[readerID].Struct.CartSN[index] = 'c'+index;
+//    }
+//
+//    RFIDRetValue = WriteValue( readerID, &TagInfo, NFC_Tag_W[readerID].Struct.CartSN, 0, dummy );
+//     */
+//    RFIDRetValue = WriteValue( readerID, &tagInfo, Serial, 0, dummy );
+
+    //Serial
+    strcpy(NFC_Tag_W[readerID].Struct.Serial, "123456789abcdefg");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.Serial, 0, dummy );
+
+    //factory id
+    strcpy(NFC_Tag_W[readerID].Struct.factoryID, "FID");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.factoryID, 1, dummy );
+/*
+    //////////////////
+    //uint8_t fillingSystem[2] = {0xab, 0xcd};
+    uint16_t fillingSystem = 50;//50 -> write 0x3035 ASCII 35 00
+    //filling system
+    //RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 2, fillingSystem );
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 2, fillingSystem );
+*/
+    //filling system
+    NFC_Tag_W[readerID].Struct.fillingSystem = 50;
+    Tuint32 = NFC_Tag_W[readerID].Struct.fillingSystem;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 2, Tuint32 );
+    //work order
+    strcpy(NFC_Tag_W[readerID].Struct.workOrder, "WORK-ORDER");
+
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.workOrder, 3, dummy );
+    //color name
+    strcpy(NFC_Tag_W[readerID].Struct.colorName, "-COLOR-NAME-");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.colorName, 4, dummy );
+
+    //category
+    strcpy(NFC_Tag_W[readerID].Struct.catagory, "CY");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.catagory, 5, dummy );
+
+    //type
+    NFC_Tag_W[readerID].Struct.type = 4321;
+    Tuint32 = NFC_Tag_W[readerID].Struct.type;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 6, Tuint32 );
+
+    delayms(2);
+
+    //cartSize
+    NFC_Tag_W[readerID].Struct.cartSize = 8765;
+    Tuint32 = NFC_Tag_W[readerID].Struct.cartSize;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 7, Tuint32 );
+
+    delayms(2);
+
+    /////////////////
+
+    //inkVolume
+    NFC_Tag_W[readerID].Struct.inkVolume = 9696;
+    Tuint32 = NFC_Tag_W[readerID].Struct.inkVolume;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 8, Tuint32 );
+
+    delayms(2);
+    //inkBatch
+
+    strcpy(NFC_Tag_W[readerID].Struct.inkBatch, "-inkBatch-");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.inkBatch, 9, dummy );
+    delayms(2);
+    //inkMfgDate
+    strcpy(NFC_Tag_W[readerID].Struct.inkMfgDate, "inkMfgDa");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.inkMfgDate, 10, dummy );
+    delayms(2);
+    //inkEolDate
+    strcpy(NFC_Tag_W[readerID].Struct.inkEolDate, "31/12/21");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.inkEolDate, 11, dummy );
+    delayms(2);
+    //CartFillDate
+    strcpy(NFC_Tag_W[readerID].Struct.CartFillDate, "CarFDate");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.CartFillDate, 12, dummy );
+    delayms(2);
+    //WasteFillDate
+    strcpy(NFC_Tag_W[readerID].Struct.WasteFillDate, "WastDate");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.WasteFillDate, 13, dummy );
+    delayms(2);
+    //Misc
+    NFC_Tag_W[readerID].Struct.Misc = 1234;
+    Tuint32 = NFC_Tag_W[readerID].Struct.Misc;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 14, Tuint32 );
+    delayms(2);
+    //pluginCounter
+    NFC_Tag_W[readerID].Struct.pluginCounter = 56;
+    Tuint32 = NFC_Tag_W[readerID].Struct.pluginCounter;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 15, Tuint32 );
+    delayms(2);
+    //inkFill
+    NFC_Tag_W[readerID].Struct.inkFill = 7;
+    Tuint32 = NFC_Tag_W[readerID].Struct.inkFill;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 16, Tuint32 );
+    delayms(2);
+    //inkUsed
+    NFC_Tag_W[readerID].Struct.inkUsed = 8;
+    Tuint32 = NFC_Tag_W[readerID].Struct.inkUsed;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 17, Tuint32 );
+    delayms(2);
+    //inkEmpty
+    NFC_Tag_W[readerID].Struct.inkEmpty = 9;
+    Tuint32 = NFC_Tag_W[readerID].Struct.inkEmpty;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 18, Tuint32 );
+    //wasteEmpty
+    NFC_Tag_W[readerID].Struct.wasteEmpty = 1;
+    Tuint32 = NFC_Tag_W[readerID].Struct.wasteEmpty;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 19, Tuint32 );
+    //wasteFilling
+    NFC_Tag_W[readerID].Struct.wasteFilling = 2;
+    Tuint32 = NFC_Tag_W[readerID].Struct.wasteFilling;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 20, Tuint32 );
+    //wasteFull
+    NFC_Tag_W[readerID].Struct.wasteFull = 3;
+    Tuint32 = NFC_Tag_W[readerID].Struct.wasteFull;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 21, Tuint32 );
+    //blocked
+    NFC_Tag_W[readerID].Struct.blocked = TAG_NOT_BLOCKED;
+    Tuint32 = NFC_Tag_W[readerID].Struct.blocked;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 22, Tuint32 );
+    //fail
+    NFC_Tag_W[readerID].Struct.fail = 5;
+    Tuint32 = NFC_Tag_W[readerID].Struct.fail;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 23, Tuint32 );
+    //wasteCounter
+    NFC_Tag_W[readerID].Struct.wasteCounter = 0;
+    Tuint32 = NFC_Tag_W[readerID].Struct.wasteCounter;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 24, Tuint32 );
+    //MachineIdInl
+    strcpy(NFC_Tag_W[readerID].Struct.MachineIdInl, "MachineIdInl1234");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.MachineIdInl, 25, dummy );
+    //WasteFillDate
+    strcpy(NFC_Tag_W[readerID].Struct.MachineIdWst, "MachineIdWst1234");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.MachineIdWst, 26, dummy );
+    //WasteFillDate
+    strcpy(NFC_Tag_W[readerID].Struct.reserved, "reserved12345");
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.reserved, 27, dummy );
+    //counter
+    NFC_Tag_W[readerID].Struct.counter = 1;//5678;
+    Tuint32 = NFC_Tag_W[readerID].Struct.counter;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 28, Tuint32 );
+    //tagValid
+    NFC_Tag_W[readerID].Struct.tagValid = TAG_VALID;
+    Tuint32 = NFC_Tag_W[readerID].Struct.tagValid;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 29, Tuint32 );
+
+    //hash
+    calcHash( tagInfo.Info.NFC_VPP.ID, sizeof(tagInfo.Info.NFC_VPP.ID), NFC_Tag_W[readerID].Struct.HashValue );//read the UID, calculate Hash and write the hash
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_W[readerID].Struct.HashValue, 30, dummy );
+
+    //crc
+    //crc_ccitt_init();
+    NFC_Tag_W[readerID].Struct.Checksum_CRC16 = crc_ccitt_update(NFC_Tag_W[readerID].Buf,(sizeof(NFC_Tag_W[readerID].Buf) - sizeof(NFC_Tag_W[readerID].Struct.Checksum_CRC16)));//calculate the crc without tag id and without crc fields
+    Tuint32 = NFC_Tag_W[readerID].Struct.Checksum_CRC16;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 31, Tuint32 );
+
 
     return SUCCESS;
 }
 
+
+//RFID_RETURN_CODE Read_Tag_All_Data()
+//{
+//
+//   // uint8_t index = 0;
+//    uint32_t dummy = 0;
+//    //uint8_t Tuint8 = 0;
+//    //uint16_t Tuint16 = 0;
+//    uint32_t Tuint32 = 0;
+//    //uint64_t Tuint64 = 0;
+//    //uint8_t Tuint8_buf10[10] = 0;
+//    RFID_RETURN_CODE RFIDRetValue;
+//
+//    //NxpNci_RfIntf_t TagInfo;
+//
+//    uint8_t tagMemory[1024];
+//
+//    RFID_READER_ID readerID = (RFID_READER_ID)(Active_RFID_Reader -1);
+//
+//    //serial
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.Serial, 0, &dummy );
+//    //factory id
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.factoryID, 1, &dummy );
+//    //filling system
+//    //RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 2, NFC_Tag_R[readerID].Struct.fillingSystem );
+//    //RFIDRetValue = ReadValue( readerID, &tagInfo, &NFC_Tag_R[readerID].Struct.fillingSystem, 2, &dummy );//OK read hex (for writubg 50 read 0335 ASCII 0)
+//    /*NFC_Tag_R[readerID].Struct.fillingSystem = ReadValue( readerID, &tagInfo, tagMemory, 2, &dummy );*/
+//    //RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 2, NFC_Tag_R[readerID].Struct.fillingSystem );
+//    /* working ok*/
+//
+//
+//    Tuint32=0;
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 2, &Tuint32 );
+//    NFC_Tag_R[readerID].Struct.fillingSystem = (uint16_t)Tuint32;
+//
+//
+//    //work order
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.workOrder, 3, &dummy );
+//    //color name
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.colorName, 4, &dummy );
+//    //category
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.catagory, 5, &dummy );
+//    //type
+//    Tuint32 = 0;
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 6, &Tuint32 );
+//    NFC_Tag_R[readerID].Struct.type = Tuint32;
+//    //cartSize
+//    Tuint32 = 0;
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 7, &Tuint32 );
+//    NFC_Tag_R[readerID].Struct.cartSize = Tuint32;
+//    //inkVolume
+//    Tuint32 = 0;
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 8, &Tuint32 );
+//    NFC_Tag_R[readerID].Struct.inkVolume = Tuint32;
+//    //inkBatch
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.inkBatch, 9, &dummy );
+//    //inkMfgDate
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.inkMfgDate, 10, &dummy );
+//    //inkEolDate
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.inkEolDate, 11, &dummy );
+//    //CartFillDate
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.CartFillDate, 12, &dummy );
+//    //WasteFillDate
+//    RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.WasteFillDate, 13, &dummy );
+//
+//
+//    //Misc
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 14, &Tuint32 );
+//        NFC_Tag_R[readerID].Struct.Misc = Tuint32;
+//        //pluginCounter
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 15, &Tuint32 );
+//        NFC_Tag_R[readerID].Struct.pluginCounter = (uint16_t)Tuint32;
+//
+//        //inkFill
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 16, &Tuint32 );//FAIL
+//        NFC_Tag_R[readerID].Struct.inkFill = (uint8_t)Tuint32;
+//
+//        //inkUsed
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 17, &Tuint32 );
+//        NFC_Tag_R[readerID].Struct.inkUsed = (uint8_t)Tuint32;
+//
+//        //inkEmpty
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 18, &Tuint32 );//FAIL
+//        NFC_Tag_R[readerID].Struct.inkEmpty = (uint8_t)Tuint32;
+//        //wasteEmpty
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 19, &Tuint32 );
+//        NFC_Tag_R[readerID].Struct.wasteEmpty = (uint8_t)Tuint32;
+//        //wasteFilling
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 20, &Tuint32 );
+//        NFC_Tag_R[readerID].Struct.wasteFilling = (uint8_t)Tuint32;
+//        //wasteFull
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 21, &Tuint32 );
+//        NFC_Tag_R[readerID].Struct.wasteFull = (uint8_t)Tuint32;
+//        //blocked
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 22, &Tuint32 );
+//        NFC_Tag_R[readerID].Struct.blocked = (uint8_t)Tuint32;
+//        //fail
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 23, &Tuint32 );
+//        NFC_Tag_R[readerID].Struct.fail = (uint8_t)Tuint32;
+//        //wasteCounter
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 24, &Tuint32 );
+//        NFC_Tag_R[readerID].Struct.wasteCounter = (uint16_t)Tuint32;
+//        //MachineIdInl
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.MachineIdInl, 25, &dummy );
+//        //MachineIdWst
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.MachineIdWst, 26, &dummy );
+//        //reserved
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.reserved, 27, &dummy );
+//        //counter
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 28, &Tuint32 );
+//        NFC_Tag_R[readerID].Struct.counter = Tuint32;
+//        //tagValid
+//        Tuint32 = 0;
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, tagMemory, 29, &Tuint32 );
+//        NFC_Tag_R[readerID].Struct.tagValid = (uint8_t)Tuint32;
+//        //hash
+//        RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.HashValue, 30, &dummy );
+//
+//
+//    //////////////
+//
+//    return RFIDRetValue;
+//
+//}
+
+
+//--------------------------------------------------------------------------------------------
+
+//--------------------------------------------------------------------------------------------
+/*
+typedef union
+{
+    uint8_t  Byte[4];
+    uint32_t Word;
+}Word_to_Bytes;
+*/
+//typedef union
+//{
+//    uint8_t  Byte[4];
+//    uint32_t Short;
+//}Short_to_Bytes;
+
+#define TagMaxFieldLength 16 // why 256? max is 16
+
+#define Uartsize sizeof(NFC_Tag)
+
+
+void Init_NFC_Tag_Union(RFID_READER_ID readerID)
+{
+    uint8_t i = 0;
+
+    memset(NFC_Tag_R[readerID].Buf, 0, Uartsize);
+
+    for(i=0;i<8;i++)//Copy UID
+    {
+        NFC_Tag_R[readerID].Struct.tagId[i]  = tagInfo.Info.NFC_VPP.ID[i];
+    }
+
+    //Padding fields:
+    NFC_Tag_R[readerID].Struct.Padding_1_8bit = NFC_Tag_R[readerID].Struct.Paddind_2_8bit = NFC_Tag_R[readerID].Struct.Paddind_3_8bit = 0xFF;
+    NFC_Tag_R[readerID].Struct.Paddind_1_16bit = NFC_Tag_R[readerID].Struct.Paddind_2_16bit = NFC_Tag_R[readerID].Struct.Paddind_3_16bit = NFC_Tag_R[readerID].Struct.Paddind_4_16bit = 0xFFFF;
+
+}
+
+RFID_RETURN_CODE Read_Tag_All_Data()
+{
+    uint32_t dummy = 0;
+    RFID_RETURN_CODE RFIDRetValue = SUCCESS;
+    uint8_t tagMemory[256];
+    uint32_t valueNum = 0;
+    uint8_t value[TagMaxFieldLength];
+    uint32_t BufferIndex = 8;//start with Serial field
+    uint8_t valueIndex, i;
+    Word_to_Bytes Word2Bytes;
+
+    RFID_READER_ID readerID = (RFID_READER_ID)(Active_RFID_Reader -1);
+
+    Init_NFC_Tag_Union(readerID);
+
+    for(valueIndex = 0; valueIndex < NUMBER_OF_FIELDS; valueIndex++)
+    {
+        delayms(5);// need to check without it sometimes read wrong values (0xFF... in Serial, MachineIdInl at MachineIdWst)
+
+        while((NFC_Tag_R[readerID].Buf[BufferIndex] != 0x00) && (BufferIndex < (Uartsize-1)))
+        {
+            NFC_Tag_R[readerID].Buf[BufferIndex] = 0; // reset the padding fields in order to calculate the crc
+            BufferIndex++; //Skip padding fields
+        }
+
+        if (Fields[valueIndex].isNum == true )//Number
+        {
+            valueNum = 0;
+            RFIDRetValue |= ReadValue( readerID, &tagInfo, tagMemory, valueIndex, &valueNum );
+
+            if(RFIDRetValue != SUCCESS)
+            {
+                RFIDRetValue = SUCCESS;
+                delayms(5);
+                RFIDRetValue |= ReadValue( readerID, &tagInfo, tagMemory, valueIndex, &valueNum );//try again
+            }
+            if(RFIDRetValue != SUCCESS)
+            {
+                RFIDRetValue = SUCCESS;
+                delayms(5);
+                RFIDRetValue |= ReadValue( readerID, &tagInfo, tagMemory, valueIndex, &valueNum );//try again
+            }
+
+            Word2Bytes.Word = valueNum;
+
+            if(Fields[valueIndex].fieldLength == 1)
+            {
+                NFC_Tag_R[readerID].Buf[BufferIndex] = Word2Bytes.Byte[0];
+                BufferIndex+=1;
+            }
+            else if(Fields[valueIndex].fieldLength == 2)
+            {
+                for ( i = 0; i < 2; i++)
+                {
+                    NFC_Tag_R[readerID].Buf[BufferIndex+i] = Word2Bytes.Byte[i];
+                }
+                BufferIndex+=2;
+            }
+            else if(Fields[valueIndex].fieldLength == 4)
+            {
+                for ( i = 0; i < 4; i++)
+                {
+                    NFC_Tag_R[readerID].Buf[BufferIndex+i] = Word2Bytes.Byte[i];
+                }
+                BufferIndex+=4;
+            }
+        }
+        else// ASCI text
+        {
+            RFIDRetValue |= ReadValue( readerID, &tagInfo, value, valueIndex, &dummy );
+
+            if(RFIDRetValue != SUCCESS)
+            {
+                RFIDRetValue = SUCCESS;
+                delayms(5);
+                RFIDRetValue |= ReadValue( readerID, &tagInfo, value, valueIndex, &dummy );//try again
+            }
+            if(RFIDRetValue != SUCCESS)
+            {
+                RFIDRetValue = SUCCESS;
+                delayms(5);
+                RFIDRetValue |= ReadValue( readerID, &tagInfo, value, valueIndex, &dummy );//try again
+            }
+
+            for ( i = 0; i < Fields[valueIndex].fieldLength; i++)
+            {
+                    NFC_Tag_R[readerID].Buf[BufferIndex] = value[i];
+                    BufferIndex+=1;
+            }
+        }
+    }
+
+    return RFIDRetValue;
+}
+
+//--------------------------------------------------------------------------------------------
+
+//--------------------------------------------------------------------------------------------
+
 RFID_RETURN_CODE  TagReadData()
 {
+    uint32_t dummy = 0;
+    RFID_RETURN_CODE RFIDRetValue;
 
-    return SUCCESS;
+    RFID_READER_ID readerID = (RFID_READER_ID)(Active_RFID_Reader -1);
+
+    RFIDRetValue = ReadValue( readerID, &tagInfo, NFC_Tag_R[readerID].Struct.Serial, 0, &dummy );
+    return RFIDRetValue;
 }
 
-RFID_RETURN_CODE  WriteCRCValue()
+RFID_RETURN_CODE  WriteWCounter(RFID_READER_ID readerID)
 {
+    RFID_RETURN_CODE RFIDRetValue = SUCCESS;
 
-    return SUCCESS;
+    RFIDRetValue = incrementCounter( readerID, &tagInfo);//every time updating data
+
+    return RFIDRetValue;
 }
 
-uint32_t RFIDTagComm_StateMachine(StateMachineMode Mode)
+RFID_RETURN_CODE  WriteCRCValue(RFID_READER_ID readerID)
+{
+    uint32_t Tuint32 = 0;
+    uint8_t tagMemory[256];
+    RFID_RETURN_CODE RFIDRetValue = SUCCESS;
+
+    //RFIDRetValue = incrementCounter( readerID, &tagInfo);//every time updating data
+
+    //crc_ccitt_init();
+    NFC_Tag_U[readerID].Struct.Checksum_CRC16 =  crc_ccitt_update(NFC_Tag_U[readerID].Buf,(sizeof(NFC_Tag_U[readerID].Buf) - sizeof(NFC_Tag_U[readerID].Struct.Checksum_CRC16)));//calculate the crc without tag id and without crc fields
+    Tuint32 = NFC_Tag_U[readerID].Struct.Checksum_CRC16;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 31, Tuint32 );
+
+    return RFIDRetValue;
+}
+
+bool NFC_Check_CRC(RFID_READER_ID readerID)//after reading all data from tag
+{
+    bool status = ERROR;
+    uint32_t CRC;
+
+    //crc_ccitt_init();
+    CRC = crc_ccitt_update(NFC_Tag_R[readerID].Buf,(sizeof(NFC_Tag_R[readerID].Buf) - sizeof(NFC_Tag_R[readerID].Struct.Checksum_CRC16)));
+
+    if(NFC_Tag_R[readerID].Struct.Checksum_CRC16 == CRC)
+    {
+        status =  OK;
+    }
+
+    return status;
+
+}
+uint8_t counterd_dis = 0;
+
+bool follow_stages[MAX_TAG_State_Machine] = {ERROR, ERROR, ERROR, ERROR, ERROR, ERROR, ERROR, ERROR, ERROR, ERROR, ERROR, ERROR, ERROR};
+uint32_t RFIDTagComm_StateMachine(RFID_READER_ID readerID, StateMachineMode Mode)
 {
     uint32_t Status = OK;
     RFID_RETURN_CODE RFIDRetValue;
-    RFID_READER_ID readerID = (RFID_READER_ID)(Active_RFID_Reader -1);
+    //RFID_READER_ID readerID = (RFID_READER_ID)(Active_RFID_Reader -1);
+    uint8_t i = 0;
+    //uint8_t ReadingCounter;
+    int RetValue = 0;
+
 
-    switch(TagStage)
+    switch(TagStage[readerID])
     {
         case Tag_Idle:
         //Tag Not Active
+            follow_stages[TagStage[readerID]] = OK;
         break;
         case Tag_PrepareForDiscovery:
 
+
+
                 RFIDRetValue = RFID_StartDiscovery(readerID);
 
-                if((RFIDRetValue == SUCCESS) && (Mode == Auto))
+                if(RFIDRetValue == SUCCESS)
+                {
+                    follow_stages[TagStage[readerID]] = OK;
+                    RFID_Status_Words[readerID].errors.ReaderPreDiscovery = S_SUCCEEDED;
+                    if(Mode == Auto)
+                        TagStage[readerID] = Tag_Indetification;
+                }
+                else
                 {
-                    TagStage = Tag_Indetification;
+                    follow_stages[TagStage[readerID]] = ERROR;
+                    RFID_Status_Words[readerID].errors.ReaderPreDiscovery = S_FAILED;
+
                 }
 
-            break;
+                delayms(1);
+                if(Mode == OneStep)
+                {
+                    break;
+                }
         case Tag_Indetification:
 
-                RFIDRetValue = RFID_WaitForDiscoveryNotification(readerID, &tagInfo);
 
-                if((RFIDRetValue == SUCCESS) && (Mode == Auto))
+
+                //RFIDRetValue = RFID_WaitForDiscoveryNotification(readerID, &tagInfo);
+                do
+                {
+                    RFIDRetValue = RFID_WaitForDiscoveryNotification(readerID, &tagInfo);
+                    delayms(100);
+                    counterd_dis++;
+
+
+
+                }while(RFIDRetValue != SUCCESS);
+
+                if(RFIDRetValue == SUCCESS)
                 {
-                    TagStage = Tag_GetUid;
+                    follow_stages[TagStage[readerID]] = OK;
+                    RFID_Status_Words[readerID].errors.TagDiscovery = S_SUCCEEDED;
+                    if(Mode == Auto)
+                        TagStage[readerID] = Tag_GetUid;
+                }
+                else
+                {
+                    follow_stages[TagStage[readerID]] = ERROR;
+                    RFID_Status_Words[readerID].errors.TagDiscovery = S_FAILED;
+                }
+                delayms(1);
+                if(Mode == OneStep)
+                {
+                    break;
                 }
-            break;
         case Tag_GetUid:
                 RFIDRetValue = getUID( readerID, &tagInfo, uid );
 
-                if((RFIDRetValue == SUCCESS) && (Mode == Auto))
+                if(RFIDRetValue == SUCCESS)
                 {
-                    TagStage = Tag_WriteValue;
+                    follow_stages[TagStage[readerID]] = OK;
+                    RFID_Status_Words[readerID].errors.TagGetUID = S_SUCCEEDED;
+                    if(Mode == Auto)
+                        TagStage[readerID] = Tag_WriteValue;
+                }
+                else
+                {
+                    follow_stages[TagStage[readerID]] = ERROR;
+                    RFID_Status_Words[readerID].errors.TagGetUID = S_FAILED;
+                    //while(1);
                 }
 
-            break;
-        case Tag_CheckPwd:
-        break;
-        case Tag_CheckValidity:
-        break;
-        case Tag_UpdateCounter:
-        break;
-        case Tag_CheckHashValue:
-        break;
-        case Tag_InvalidLock:
+//                memset(NFC_Tag_R[readerID].Buf,0,sizeof(NFCTag_t));
+//
+//                for(i=0;i<8;i++)//Copy UID
+//                {
+//                    NFC_Tag_R[readerID].Struct.tagId[i]  = tagInfo.Info.NFC_VPP.ID[i];
+//                }
+                delayms(1);
+
+                if(Mode == OneStep)
+                {
+                    break;
+                }
+/*        case Tag_CheckPwd:
         break;
+        */
+
+/*        case Tag_InvalidLock:
+        break;*/
+
+#ifndef RFID_READ_ONLY
+        case Tag_WriteValue:
+
+                RFIDRetValue = TagWriteData(readerID);
+
+                if(RFIDRetValue == SUCCESS)
+                {
+                    follow_stages[TagStage[readerID]] = OK;
+                    RFID_Status_Words[readerID].errors.TagwriteData = S_SUCCEEDED;
+                    if(Mode == Auto)
+                        TagStage[readerID] = Tag_ReadValue;
+                }
+                else
+                {
+                    follow_stages[TagStage[readerID]] = ERROR;
+                    RFID_Status_Words[readerID].errors.TagwriteData = S_FAILED;
+                    while(1);
+                }
+                delayms(1);
+
+                if(Mode == OneStep)
+                {
+                    break;
+                }
+#endif
         case Tag_ReadValue:
 
-                RFIDRetValue = TagReadData();
+                //RFIDRetValue = Read_Tag_All_Data();//TagReadData();
+
+            //
+           /* ReadingCounter = 0;
+            do
+            {
+                RFIDRetValue = Read_Tag_All_Data();
+                ReadingCounter++;
+            }
+            while ((NFC_Check_CRC() == ERROR) && (ReadingCounter < 1));//if the crc is wrong read the tag again (max 3 times)
+
+            if(NFC_Check_CRC() == ERROR)
+            {
+                while(true);
+            }*/
+
+            RFIDRetValue = Read_Tag_All_Data();
+
 
-                if((RFIDRetValue == SUCCESS) && (Mode == Auto))
+            //
+
+                if(RFIDRetValue == SUCCESS)
+                {
+                    follow_stages[TagStage[readerID]] = OK;
+                    RFID_Status_Words[readerID].errors.TagReadData = S_SUCCEEDED;
+                    if(Mode == Auto)
+                        TagStage[readerID] = Tag_CheckUpdateBlocked;
+                }
+                else
                 {
-                    TagStage = Tag_WriteValue;
+                    follow_stages[TagStage[readerID]] = ERROR;
+                    RFID_Status_Words[readerID].errors.TagReadData = S_FAILED;
                 }
+                delayms(1);
 
-            break;
-        case Tag_WriteValue:
+                for(i = 0; i < sizeof(NFCTag_t); i++)
+                {
+                    NFC_Tag_U[readerID].Buf[i] = NFC_Tag_R[readerID].Buf[i];
+                }
+
+                if(Mode == OneStep)
+                {
+                    break;
+                }
+        case Tag_CheckCRC:
+            if(NFC_Check_CRC(readerID) != OK)
+            {
+                RFID_Status_Words[readerID].errors.TAgCheckDataCRC = S_FAILED;
+            }
+            else
+            {
+                RFID_Status_Words[readerID].errors.TAgCheckDataCRC = S_SUCCEEDED;
+            }
+            if(Mode == OneStep)
+            {
+                break;
+            }
+        case Tag_CheckUpdateBlocked:
 
-                RFIDRetValue = TagWriteData();
+                RFIDRetValue = IsTagBlocked(readerID);//check blocked bit
 
-                if((RFIDRetValue == SUCCESS) && (Mode == Auto))
+                if(RFIDRetValue == SUCCESS)
                 {
-                    TagStage = Tag_DisconnectComm;
+                    follow_stages[TagStage[readerID]] = OK;
+                    RFID_Status_Words[readerID].errors.TAgCheckBlockedField = S_SUCCEEDED;
+                    if(Mode == Auto)
+                        TagStage[readerID] = Tag_CheckHashValue;
                 }
+                else
+                {
+                    follow_stages[TagStage[readerID]] = ERROR;
+                    RFID_Status_Words[readerID].errors.TAgCheckBlockedField = S_FAILED;
+                }
+                #warning Add here blacklist functionality
+                //check black list
+                // update block bit + checsum  + block field for changing
+                delayms(1);
+
+                if(Mode == OneStep)
+                {
+                    break;
+                }
+        case Tag_CheckHashValue:
+             RetValue = checkHashValue( readerID, &tagInfo);
 
+             if(RetValue == 1)
+             {
+                 follow_stages[TagStage[readerID]] = OK;
+                 RFID_Status_Words[readerID].errors.TagCheckHash = S_SUCCEEDED;
+                 if(Mode == Auto)
+                     TagStage[readerID] = Tag_CheckValidity;
+             }
+             else
+             {
+                 follow_stages[TagStage[readerID]] = ERROR;
+                 RFID_Status_Words[readerID].errors.TagCheckHash = S_FAILED;
+                 //while(1);
+             }
+             delayms(1);
+
+             if(Mode == OneStep)
+             {
+                 break;
+             }
+        case Tag_CheckValidity:
+
+                RFIDRetValue = IsTagValid(readerID);
+
+                if(RFIDRetValue == SUCCESS)
+                {
+                    follow_stages[TagStage[readerID]] = OK;
+                    RFID_Status_Words[readerID].errors.TagCheckValidity = S_SUCCEEDED;
+                    if(Mode == Auto)
+                        TagStage[readerID] = Tag_CheckUpdateCounter;
+                }
+                else
+                {
+                    follow_stages[TagStage[readerID]] = ERROR;
+                    RFID_Status_Words[readerID].errors.TagCheckValidity = S_FAILED;
+                    //while(1);
+                }
+                delayms(1);
+
+                if(Mode == OneStep)
+                {
+                    break;
+                }
+#ifndef RFID_READ_ONLY
+        case Tag_CheckUpdateCounter:
+            RetValue = incrementCounterAndCompare( readerID, &tagInfo);
+
+            if(RetValue == 1)
+            {
+                follow_stages[TagStage[readerID]] = OK;
+                RFID_Status_Words[readerID].errors.TagCheckCounterLimit = S_SUCCEEDED;
+                RFID_Status_Words[readerID].errors.TagUpdateCounters = S_SUCCEEDED;
+                if(Mode == Auto)
+                    TagStage[readerID] = Tag_UpdateCheckSum;
+            }
+            else if(RetValue == -1)
+            {
+                follow_stages[TagStage[readerID]] = ERROR;
+                RFID_Status_Words[readerID].errors.TagCheckCounterLimit = S_FAILED;
+            }
+            else if(RetValue == -2)
+            {
+                follow_stages[TagStage[readerID]] = ERROR;
+                RFID_Status_Words[readerID].errors.TagUpdateCounters = S_FAILED;
+            }
+
+            delayms(1);
+
+        if(Mode == OneStep)
+        {
             break;
-        case Tag_UpdateCheckSum:
-                crc_ccitt_init();
-                //crc_ccitt_update(buf,len);
-                WriteCRCValue();
+        }
+        case Tag_UpdateWCounter:
 
+                RFIDRetValue = WriteWCounter(readerID);
+                if(RFIDRetValue == SUCCESS)
+                {
+                    follow_stages[TagStage[readerID]] = OK;
+                    RFID_Status_Words[readerID].errors.TagUpdateWriteCounter = S_SUCCEEDED;
+                }
+                else
+                {
+                    follow_stages[TagStage[readerID]] = ERROR;
+                    RFID_Status_Words[readerID].errors.TagUpdateWriteCounter = S_FAILED;
+                }
             break;
+
+        case Tag_UpdateCRC:
+/*
+                RFIDRetValue = WriteCRCValue(readerID);
+                if(RFIDRetValue == SUCCESS)
+                {
+                    follow_stages[TagStage[readerID]] = OK;
+                    RFID_Status_Words[readerID].errors.TagUpdateCRC = S_SUCCEEDED;
+                }
+                else
+                {
+                    follow_stages[TagStage[readerID]] = ERROR;
+                    RFID_Status_Words[readerID].errors.TagUpdateCRC = S_FAILED;
+                }
+            break;
+
+            */
+        case Tag_UpdateCheckSum://after updating the counter
+
+            RFIDRetValue = WriteCRCValue(readerID);
+            if(RFIDRetValue == SUCCESS)
+            {
+                follow_stages[TagStage[readerID]] = OK;
+                RFID_Status_Words[readerID].errors.TagUpdateCRC = S_SUCCEEDED;
+                if(Mode == Auto)
+                    TagStage[readerID] = Tag_DisconnectComm;
+            }
+            else
+            {
+                follow_stages[TagStage[readerID]] = ERROR;
+                RFID_Status_Words[readerID].errors.TagUpdateCRC = S_FAILED;
+                //while(1);  /---------------------------------------------------------------------------------------------- ERROR
+            }
+
+            if(Mode == OneStep)
+            {
+                break;
+            }
+#endif
         case Tag_DisconnectComm:
 
                 RFIDRetValue = RFID_StopDiscovery(readerID);
 
-                if((RFIDRetValue == SUCCESS) && (Mode == Auto))
+                if(RFIDRetValue == SUCCESS)
+                {
+                    TagStage[readerID] = Tag_Idle;
+                    RFID_Status_Words[readerID].errors.TagDisconnect = S_SUCCEEDED;
+                }
+                else
                 {
-                    TagStage = Tag_Idle;
+                    follow_stages[TagStage[readerID]] = ERROR;
+                    RFID_Status_Words[readerID].errors.TagDisconnect = S_FAILED;
+                    //while(1);  /---------------------------------------------------------------------------------------------- ERROR
                 }
 
         break;
@@ -270,7 +1074,344 @@ uint32_t RFIDTagComm_StateMachine(StateMachineMode Mode)
         break;
     }
 
-    Status |= RFID_Pint_Errors(readerID, RFIDRetValue);
+    Status |= RFID_Print_Errors(readerID, RFIDRetValue);
 
     return Status;
 }
+
+/*
+//-------------------------------------------------------------------------------
+//every 1 sec
+
+void RFIDCycleEvery1Sec()
+{
+    static RFID_READER_ID readerID;
+    static bool FirstTime = true;
+
+    for(readerID = READER_1; readerID < Max_Readers ; readerID++)
+    {
+        Active_RFID_Reader = readerID + 1;
+
+        if (    (Is_Cartridge_Present(Active_RFID_Reader) == true)               &&
+                (RFID_Status_Words[readerID].errors.ReaderInit   == S_SUCCEEDED) &&
+                ( (FirstTime                     == true) ||
+                  (RequestForDiscovery[readerID] == true)
+                )
+            )
+        {
+            if(RFID_Status_Words[readerID].errors.TagDiscovery   == S_SUCCEEDED)
+            {
+                FirstTime = false;
+            }
+            else
+            {
+                Trigger_RFID_TestTask(readerID);
+            }
+
+//            if(RFID_Status_Words[readerID].errors.TagDiscovery != S_SUCCEEDED)
+//            {
+//                DiscoverRFIDTagEvery100mSec[readerID] = true;
+//                RFID_Discovery_TimeOut[readerID] = RFID_Discovery_TimeOut_Limit;
+//            }
+//            else if(TagStage[readerID] < Tag_DisconnectComm)
+//            {
+//                DiscoverRFIDTagEvery100mSec[readerID] = true;
+//            }
+//
+        }
+//        else if(Is_Cartridge_Present(Active_RFID_Reader) == false)
+//        {
+//            //Initial values
+//            RFID_Status_Words[readerID].uint  &= 0x000F;//Reset status except ReaderAvailability  + ReaderInit
+//            FirstTime = true;
+//            RequestForDiscovery[readerID] = false;
+//            DiscoverRFIDTagEvery100mSec[readerID] = false;
+//        }
+        else if (RFID_Status_Words[readerID].errors.TagDisconnect   == S_SUCCEEDED)
+            {
+                FirstTime = false;
+            }
+
+    }
+//    FirstTime = false;
+}
+//---------------------------------------------------------------------------------
+//every 100 m sec
+
+void RFIDCallEvery100mSec(RFID_READER_ID readerID)
+{
+    if ((RFID_Status_Words[readerID].errors.TagDiscovery != S_SUCCEEDED) && (RFID_Discovery_TimeOut[readerID] > 0))
+    {
+        if(RFID_Status_Words[readerID].errors.ReaderPreDiscovery != S_SUCCEEDED)
+        {
+            //call ReaderPreDiscovery[i]
+            TagStage[readerID] = Tag_PrepareForDiscovery;
+        }
+        else
+        {
+            //call TagDiscovery[i]
+            TagStage[readerID] = Tag_Indetification;
+        }
+        RFIDTagComm_StateMachine(readerID, OneStep);
+        RFID_Discovery_TimeOut[readerID]--;
+    }
+    else if( (RFID_Discovery_TimeOut[readerID] == 0) && (RFID_Status_Words[readerID].errors.TagDiscovery != S_SUCCEEDED) )
+    {
+        //RFID_Discovery_TimeOut error
+        DiscoverRFIDTagEvery100mSec[readerID] = false;
+    }
+    else if ((RFID_Status_Words[readerID].errors.TagDiscovery == S_SUCCEEDED) && (TagStage[readerID] < Tag_DisconnectComm))
+    {
+        DiscoverRFIDTagEvery100mSec[readerID] = false;
+        TagStage[readerID]  = Tag_GetUid;
+        //if(TagStage[readerID] < Tag_DisconnectComm)
+        //    TagStage[readerID]++;
+        RFIDTagComm_StateMachine(readerID, Auto);
+    }
+    else
+    {
+        DiscoverRFIDTagEvery100mSec[readerID] = false;
+    }
+}
+//------------------------------------------------------------
+*/
+
+//-------------------------------------------------------------------------------
+//every 1 sec
+
+void RFIDCycleEvery1Sec()
+{
+    static RFID_READER_ID readerID;
+    //static bool FirstTime = true;
+
+    if(Is_AnyCartridge_presence() == true)//at least one is presence
+    {
+        for(readerID = READER_1; readerID < Max_Readers ; readerID++)
+        {
+            Active_RFID_Reader = readerID + 1;
+
+            if (    (Is_Cartridge_Present(Active_RFID_Reader) == true)               &&
+                    (RFID_Status_Words[readerID].errors.ReaderInit   == S_SUCCEEDED)
+                )
+            {
+                //if(RFID_Status_Words[readerID].errors.ReaderPreDiscovery   != S_SUCCEEDED)
+                if(RFID_Status_Words[readerID].errors.ReaderPreDiscovery   == S_NA)
+                {
+                    DiscoverRFIDTagEvery100mSec[readerID] = false;
+                    Trigger_RFID_Prepare(readerID);//Send Commnd to start discovery
+                    //Trigger_RFID_TestTask(readerID);
+                }
+                else if(RFID_Status_Words[readerID].errors.ReaderPreDiscovery   == S_FAILED)
+                {
+                    //IFS Error - Reader error!!!
+                }
+                else // S_SUCCEEDED - recieve the start discovery command
+                {
+                    if(RFID_Status_Words[readerID].errors.TagDiscovery   == S_NA)
+                    {
+                        Trigger_RFID_Discovery(readerID);//check if discovery
+                    }
+                    else if(RFID_Status_Words[readerID].errors.TagDiscovery   == S_FAILED)
+                    {
+                        //didn't find tag yet - TBD - Tag error after X cycles !!!!
+                    }
+                    else // S_SUCCEEDED
+                    {
+                        DiscoverRFIDTagEvery100mSec[readerID] = true;
+                    }
+                }
+            }
+            else
+            {
+                if(RFID_Status_Words[readerID].errors.ReaderInit   == S_FAILED)
+                {
+                    //IFS Error - Reader error!!!
+                }
+                else
+                if(Is_Cartridge_Present(Active_RFID_Reader) == false)
+                {
+                    //Initial values
+                    RFID_Status_Words[readerID].uint64bit  &= 0x000F;//Reset status except ReaderAvailability  + ReaderInit
+                    DiscoverRFIDTagEvery100mSec[readerID] = false;
+                }
+            }
+        }
+    }
+}
+//---------------------------------------------------------------------------------
+//every 100 m sec
+
+
+void RFIDCallEvery100mSec(RFID_READER_ID readerID)
+{
+    static uint16_t Offset = 0;
+    STATUS_ENUM Status = 0;
+
+    if (Is_Cartridge_Present(Active_RFID_Reader) == true)
+    {
+        Status = (RFID_Status_Words[readerID].uint64bit & (0x0300 << Offset))>>(8 + Offset);
+
+        switch(Status)
+        {
+            case S_NA:
+                switch (Offset)
+                {
+                    case 0x00:
+                            Trigger_RFID_GetUid(readerID);
+                        break;
+                    case 0x02:
+                        #ifdef RFID_NOTUSE_PASSWORD
+                            RFID_Status_Words[readerID].errors.TagCheckPassword = S_NOTINUSE;
+                        #else
+                            Trigger_RFID_GetPassword(readerID);
+                        #endif
+                        break;
+                    case 0x04:
+                            Trigger_RFID_ReadData(readerID);
+                        break;
+                    case 0x06:
+                        #ifdef RFID_NOTUSE_CRC
+                            RFID_Status_Words[readerID].errors.TAgCheckDataCRC = S_NOTINUSE;
+                        #else
+                            Trigger_RFID_CheckCRC(readerID);
+                        #endif
+                        break;
+                    case 0x08:
+                        #ifdef  RFID_NOTUSE_HASHTABLE
+                            RFID_Status_Words[readerID].errors.TagCheckHash = S_NOTINUSE;
+                        #else
+                            Trigger_RFID_CheckHash(readerID);
+                        #endif
+                        break;
+                    case 0x0A:
+                            #ifdef RFID_NOTUSE_VALIDITY
+                                RFID_Status_Words[readerID].errors.TagCheckValidity = S_NOTINUSE;
+                            #else
+                                Trigger_RFID_CheckValid(readerID);
+                            #endif
+                        break;
+                    case 0x0C:
+                        #ifdef RFID_NOTUSE_COUNTER
+                            RFID_Status_Words[readerID].errors.TagCounterLimit = S_NOTINUSE;
+                        #else
+
+                            #ifdef RFID_FORCE_READER2
+                                RFID_Check_EOL_Date();
+                            #endif
+
+                            if(readerID != READER_1)
+                            {
+                                RFID_Update_Waste_Filled_Date(readerID);
+                            }
+                            else if(readerID == READER_1)
+                            {
+                                RFID_Check_EOL_Date();
+                            }
+                            Trigger_RFID_CheckCounter(readerID);
+                        #endif
+                        break;
+                    case 0x0E:
+                        #ifdef RFID_NOTUSE_BLACKLIST
+                            RFID_Status_Words[readerID].errors.TAgCheckBlockedField = S_NOTINUSE;
+                        #else
+                            Trigger_RFID_CheckBlockedField(readerID);
+                        #endif
+                        break;
+                    case 0x10:
+                        //Trigger_RFID..
+                        break;
+                    case 0x12:
+                        //Trigger_RFID..
+                    break;
+
+                    default:
+                    break;
+
+                }
+
+            break;
+            case S_FAILED:
+            case 2:
+                    //Tag Error;
+                break;
+            case S_SUCCEEDED:
+            case S_NOTINUSE:
+            case 3:
+                    if(Offset < 0xC0000000)
+                        Offset = Offset + 2;
+                break;
+        }
+    }
+    else
+    {
+        DiscoverRFIDTagEvery100mSec[readerID] = false;
+    }
+
+
+}
+
+/*
+void RFIDCallEvery100mSec(RFID_READER_ID readerID)
+{
+    if (Is_Cartridge_Present(Active_RFID_Reader) == true)
+    {
+                if(RFID_Status_Words[readerID].errors.TagGetUID   == S_NA)
+                {
+                    Trigger_RFID_GetUid(readerID);
+                }
+                else if(RFID_Status_Words[readerID].errors.TagGetUID   == S_FAILED)
+                {
+                    //Tag error!!!
+                }
+                else // S_SUCCEEDED
+                {
+                    if(RFID_Status_Words[readerID].errors.TagReadData   == S_NA)
+                    {
+                        Trigger_RFID_ReadData(readerID);// + CRC
+                    }
+                    else if(RFID_Status_Words[readerID].errors.TagReadData   == S_FAILED)
+                    {
+                        //Tag error!!!
+                    }
+                    else // TagReadData S_SUCCEEDED
+                    {
+                        #ifdef RFID_NOTUSE_CRC
+                            RFID_Status_Words[readerID].errors.TAgCheckDataCRC =  S_NOTINUSE;
+                        #endif
+
+                        if((RFID_Status_Words[readerID].errors.TAgCheckDataCRC   == S_SUCCEEDED) || (RFID_Status_Words[readerID].errors.TAgCheckDataCRC   == S_NOTINUSE))
+                        {
+                            if(RFID_Status_Words[readerID].errors.TagCheckHash   == S_NA)
+                            {
+                                Trigger_RFID_CheckHash(readerID);
+                            }
+                            else if(RFID_Status_Words[readerID].errors.TagCheckHash   == S_FAILED)
+                            {
+                                //Tag error!!!
+                            }
+                            else // S_SUCCEEDED
+                            {
+                                DiscoverRFIDTagEvery100mSec[readerID] = false; // temporaray - move to the next stage
+                            }
+                        }
+                        else
+                        {
+                            //Tag error!!!
+                        }
+                    }
+                }
+    }
+    else
+    {
+        DiscoverRFIDTagEvery100mSec[readerID] = false;
+    }
+
+}
+*/
+//------------------------------------------------------------
+
+void UpdateWCounterCRC(RFID_READER_ID readerID)//Call after any change in the tag  - to call from WriteValue function
+{
+    Trigger_RFID_UpdateWCounter(readerID);//Counts writes to the tag (not include this counter and CRC)
+    Trigger_RFID_UpdateCRC(readerID);
+}
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC.h
index 1e1f0fd91..d09fd0430 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC.h
@@ -8,6 +8,12 @@
 #ifndef DRIVERS_I2C_COMMUNICATION_RFID_NFC_NFC_H_
 #define DRIVERS_I2C_COMMUNICATION_RFID_NFC_NFC_H_
 
+#define TAG_NOT_VALID 0
+#define TAG_VALID 1
+
+#define TAG_NOT_BLOCKED 0
+#define TAG_BLOCKED 1
+
 typedef enum
 {
     Tag_Idle,
@@ -16,23 +22,128 @@ typedef enum
     Tag_GetUid,
     Tag_CheckPwd,
     Tag_CheckValidity,
-    Tag_UpdateCounter,
+    Tag_CheckUpdateCounter,
     Tag_CheckHashValue,
-    Tag_InvalidLock,
+    Tag_CheckUpdateBlocked,//blacklist
     Tag_ReadValue,
     Tag_WriteValue,
+    Tag_CheckCRC,
     Tag_UpdateCheckSum,
+    Tag_UpdateValue,
+    Tag_Update_Ink_Fill,
+    Tag_Update_Ink_Used,
+    Tag_Update_Ink_Empty,
+    Tag_UpdateInkMachineID,
+    Tag_Update_Waste_Fill,
+    Tag_Update_Waste_Full,
+    Tag_Update_Waste_Empty,
+    Tag_UpdateWasteMachineID,
+    Tag_UpdateWCounter,
+    Tag_UpdateCRC,
     Tag_DisconnectComm,
+    MAX_TAG_State_Machine,
 }RFID_TAG_State_Machine;
 
-extern RFID_TAG_State_Machine    TagStage;
+/*
+typedef struct
+{
+    //STATUS_ENUM: 0 -  S_NA, 1 - S_SUCCEEDED, 2 - S_FAILED
+    STATUS_ENUM ReaderAvailability      : 2; //0-1
+    STATUS_ENUM ReaderInit              : 2; //2-3
+    STATUS_ENUM ReaderPreDiscovery      : 2; //4-5
+    STATUS_ENUM TagDiscovery            : 2; //6-7
+    STATUS_ENUM TagGetUID               : 2; //8-9
+    STATUS_ENUM TagCheckPassword        : 2; //10-11 //temporary - TagUpdateBlokedField
+    STATUS_ENUM TagReadData             : 2; //12-13
+    STATUS_ENUM TAgCheckDataCRC         : 2; //14-15
+    STATUS_ENUM TagCheckHash            : 2; //16-17
+    STATUS_ENUM TagValidity             : 2; //18-19
+    STATUS_ENUM TagCounterLimit         : 2; //20-21
+    STATUS_ENUM TagUpdateCounters       : 2; //22-23
+    STATUS_ENUM TAgCheckBlockedField    : 2; //24-25
+    STATUS_ENUM TagwriteData            : 2; //26-27
+    STATUS_ENUM TagUpdateCRC            : 2; //28-29
+    STATUS_ENUM TagDisconnect           : 2; //30-31
+}RFID_Errors;
+
+//RFID_Status_Word
+typedef union
+{
+    RFID_Errors errors;
+    uint32_t uint;
+}RFID_STATUS;
+*/
+
+
+typedef struct
+{
+    //STATUS_ENUM: 0 -  S_NA, 1 - S_SUCCEEDED, 2 - S_FAILED
+    STATUS_ENUM ReaderAvailability      : 2; //0-1
+    STATUS_ENUM ReaderInit              : 2; //2-3
+    STATUS_ENUM ReaderPreDiscovery      : 2; //4-5
+    STATUS_ENUM TagDiscovery            : 2; //6-7
+    STATUS_ENUM TagGetUID               : 2; //8-9
+    STATUS_ENUM TagCheckPassword        : 2; //10-11
+    STATUS_ENUM TagReadData             : 2; //12-13
+    STATUS_ENUM TAgCheckDataCRC         : 2; //14-15
+    STATUS_ENUM TagCheckHash            : 2; //16-17
+    STATUS_ENUM TagCheckValidity        : 2; //18-19
+    STATUS_ENUM TagCheckCounterLimit    : 2; //20-21 ----
+    STATUS_ENUM TagUpdateCounters       : 2; //22-23 ----
+    STATUS_ENUM TAgCheckBlockedField    : 2; //24-25
+    STATUS_ENUM TagwriteData            : 2; //26-27 ----
+    STATUS_ENUM TagUpdateCRC            : 2; //28-29
+    STATUS_ENUM TagDisconnect           : 2; //30-31
+
+    STATUS_ENUM TagUpdateInkFill        : 2; //32-31
+    STATUS_ENUM TagUpdateInkUsed        : 2; //34-35
+    STATUS_ENUM TagUpdateInkEmpty       : 2; //36-37
+    STATUS_ENUM TagUpdateInkMachineID   : 2; //38-39
+    STATUS_ENUM TagUpdateWasteFill      : 2; //40-41
+    STATUS_ENUM TagUpdateWasteFull      : 2; //42-43
+    STATUS_ENUM TagUpdateWasteEmpty     : 2; //44-45
+    STATUS_ENUM TagUpdateWasteMachineID : 2; //46-47
+    STATUS_ENUM TagUpdateBlockedField   : 2; //48-49
+    STATUS_ENUM TagUpdateValidity       : 2; //50-51
+    STATUS_ENUM TagUpdateWriteCounter   : 2; //52-53
+    STATUS_ENUM TagLockFields           : 2; //54-55
+    STATUS_ENUM TagEOL                  : 2; //56-57
+    STATUS_ENUM TagUpdateWasteFillDate  : 2; //58-59
+    STATUS_ENUM TagSpare6061            : 2; //60-61
+    STATUS_ENUM TagSpare6232            : 2; //62-63
+}RFID_Errors;
+
+//RFID_Status_Word
+typedef union
+{
+    RFID_Errors errors;
+    uint64_t uint64bit;
+}RFID_STATUS;
+
+extern RFID_STATUS RFID_Status_Words[Max_Readers];
+
+extern RFID_TAG_State_Machine    TagStage[3];
 
 bool  Init_RFID_Reader(RFID_READER_ID Reader_ID);
 uint8_t Connect_to_RFID_Reder(uint8_t readerID);
-uint32_t RFIDTagComm_StateMachine(StateMachineMode Mode);
+uint32_t RFIDTagComm_StateMachine(RFID_READER_ID readerID, StateMachineMode Mode);
+RFID_RETURN_CODE  WriteCRCValue(RFID_READER_ID readerID);
+void RFIDCycleEvery1Sec();
+void RFIDCallEvery100mSec(RFID_READER_ID readerID);
+
+void UpdateWCounterCRC(RFID_READER_ID readerID);//Call after any change in the tag  - to call from WriteValue function
 
 extern uint8_t uid[7];
 extern RFID_RETURN_CODE RFID_Status;
 
+extern NFC_Tag NFC_Tag_R[Max_Readers];
+extern NFC_Tag NFC_Tag_W[Max_Readers];
+extern NFC_Tag NFC_Tag_U[Max_Readers];
+
+extern NxpNci_RfIntf_t tagInfo;
+
+extern bool RequestForDiscovery[3];//request from WHS logic
+extern bool DiscoverRFIDTagEvery100mSec[3];
+
 
 #endif /* DRIVERS_I2C_COMMUNICATION_RFID_NFC_NFC_H_ */
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.c
index 74b00f4ad..b3614ec48 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.c
@@ -14,6 +14,9 @@
 #include "driverlib/sysctl.h"
 #include <Drivers/I2C_Communication/I2C.h>
 #include "drivers/FPGA/FPGA_Comm.h"
+#include <Utilities/delay.h>
+#include "Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h"
+#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
 #include <Drivers/I2C_Communication/RFID_NFC/PN7150/PN7150.h>
 #include <Drivers/I2C_Communication/RFID_NFC/Test_RFID.h>
 #include "drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h"
@@ -21,7 +24,6 @@
 #include "Drivers/I2C_Communication/RFID_NFC/NFC.h"
 #include "driverlib/i2c.h"
 #include <Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC.h>
-#include <Utilities/delay.h>
 //#include <Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC.h>
 //#include <Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC_Settings.h>
 //#include <Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.h>
@@ -191,19 +193,25 @@ void Init_IFS()//must be done after FPGA_Init
         if(Status == OK) // Cstage = 14
         {
             IFS_Init_Status[Cart_i] = IFS_RECOGNIZED_INIT_PASSED;
+            RFID_Status_Words[Cart_i].errors.ReaderAvailability = S_SUCCEEDED;
+            RFID_Status_Words[Cart_i].errors.ReaderInit = S_SUCCEEDED;
             ReportWithPackageFilter(IDSFilter,"------------ IFS RFID reader Initialization Passed -----------------", __FILE__,__LINE__, Cart_i, RpMessage, 0, 0);
             Active_RFID_Reader = (PANEL_BUTTON_OR_CRAT_ID)(Cart_i +1);
         }
         else if(Cstage == 0)//failed in the first step - NFC_Config
         {
             IFS_Init_Status[Cart_i] = IFS_NOT_RECOGNIZED;
+            RFID_Status_Words[Cart_i].errors.ReaderAvailability = S_FAILED;
             ReportWithPackageFilter(IDSFilter,"------------ IFS RFID reader Not recognized-----------------", __FILE__,__LINE__, Cart_i, RpMessage, 0, 0);
         }
         else //(Cstage > 0)  - Passed HW + SW reset, failed in initialization
         {
             IFS_Init_Status[Cart_i] = IFS_RECOGNIZED_INIT_FAILED;
+            RFID_Status_Words[Cart_i].errors.ReaderInit = S_FAILED;
             ReportWithPackageFilter(IDSFilter,"------------ IFS RFID reader Initialization failed-----------------", __FILE__,__LINE__, Cart_i, RpMessage, 0, 0);
         }
+
+        IFS_Availability[Cart_i] = IFS_Init_Status[Cart_i];
     }
 }
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h
index 1440dba1f..fe9ee34bf 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h
@@ -22,6 +22,7 @@ typedef enum
     READER_1 = 0,
     READER_2 = 1,
     READER_3 = 2,
+    Max_Readers = 3,
 }RFID_READER_ID;
 
 #ifndef EVALUATION_BOARD
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h
index 70e82cb39..7d11720a5 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h
@@ -8,6 +8,42 @@
 #ifndef DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFIDTAGINFO_H_
 #define DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFIDTAGINFO_H_
 
+typedef enum
+{
+    Serial_i         =  0,
+    FactoryID_i      =  1,
+    FillingSystem_i  =  2,
+    WorkOrder_i      =  3,
+    ColorName_i      =  4,
+    Catagory_i       =  5,
+    Type_i           =  6,
+    CartSize_i       =  7,
+    InkVolume_i      =  8,
+    InkBatch_i       =  9,
+    InkMfgDate_i     = 10,
+    InkEolDate_i     = 11,
+    CartFillDate_i   = 12,
+    WasteFillDate_i  = 13,
+    Misc_i           = 14,
+    PluginCounter_i  = 15,
+    InkFill_i        = 16,
+    InkUsed_i        = 17,
+    InkEmpty_i       = 18,
+    WasteEmpty_i     = 19,
+    WasteFilling_i   = 20,
+    WasteFull_i      = 21,
+    Blocked_i        = 22,
+    Fail_i           = 23,
+    WasteCounter_i   = 24,
+    MachineIdInl_i   = 25,
+    MachineIdWst_i   = 26,
+    rReserved_i      = 27,
+    Counter_i        = 28,
+    TagValid_i       = 29,
+    HashValue_i      = 30,
+    CRC16_i          = 31,
+}RFIFvalueIndex;
+
 typedef struct
 {
     uint8_t  tagId[8];        //RFID Tag unique ID
@@ -77,7 +113,7 @@ typedef struct
 typedef union
 {
     NFCTag_t Struct;
-    uint8_t  Buf[sizeof(NFCTag_t)];
+    uint8_t     Buf[sizeof(NFCTag_t)];
 }NFC_Tag;
 
 #endif /* DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFIDTAGINFO_H_ */
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_IFS.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_IFS.c
new file mode 100644
index 000000000..a14b3b4e7
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_IFS.c
@@ -0,0 +1,294 @@
+/*
+ * RFID_IFS.c
+ *
+ *  Created on: 24 Sep 2020
+ *      Author: avi
+ */
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h> //memset
+#include <DataDef.h>
+#include "datadef.h"
+#include <Utilities/delay.h>
+#include "drivers/I2C_Communication/I2C.h"
+#include "drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.h"
+#include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
+#include "drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h"
+
+#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
+#include <Drivers/I2C_Communication/RFID_NFC/NFC.h>
+#include "drivers/Flash_ram/MCU_E2Prom.h"
+#include "Communication/Connection.h"
+#include "RFID_Settings.h"
+
+bool RFID_Check_EOL_Date() // InK EOL only for READER_1
+{
+
+    uint8_t Status = OK;
+    uint8_t EolYear = 0;
+    uint8_t EolMonth = 0;
+    uint8_t EolDay = 0;
+
+    RFID_READER_ID readerID = READER_1;// InK EOL only for READER_1
+
+    #ifdef RFID_FORCE_READER2
+        readerID = READER_2;// Just for testing
+    #endif
+
+//#ifdef USE_RFID_LOGIC
+    if(RFID_Status_Words[readerID].errors.TagEOL == S_NA)
+    {
+        EolYear  = (NFC_Tag_R[readerID].Struct.inkEolDate[6] - '0') *10 + (NFC_Tag_R[readerID].Struct.inkEolDate[7] - '0');
+        EolMonth = (NFC_Tag_R[readerID].Struct.inkEolDate[3] - '0') *10 + (NFC_Tag_R[readerID].Struct.inkEolDate[4] - '0');
+        EolDay = (NFC_Tag_R[readerID].Struct.inkEolDate[0] - '0') *10 + (NFC_Tag_R[readerID].Struct.inkEolDate[1] - '0');
+
+        if((EolYear <19) || (EolMonth > 12 ) || (EolDay > 31))
+        {
+            Status = WARNING;//illegal Date value
+        }
+        else if((LocalTime.tm_year < 20) || (LocalTime.tm_mday == 0) || ((LocalTime.tm_mon) > 12) || (LocalTime.tm_mday > 31))
+        {
+            Status = WARNING;//illegal Date value
+        }
+        else
+        if(EolYear >  LocalTime.tm_year)
+        {
+            Status = ERROR;
+        }
+        else
+        if(EolYear <  LocalTime.tm_year)
+        {
+            Status = OK;
+        }
+        else
+        {
+            if(EolMonth >  (LocalTime.tm_mon))
+            {
+                Status = ERROR;
+            }
+            else
+            if(EolMonth <  (LocalTime.tm_mon))
+            {
+                Status = OK;
+            }
+            else
+            {
+                if(EolDay >  LocalTime.tm_mday)
+                {
+                    Status = ERROR;
+                }
+                else
+                if(EolDay <=  LocalTime.tm_mday)
+                {
+                    Status = OK;
+                }
+            }
+        }
+
+        if(Status == OK)
+        {
+            RFID_Status_Words[readerID].errors.TagEOL = S_SUCCEEDED;
+        }
+        else if(Status == WARNING)
+        {
+            //RFID_Status_Words[readerID].errors.TagEOL = S_FAILED;//?
+             //we have no date from  USB comm.
+            //leave the TagEOL ERROR as S_NA
+            //Print log error
+        }
+        else
+        {
+            RFID_Status_Words[readerID].errors.TagEOL = S_FAILED;
+        }
+    }
+//#endif
+    return Status;
+}
+
+
+bool RFID_Check_Coloer_Match(uint8_t * UserColorName, uint8_t ColorNameSize)
+{
+#ifdef USE_RFID_LOGIC
+    uint8_t i = 0;
+
+    RFID_READER_ID readerID = READER_1;// InK EOL only for READER_1
+
+    #ifdef RFID_FORCE_READER2
+        readerID = READER_2;// Just for testing
+    #endif
+
+    if(ColorNameSize > sizeof(NFC_Tag_R[readerID].Struct.colorName))
+        return ERROR;
+
+    for(i = 0; i < ColorNameSize; i++)
+    {
+        if(NFC_Tag_R[readerID].Struct.colorName[i] != UserColorName[i])
+            return ERROR;
+    }
+#endif
+    return OK;
+}
+
+bool RFID_Update_Ink_Fill()//must update CRC
+{
+#ifdef USE_RFID_LOGIC
+
+    RFID_READER_ID readerID = READER_1;// InK EOL only for READER_1
+
+    #ifdef RFID_FORCE_READER2
+        readerID = READER_2;// Just for testing
+    #endif
+
+    uint32_t Tuint32 = 0;
+    RFID_RETURN_CODE RFIDRetValue;
+    uint8_t tagMemory[1024];
+
+    if(NFC_Tag_R[readerID].Struct.inkFill != 0)
+    {
+        return ERROR;
+    }
+
+    //inkFill
+    NFC_Tag_U[readerID].Struct.inkFill = 1;
+    Tuint32 = NFC_Tag_U[readerID].Struct.inkFill;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 16, Tuint32 );
+
+    //RFIDRetValue |= WriteCRCValue(readerID);
+
+    if(RFIDRetValue != SUCCESS)
+    {
+        RFID_Status_Words[readerID].errors.TagUpdateInkFill = S_FAILED;
+        return ERROR;
+    }
+    RFID_Status_Words[readerID].errors.TagUpdateInkFill = S_SUCCEEDED;
+#endif
+    return OK;
+}
+
+bool RFID_Update_Ink_Used()//must update CRC
+{
+#ifdef USE_RFID_LOGIC
+
+    RFID_READER_ID readerID = READER_1;// InK EOL only for READER_1
+
+    #ifdef RFID_FORCE_READER2
+        readerID = READER_2;// Just for testing
+    #endif
+
+    uint32_t Tuint32 = 0;
+    RFID_RETURN_CODE RFIDRetValue;
+    uint8_t tagMemory[1024];
+
+    if(NFC_Tag_R[readerID].Struct.inkUsed != 0)
+    {
+        return ERROR;
+    }
+
+    //inkUsed
+    NFC_Tag_U[readerID].Struct.inkUsed = 1;
+    Tuint32 = NFC_Tag_U[readerID].Struct.inkUsed;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 17, Tuint32 );
+
+    //RFIDRetValue |= WriteCRCValue(readerID);
+
+    if(RFIDRetValue != SUCCESS)
+    {
+        RFID_Status_Words[readerID].errors.TagUpdateInkUsed = S_FAILED;
+        return ERROR;
+
+    }
+    RFID_Status_Words[readerID].errors.TagUpdateInkUsed = S_SUCCEEDED;
+#endif
+
+    return OK;
+}
+
+bool RFID_Update_Ink_Empty()//must update CRC
+{
+#ifdef USE_RFID_LOGIC
+    uint32_t Tuint32 = 0;
+    RFID_RETURN_CODE RFIDRetValue;
+    uint8_t tagMemory[1024];
+
+    RFID_READER_ID readerID = READER_1;// InK EOL only for READER_1
+
+    #ifdef RFID_FORCE_READER2
+        readerID = READER_2;// Just for testing
+    #endif
+
+    if(NFC_Tag_R[readerID].Struct.inkEmpty != 0)
+    {
+        return ERROR;
+    }
+
+    //inkEmpty
+    NFC_Tag_U[readerID].Struct.inkEmpty = 1;
+    Tuint32 = NFC_Tag_U[readerID].Struct.inkEmpty;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 18, Tuint32 );
+
+    //RFIDRetValue |= WriteCRCValue(readerID);
+
+    if(RFIDRetValue != SUCCESS)
+    {
+        RFID_Status_Words[readerID].errors.TagUpdateInkEmpty = S_FAILED;
+        return ERROR;
+    }
+    RFID_Status_Words[readerID].errors.TagUpdateInkEmpty = S_SUCCEEDED;
+#endif
+
+    return OK;
+}
+
+bool RFID_Update_Ink_Filled_MachineID()//must update CRC
+{
+#ifdef USE_RFID_LOGIC
+    RFID_RETURN_CODE RFIDRetValue;
+    uint32_t dummy = 0;
+    uint8_t i = 0;
+
+    RFID_READER_ID readerID = READER_1;// InK EOL only for READER_1
+
+    #ifdef RFID_FORCE_READER2
+        readerID = READER_2;// Just for testing
+    #endif
+
+    if(NFC_Tag_R[readerID].Struct.MachineIdInl != 0)
+    {
+        return ERROR;
+    }
+
+    //MachineIdInl
+    for(i = 0; i < sizeof(NFC_Tag_U[readerID].Struct.MachineIdInl); i++)
+    {
+        NFC_Tag_U[readerID].Struct.MachineIdInl[i] = Serial[i] + 0x30; // move to ASCII
+    }
+
+    RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_U[readerID].Struct.MachineIdInl, 25, dummy );
+
+    //RFIDRetValue |= WriteCRCValue(readerID);
+
+    if(RFIDRetValue != SUCCESS)
+    {
+        RFID_Status_Words[readerID].errors.TagUpdateInkMachineID = S_FAILED;
+        return ERROR;
+    }
+    RFID_Status_Words[readerID].errors.TagUpdateInkMachineID = S_SUCCEEDED;
+#endif
+
+    return OK;
+}
+
+
+uint32_t RFID_Get_Identification_Status(RFID_READER_ID readerID)
+{
+    //return OK if the steps Availability,Init,PreDiscovery,Discovery,UID,Password,Read,CRC,Hash,Validity Passed (or skipped if NA)
+#warning To add support for blacklist
+    if( (RFID_Status_Words[readerID].errors.TagCheckValidity == S_NOTINUSE)  ||
+        (RFID_Status_Words[readerID].errors.TagCheckValidity == S_SUCCEEDED) ||
+        (RFID_Status_Words[readerID].errors.TagCheckValidity == 0x03)            )//NA
+    {
+        return OK;
+    }
+
+    return ERROR;
+}
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_IFS.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_IFS.h
new file mode 100644
index 000000000..cc929f467
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_IFS.h
@@ -0,0 +1,19 @@
+/*
+ * RFID_IFS.h
+ *
+ *  Created on: 24 Sep 2020
+ *      Author: avi
+ */
+
+#ifndef DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFID_IFS_H_
+#define DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFID_IFS_H_
+
+bool RFID_Check_EOL_Date(); // InK EOL only for READER_1
+bool RFID_Check_Coloer_Match(uint8_t * UserColorName, uint8_t ColorNameSize);
+bool RFID_Update_Ink_Fill();
+bool RFID_Update_Ink_Used();
+bool RFID_Update_Ink_Empty();
+bool RFID_Update_Ink_Filled_MachineID();
+uint32_t RFID_Get_Identification_Status(RFID_READER_ID readerID);
+
+#endif /* DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFID_IFS_H_ */
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_Settings.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_Settings.h
new file mode 100644
index 000000000..dec268ea5
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_Settings.h
@@ -0,0 +1,40 @@
+/*
+ * RFID_Settings.h
+ *
+ *  Created on: 22 Sep 2020
+ *      Author: avi
+ */
+
+#ifndef DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFID_SETTINGS_H_
+#define DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFID_SETTINGS_H_
+
+//#define RFID_LOCK_FILEDS_FOR_READ_ONLY
+#define RFID_NOTUSE_BLACKLIST
+#define RFID_NOTUSE_PASSWORD
+#define RFID_NOTUSE_CRC
+
+#define RFID_NOTUSE_VALIDITY
+//#define RFID_NOTUSE_COUNTER
+#define RFID_NOTUSE_HASHTABLE
+//#define RFID_USE_EMOTY_TAG_FOR_WASTE
+
+//#define  RFID_FORCE_READER2 //only for development
+
+#ifdef RFID_FORCE_READER2
+    #warning RFID_FORCE_READER2 is set !!!
+#endif
+
+#define USE_DELAY_WRITE_TAG
+//#define USE_DELAY_READ_TAG
+
+#ifdef USE_DELAY_WRITE_TAG
+    #define RFID_WRITE_DELAY 5000// 5m
+#endif
+
+#ifdef USE_DELAY_READ_TAG
+    #define RFID_READ_DELAY 10000// 10m
+#endif
+
+//-------------------------------------------
+#define RFID_Discovery_TimeOut_Limit 6
+#endif /* DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFID_SETTINGS_H_ */
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_Waste.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_Waste.c
new file mode 100644
index 000000000..5906d1678
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_Waste.c
@@ -0,0 +1,219 @@
+/*
+ * RFID_Waste.c
+ *
+ *  Created on: 24 Sep 2020
+ *      Author: avi
+ */
+
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h> //memset
+#include <DataDef.h>
+#include "datadef.h"
+#include <Utilities/delay.h>
+#include "drivers/I2C_Communication/I2C.h"
+#include "drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.h"
+#include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
+#include "drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h"
+
+#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
+#include <Drivers/I2C_Communication/RFID_NFC/NFC.h>
+#include "drivers/Flash_ram/MCU_E2Prom.h"
+#include "Communication/Connection.h"
+#include "RFID_Settings.h"
+
+bool RFID_Update_Waste_Fill(RFID_READER_ID readerID)//must update CRC
+{
+#ifdef USE_RFID_LOGIC
+    uint32_t Tuint32 = 0;
+    RFID_RETURN_CODE RFIDRetValue;
+    uint8_t tagMemory[1024];
+
+    if((readerID != READER_2) && (readerID != READER_3))
+        return ERROR;
+
+    #ifdef RFID_FORCE_READER2
+        readerID = READER_2;// Just for testing
+    #endif
+
+    if(NFC_Tag_R[readerID].Struct.wasteFilling != 0)
+    {
+        return ERROR;
+    }
+
+    //wasteFilling
+    NFC_Tag_U[readerID].Struct.wasteFilling = 1;
+    Tuint32 = NFC_Tag_U[readerID].Struct.wasteFilling;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 20, Tuint32 );
+
+    //RFIDRetValue |= WriteCRCValue(readerID);
+
+    if(RFIDRetValue != SUCCESS)
+    {
+        RFID_Status_Words[readerID].errors.TagUpdateWasteFill = S_FAILED;
+        return ERROR;
+    }
+    RFID_Status_Words[readerID].errors.TagUpdateWasteFill = S_SUCCEEDED;
+#endif
+    return OK;
+}
+
+bool RFID_Update_Waste_Full(RFID_READER_ID readerID)//must update CRC
+{
+#ifdef USE_RFID_LOGIC
+    uint32_t Tuint32 = 0;
+    RFID_RETURN_CODE RFIDRetValue;
+    uint8_t tagMemory[1024];
+
+    if((readerID != READER_2) && (readerID != READER_3))
+        return ERROR;
+
+    #ifdef RFID_FORCE_READER2
+        readerID = READER_2;// Just for testing
+    #endif
+
+    if(NFC_Tag_R[readerID].Struct.wasteFull != 0)
+    {
+        return ERROR;
+    }
+
+    //wasteFull
+    NFC_Tag_U[readerID].Struct.wasteFull = 1;
+    Tuint32 = NFC_Tag_U[readerID].Struct.wasteFull;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 21, Tuint32 );
+
+    //RFIDRetValue |= WriteCRCValue(readerID);
+
+    if(RFIDRetValue != SUCCESS)
+    {
+        RFID_Status_Words[readerID].errors.TagUpdateWasteFull = S_FAILED;
+        return ERROR;
+    }
+    RFID_Status_Words[readerID].errors.TagUpdateWasteFull = S_SUCCEEDED;
+#endif
+
+    return OK;
+}
+
+bool RFID_Update_Waste_Empty(RFID_READER_ID readerID)//must update CRC
+{
+#ifdef USE_RFID_LOGIC
+    uint32_t Tuint32 = 0;
+    RFID_RETURN_CODE RFIDRetValue;
+    uint8_t tagMemory[1024];
+
+    if((readerID != READER_2) && (readerID != READER_3))
+        return ERROR;
+
+    #ifdef RFID_FORCE_READER2
+        readerID = READER_2;// Just for testing
+    #endif
+
+    if(NFC_Tag_R[readerID].Struct.inkEmpty != 0)
+    {
+        return ERROR;
+    }
+
+    //wasteEmpty
+    NFC_Tag_U[readerID].Struct.wasteEmpty = 1;
+    Tuint32 = NFC_Tag_U[readerID].Struct.wasteEmpty;
+    RFIDRetValue = WriteValue( readerID, &tagInfo, tagMemory, 19, Tuint32 );
+
+    //RFIDRetValue |= WriteCRCValue(readerID);
+
+    if(RFIDRetValue != SUCCESS)
+    {
+        RFID_Status_Words[readerID].errors.TagUpdateWasteEmpty = S_FAILED;
+        return ERROR;
+    }
+    RFID_Status_Words[readerID].errors.TagUpdateWasteEmpty = S_SUCCEEDED;
+#endif
+
+    return OK;
+}
+
+bool RFID_Update_Waste_Filled_MachineID(RFID_READER_ID readerID)//must update CRC
+{
+#ifdef USE_RFID_LOGIC
+    RFID_RETURN_CODE RFIDRetValue;
+    uint32_t dummy = 0;
+    uint8_t i = 0;
+
+    if(RFID_Status_Words[readerID].errors.TagUpdateWasteMachineID == S_NA)
+    {
+        if((readerID != READER_2) && (readerID != READER_3))
+            return ERROR;
+
+        #ifdef RFID_FORCE_READER2
+            readerID = READER_2;// Just for testing
+        #endif
+
+        if(NFC_Tag_R[readerID].Struct.MachineIdWst != 0)
+        {
+            return ERROR;
+        }
+
+        //WasteFillDate
+        for(i = 0; i < sizeof(NFC_Tag_U[readerID].Struct.MachineIdWst); i++)
+        {
+            NFC_Tag_U[readerID].Struct.MachineIdWst[i] = Serial[i] + 0x30; // move to ASCII
+        }
+
+        RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_U[readerID].Struct.MachineIdWst, 26, dummy );
+
+        //RFIDRetValue |= WriteCRCValue(readerID);
+
+        if(RFIDRetValue != SUCCESS)
+        {
+            RFID_Status_Words[readerID].errors.TagUpdateWasteMachineID = S_FAILED;
+            return ERROR;
+        }
+        RFID_Status_Words[readerID].errors.TagUpdateWasteMachineID = S_SUCCEEDED;
+    }
+#endif
+    return OK;
+}
+
+bool RFID_Update_Waste_Filled_Date(RFID_READER_ID readerID)//must update CRC
+{
+//#ifdef USE_RFID_LOGIC
+    RFID_RETURN_CODE RFIDRetValue;
+    uint32_t dummy = 0;
+    uint8_t i = 0;
+
+    if(RFID_Status_Words[readerID].errors.TagUpdateWasteFillDate == S_NA)
+    {
+        if((readerID != READER_2) && (readerID != READER_3))
+            return ERROR;
+
+        #ifdef RFID_FORCE_READER2
+            readerID = READER_2;// Just for testing
+        #endif
+
+        if(LocalTimeInAScii.Uint64 == 0) //we have no date from  USB comm.
+        {
+            //Add log print
+            return WARNING;// leave the TagUpdateWasteFillDate ERROR as S_NA
+        }
+
+        //WasteFillDate
+        for(i = 0; i< 8; i++)
+            NFC_Tag_U[readerID].Struct.WasteFillDate[i] = LocalTimeInAScii.Buf[i];
+
+        RFIDRetValue = WriteValue( readerID, &tagInfo, NFC_Tag_U[readerID].Struct.WasteFillDate, 13, dummy );
+
+        //RFIDRetValue |= WriteCRCValue(readerID);
+
+        if(RFIDRetValue != SUCCESS)
+        {
+            RFID_Status_Words[readerID].errors.TagUpdateWasteFillDate = S_FAILED;
+            return ERROR;
+        }
+        RFID_Status_Words[readerID].errors.TagUpdateWasteFillDate = S_SUCCEEDED;
+    }
+//#endif
+    return OK;
+}
+
+
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_Waste.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_Waste.h
new file mode 100644
index 000000000..0a55d693c
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/RFID_Waste.h
@@ -0,0 +1,17 @@
+/*
+ * RFID_Waste.h
+ *
+ *  Created on: 24 Sep 2020
+ *      Author: avi
+ */
+
+#ifndef DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFID_WASTE_H_
+#define DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFID_WASTE_H_
+
+bool RFID_Update_Waste_Fill(RFID_READER_ID readerID);
+bool RFID_Update_Waste_Full(RFID_READER_ID readerID);
+bool RFID_Update_Waste_Empty(RFID_READER_ID readerID);
+bool RFID_Update_Waste_Filled_MachineID(RFID_READER_ID readerID);
+bool RFID_Update_Waste_Filled_Date(RFID_READER_ID readerID);
+
+#endif /* DRIVERS_I2C_COMMUNICATION_RFID_NFC_RFID_WASTE_H_ */
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC.c
index 9b1343321..322844146 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC.c
@@ -1,5 +1,6 @@
 #include "include.h"
 #include <Drivers/I2C_Communication/I2C.h>
+#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
 #include <Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC.h>
 #include <Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC_Settings.h>
 #include <Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.h>
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC_Settings.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC_Settings.h
index dc98d7da1..4dfa37811 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC_Settings.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC_Settings.h
@@ -4,7 +4,7 @@
 #define NXPNCI_I2C_ADDR             0x28
 #define MAX_NCI_FRAME_SIZE          258
 #define TIMEOUT_COMMAND             1000
-#define TIMEOUT_DISCOVERY           200
+#define TIMEOUT_DISCOVERY           500//200  AVI
 #define TIMEOUT_EXT                 100
 
 // NCI standard dedicated settings
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.c
index 4171a62a6..7035d172e 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.c
@@ -1,7 +1,15 @@
 #include "include.h"
+#include "datadef.h"
+#include "Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h"
+#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
 #include <Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_NFC.h>
 #include <Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h>
+#include "Drivers/I2C_Communication/RFID_NFC/NFC.h"
 #include "../NFC_MainBaord.h"
+#include <Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.h>
+#include <Drivers/I2C_Communication/RFID_NFC/RFID_Settings.h>
+#include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
+#include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
 
 #define NUM_OF_READERS          4
 #define T2T_BLOCK_SIZE          4 // tested only in NTAG216
@@ -188,7 +196,7 @@ int checkPwd(uint8_t *pwd, uint8_t *PACK)
     return 1;
 }
 
-bool setPwdValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *newPwd )
+bool setPwdValue(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *newPwd )
 {
     uint8_t succeededSize, pwdAddr = 0xE5;
     //if ( pwdAuth( oldPwd, BLOCK_SIZE, &succeededSize) == true)
@@ -199,7 +207,7 @@ bool setPwdValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *newPwd )
     return false;
 }
 
-bool setPACKValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *newPwd )
+bool setPACKValue(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *newPwd )
 {
     uint8_t succeededSize, pwdAddr = 0xE6, dataSize = 2;
 
@@ -209,7 +217,7 @@ bool setPACKValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *newPwd
     return false;
 }
 
-bool activatePwd( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t startingBlock )
+bool activatePwd(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t startingBlock )
 {
     uint8_t pageData[100], dataAddr = 0xE3;
     uint16_t succeededSize;
@@ -302,7 +310,7 @@ RFID_RETURN_CODE RFID_CardModeSend(uint32_t readerID, NxpNci_RfIntf_t *tagInfo,
     }
     return UNSUPPORTED_PROTOCOL;
 }
-
+/* AVI-
 bool IsTagValid(uint32_t readerID, NxpNci_RfIntf_t *tagInfo){
     uint8_t data[16], tagValid;
     if ( ReadValue( readerID, tagInfo, data, 29, &tagValid ) == SUCCESS )
@@ -311,8 +319,30 @@ bool IsTagValid(uint32_t readerID, NxpNci_RfIntf_t *tagInfo){
 
 
 
+}*/
+
+bool IsTagValid(uint32_t readerID)//AVI+
+{
+    bool Status = OK;
+
+    if(NFC_Tag_R[readerID].Struct.tagValid != TAG_VALID)
+        Status = ERROR;
+
+    return Status;
+}
+
+
+bool IsTagBlocked(uint32_t readerID)//AVI+
+{
+    bool Status = OK;
+
+    if(NFC_Tag_R[readerID].Struct.blocked != TAG_NOT_BLOCKED)
+        Status = ERROR;
+
+    return Status;
 }
 
+
 uint32_t GetCounter (uint32_t readerID, NxpNci_RfIntf_t *tagInfo)
 {
     uint8_t data[16];
@@ -334,7 +364,7 @@ void GetBlocksToRead( uint16_t* firstBlock, uint16_t* lastBlock, uint16_t beginB
 }
 
 // the function gets a value and places it into an array that can be written back to the memory
-RFID_RETURN_CODE SetFieldValueIntoBlocks( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *data, uint8_t* dataLen, uint8_t *value, uint8_t valueIndex ){
+RFID_RETURN_CODE SetFieldValueIntoBlocks(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *data, uint8_t* dataLen, uint8_t *value, uint8_t valueIndex ){
     uint16_t firstBlock, lastBlock;
     RFID_RETURN_CODE retCode;
     uint16_t succeededReadSize = 0;
@@ -353,14 +383,52 @@ RFID_RETURN_CODE SetFieldValueIntoBlocks( uint32_t readerID, NxpNci_RfIntf_t *ta
 
 }
 
-RFID_RETURN_CODE WriteValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *value, uint8_t valueIndex, uint32_t valueNum )
+RFID_RETURN_CODE WriteValue(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *value, uint8_t valueIndex, uint32_t valueNum )
 {
     RFID_RETURN_CODE retCode;
     uint8_t dataSize;
     int i;
-    uint8_t data[512], valueNumChar[4] = {0};
+    uint8_t data[256], valueNumChar[4] = {0};
     uint16_t succeededSize = 0;
     uint8_t startBlock;
+
+//    uint8_t Serial[16],index;
+
+//    if(valueIndex == 0)
+//    {
+//        for(index = 0; index < sizeof(Serial); index++)
+//        {
+//            Serial[index] = value[index];
+//        }
+//        index ++;
+//    }
+/*
+    if(Active_RFID_Reader != CART_2)
+    {
+        SetReader(READER_2);//select I2C MUX on main board
+
+    }
+    delayms(5);
+*/
+
+    #ifdef RFID_FORCE_READER2
+        readerID = READER_2;// Just for testing
+    #endif
+
+    if(Active_RFID_Reader != CART(readerID))
+    {
+        Active_RFID_Reader = CART(readerID);
+        SetReader(readerID);//select I2C MUX on main board
+
+    #ifdef USE_DELAY_WRITE_TAG
+           //delayms(5);
+        Task_sleep(RFID_WRITE_DELAY);
+    #endif
+    }
+
+    //SetReader(readerID);//select I2C MUX on main board
+    delayms(5);
+
     if ( Fields[valueIndex].isNum == true )
     {
         for ( i = Fields[valueIndex].fieldLength - 1; i >= 0 ; i-- )
@@ -378,12 +446,18 @@ RFID_RETURN_CODE WriteValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_
         return retCode;
     startBlock = GetBlockLocation( Fields[valueIndex].fieldLocation );
     retCode = RFID_CardModeSend( readerID, tagInfo, data, startBlock, dataSize, &succeededSize);
+
+    if ((valueIndex != Counter_i) && (valueIndex != CRC16_i) && (retCode == SUCCESS))//UPDATE WRITE COUNTER + CRC
+    {
+        UpdateWCounterCRC(readerID);
+    }
+
     return retCode;
 }
 
 
-
-uint32_t ReadValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *value, uint8_t valueIndex, RFID_RETURN_CODE *rfidCode )
+/*
+uint32_t ReadValue(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *value, uint8_t valueIndex, RFID_RETURN_CODE *rfidCode )
 {
     uint32_t retCode = 0;
     uint8_t i, data[512];
@@ -407,8 +481,42 @@ uint32_t ReadValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *value,
     }
     return retCode;
 }
+*/
 
-RFID_RETURN_CODE getUID( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid )
+RFID_RETURN_CODE ReadValue(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *value, uint8_t valueIndex, uint32_t *valueNum )
+{
+    RFID_RETURN_CODE retCode = 0;
+    uint8_t i, data[256];
+    uint16_t succeededSize = 0;
+
+    uint32_t temp;
+
+    uint8_t startBlock = GetBlockLocation( Fields[valueIndex].fieldLocation );
+
+#ifdef USE_DELAY_READ_TAG
+    delayms(RFID_READ_DELAY);//AVI+
+#endif
+
+    retCode = RFID_CardModeReceive( readerID, tagInfo, data, startBlock, Fields[valueIndex].fieldLength + BLOCK_SIZE, &succeededSize);
+    if ( retCode != SUCCESS )
+        return retCode;
+    memcpy( value, &data[ Fields[valueIndex].fieldLocation % BLOCK_SIZE], Fields[valueIndex].fieldLength ) ;
+    if ( Fields[valueIndex].isNum == true )
+    {
+       temp = 0;
+        for ( i = 0; i < Fields[valueIndex].fieldLength; i++ )
+        {
+           temp *= 10;
+           temp += (value[i] - '0');
+
+        }
+        *valueNum = temp;
+
+    }
+    return retCode;
+}
+
+RFID_RETURN_CODE getUID(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid )
 {
     RFID_RETURN_CODE retCode = 0;
     uint16_t succeededSize = 0, block = 0, bytes = 7;
@@ -421,7 +529,7 @@ RFID_RETURN_CODE getUID( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *u
     return retCode;
 }
 
-RFID_RETURN_CODE getPwd( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *pwd )
+RFID_RETURN_CODE getPwd(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *pwd )
 {
     RFID_RETURN_CODE retCode = 0;
     uint16_t succeededSize = 0, block = 230, bytes = 4;
@@ -544,7 +652,7 @@ void InitFields()
     Fields[26].isNum = false;
     //reserved
     Fields[27].fieldLocation = 184;
-    Fields[27].fieldLength   = 16;
+    Fields[27].fieldLength   = 13;//AVI+ 16->13
     Fields[27].isNum = false;
     //counter
     Fields[28].fieldLocation = 197;
@@ -558,6 +666,11 @@ void InitFields()
     Fields[30].fieldLocation = 202;
     Fields[30].fieldLength   = 16;
     Fields[30].isNum = false;
+    //crc                          //AVI+ how many bits?? valueNum in ReadValue is 32 bit
+    Fields[31].fieldLocation = 218;
+    Fields[31].fieldLength   = 4;
+    Fields[31].isNum = true;
+
 }
 
 
@@ -848,7 +961,7 @@ void calcHash( uint8_t *input, uint16_t inputLen, uint8_t *output  )
     MD5Final( output, &context);
 }
 
-bool setHashValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid, uint16_t inputLen )
+bool setHashValue(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid, uint16_t inputLen )
 {
     RFID_RETURN_CODE retCode;
     uint8_t hashValue[16], dummy = 0;
@@ -858,8 +971,8 @@ bool setHashValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid, ui
         return true;
     return false;
 }
-
-int checkHashValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid, uint16_t inputLen )
+/*
+int checkHashValue(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid, uint16_t inputLen )//AVI-
 {
     RFID_RETURN_CODE retCode;
     uint8_t i, hashValueCalc[16], hashValueOnTag[16], dummy = 0;
@@ -881,8 +994,28 @@ int checkHashValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid, u
     }
     return -2;
 }
+*/
 
-bool lockDataSetOnInit( uint32_t readerID, NxpNci_RfIntf_t *tagInfo )
+
+uint8_t hashValueCalc[16];
+int checkHashValue( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo )//AVI+
+{
+    uint8_t i/*, hashValueCalc[16]*/;
+
+    calcHash( NFC_Tag_R[readerID].Struct.tagId, sizeof(NFC_Tag_R[readerID].Struct.tagId), hashValueCalc);
+
+    for ( i = 0; i < 16; i++ )
+    {
+        if ( hashValueCalc[i] != NFC_Tag_R[readerID].Struct.HashValue[i] )
+        {
+            setTagInvalidAndLock( readerID, tagInfo );
+            return -1;
+        }
+    }
+    return 1;
+}
+
+bool lockDataSetOnInit(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo )
 {
     RFID_RETURN_CODE retCode;
     uint16_t succeededSize = 0;
@@ -914,7 +1047,50 @@ bool lockDataSetOnInit( uint32_t readerID, NxpNci_RfIntf_t *tagInfo )
     return false;
 }
 
-bool setTagInvalidAndLock( uint32_t readerID, NxpNci_RfIntf_t *tagInfo )
+bool setTagInvalidAndLock(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo )//+ update checksum //AVI+
+{
+    RFID_RETURN_CODE retCode;
+    #ifdef RFID_LOCK_FILEDS_FOR_READ_ONLY
+        uint16_t succeededSize = 0;
+        uint8_t block = 0xE2, tagValid = 0;
+    #endif
+    uint32_t Tuint32;
+    uint8_t data[16];
+
+    NFC_Tag_U[readerID].Struct.tagValid = TAG_NOT_VALID;
+    Tuint32 = NFC_Tag_U[readerID].Struct.tagValid;
+    retCode = WriteValue( readerID, tagInfo, data, 29, Tuint32 );
+
+    //retCode |= WriteCRCValue(readerID);
+
+    if ( retCode == SUCCESS )
+    {
+        #ifdef RFID_LOCK_FILEDS_FOR_READ_ONLY
+            //lock page 50, pages 48 - 63 (bit 02 of byte 1)
+            retCode = RFID_CardModeReceive( readerID, tagInfo, data, block, BLOCK_SIZE, &succeededSize);
+            if ( retCode == SUCCESS )
+            {
+                retCode = RFID_CardModeReceive( readerID, tagInfo, data, block, BLOCK_SIZE, &succeededSize);
+                if ( retCode == SUCCESS )
+                {
+                    data[0] |= 0x08;//lock pages 48 - 63 (bit 02 of byte 1)
+                    retCode = RFID_CardModeSend( readerID, tagInfo, data, block, BLOCK_SIZE, &succeededSize);
+                    if ( retCode == SUCCESS )
+                    {
+                        return true;
+                    }
+                }
+
+            }
+        #else
+                return true;
+        #endif
+
+    }
+    return false;
+}
+/* AVI-
+bool setTagInvalidAndLock(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo )
 {
     RFID_RETURN_CODE retCode;
     uint16_t succeededSize = 0;
@@ -938,9 +1114,9 @@ bool setTagInvalidAndLock( uint32_t readerID, NxpNci_RfIntf_t *tagInfo )
 
     }
     return false;
-}
-
-int incrementCounterAndCompare( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, int limit )
+}*/
+/*
+int incrementCounterAndCompare(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, int limit )//AVI-
 {
     RFID_RETURN_CODE retCode;
     uint16_t succeededSize = 0;
@@ -956,18 +1132,66 @@ int incrementCounterAndCompare( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, int
     if ( retCode != SUCCESS )
         return -2;
     return 1;
+}*/
+
+int incrementCounterAndCompare( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo)//AVI+
+{
+    RFID_RETURN_CODE retCode;
+    uint8_t tagMemory[1024];
+
+    if (( NFC_Tag_R[readerID].Struct.pluginCounter == 0 ) && (readerID == READER_1))
+    {
+        //OK
+        NFC_Tag_U[readerID].Struct.pluginCounter = NFC_Tag_R[readerID].Struct.pluginCounter + 1; // need to update CRC
+
+        retCode = WriteValue( readerID, tagInfo, tagMemory, 28, NFC_Tag_U[readerID].Struct.pluginCounter );
+        //Add log print
+    }
+    else  if (( NFC_Tag_R[readerID].Struct.wasteCounter == 0 ) && ((readerID == READER_2) || (readerID == READER_3)))
+    {
+        //OK
+        NFC_Tag_U[readerID].Struct.wasteCounter = NFC_Tag_R[readerID].Struct.wasteCounter + 1; // need to update CRC
+
+        retCode = WriteValue( readerID, tagInfo, tagMemory, 28, NFC_Tag_U[readerID].Struct.wasteCounter );
+        //Add log print
+    }
+    else
+    {
+        return -1;
+        //Add log print
+    }
+
+    if ( retCode != SUCCESS )
+    {
+        return -2;
+    }
+    return 1;
 }
 
-void crc_ccitt_init(void)
+RFID_RETURN_CODE incrementCounter( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo)//AVI+
 {
-    m_crc = 0xffff;
+    RFID_RETURN_CODE retCode;
+    uint8_t tagMemory[1024];
+
+    NFC_Tag_U[readerID].Struct.counter = NFC_Tag_R[readerID].Struct.counter + 1; // need to update CRC
+
+    retCode = WriteValue( readerID, tagInfo, tagMemory, 28, NFC_Tag_U[readerID].Struct.counter );
+
+    return retCode;
 }
 
-void crc_ccitt_update(unsigned char * buf, int len)
+//void crc_ccitt_init(void)//AVFI-
+//{
+//    m_crc = 0xffff;
+//}
+
+uint32_t crc_ccitt_update(unsigned char * buf, int len)
 {
     int i, j;
 
-    for ( i = 0; i < len; i++ )
+    uint32_t m_crc = 0xffff;
+
+    for ( i = 8; i < len; i++ )//AVI 0-> 8 start after tag id
     {
         m_crc ^= buf[i];
         for ( j = 0; j < 8; j++ )
@@ -978,6 +1202,10 @@ void crc_ccitt_update(unsigned char * buf, int len)
                 m_crc = (m_crc >> 1);
         }
     }
+
+    m_crc %= 10000; //AVI+ because we are using ASCII the max number in 32 bit is 9999 - need to check with logitag
+
+    return m_crc;
 }
 
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h
index b07c79f4f..150779f70 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h
@@ -3,6 +3,7 @@
 
 #include "LT_NFC_Info.h"
 #include "include.h"
+#include "datadef.h"
 
 typedef enum {
     SUCCESS = 0,
@@ -56,7 +57,7 @@ typedef struct{
 } FIELDS_LOCATIONS;
 
 FIELDS_LOCATIONS Fields[NUMBER_OF_FIELDS];
-unsigned int m_crc;
+//unsigned int m_crc; //AVI-
 
 RFID_RETURN_CODE RFID_Connect(uint32_t readerID);
 RFID_RETURN_CODE RFID_Disconnect(uint32_t readerID);
@@ -68,23 +69,29 @@ RFID_RETURN_CODE RFID_StopDiscovery(uint32_t readerID);
 RFID_RETURN_CODE RFID_WaitForDiscoveryNotification(uint32_t readerID, NxpNci_RfIntf_t *pRfIntf);
 RFID_RETURN_CODE RFID_CardModeReceive(uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *data, uint8_t startBlock, uint16_t dataSize, uint16_t *succeededSize);
 RFID_RETURN_CODE RFID_CardModeSend(uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *data, uint8_t startBlock, uint16_t dataSize, uint16_t *succeededSize);
-bool IsTagValid(uint32_t readerID, NxpNci_RfIntf_t *tagInfo);
+//bool IsTagValid(uint32_t readerID, NxpNci_RfIntf_t *tagInfo);
+bool IsTagValid(uint32_t readerID);//AVI+
+bool IsTagBlocked(uint32_t readerID);//AVI+
 uint32_t GetCounter (uint32_t readerID, NxpNci_RfIntf_t *tagInfo);
-RFID_RETURN_CODE WriteValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *value, uint8_t valueIndex, uint32_t valueNum );
+RFID_RETURN_CODE WriteValue(RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *value, uint8_t valueIndex, uint32_t valueNum );
 
-uint32_t ReadValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *value, uint8_t valueIndex, RFID_RETURN_CODE *rfidCode );
-RFID_RETURN_CODE getUID( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid );
+//uint32_t ReadValue( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *value, uint8_t valueIndex, RFID_RETURN_CODE *rfidCode );
+RFID_RETURN_CODE ReadValue( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *value, uint8_t valueIndex, uint32_t *valueNum );
+RFID_RETURN_CODE getUID( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid );
 void calcHash( uint8_t *input, uint16_t inputLen, uint8_t *output  );
-bool setHashValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid, uint16_t inputLen );
-int checkHashValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid, uint16_t inputLen );
-RFID_RETURN_CODE getPwd( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *pwd );
+bool setHashValue( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid, uint16_t inputLen );
+//int checkHashValue( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *uid, uint16_t inputLen );
+int checkHashValue( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo );//AVI+
+RFID_RETURN_CODE getPwd( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *pwd );
 int checkPwd(uint8_t *pwd, uint8_t *PACK );
-bool setPwdValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *newPwd );
-bool setPACKValue( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *newPwd );
-bool activatePwd( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t startingBlock );
-bool lockDataSetOnInit( uint32_t readerID, NxpNci_RfIntf_t *tagInfo );
-bool setTagInvalidAndLock( uint32_t readerID, NxpNci_RfIntf_t *tagInfo );
-int incrementCounterAndCompare( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, int limit );
+bool setPwdValue( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *newPwd );
+bool setPACKValue( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *newPwd );
+bool activatePwd( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t startingBlock );
+bool lockDataSetOnInit( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo );
+bool setTagInvalidAndLock( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo );
+//int incrementCounterAndCompare( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, int limit );//AVI-
+int incrementCounterAndCompare( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo);//AVI+
+RFID_RETURN_CODE incrementCounter( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo);//AVI+
 /////////////////////////////////////////////////////////////////////////////////////
 void MD5Init( MD5_CTX * );
 void MD5Update( MD5_CTX *, unsigned char *, unsigned int );
@@ -92,7 +99,7 @@ void MD5Final( unsigned char results[16], MD5_CTX * );
 
 //////////////////////////////////////
 void crc_ccitt_init(void);
-void crc_ccitt_update(unsigned char * buf, int len);
+uint32_t crc_ccitt_update(unsigned char * buf, int len);
 //////////////////////////////////////
 
 // helpers
@@ -100,6 +107,6 @@ void InitFields();
 void SetFieldsLocationLen();
 uint16_t GetBlockLocation(uint16_t byte);
 void GetBlocksToRead( uint16_t* firstBlock, uint16_t* lastBlock, uint16_t beginByte, uint16_t length);
-RFID_RETURN_CODE SetFieldValueIntoBlocks( uint32_t readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *data, uint8_t* dataLen, uint8_t *value, uint8_t valueIndex );
+RFID_RETURN_CODE SetFieldValueIntoBlocks( RFID_READER_ID readerID, NxpNci_RfIntf_t *tagInfo, uint8_t *data, uint8_t* dataLen, uint8_t *value, uint8_t valueIndex );
 /////////////////////////////////////
 #endif
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_EMC2302_Fan/WHS_Fan.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_EMC2302_Fan/WHS_Fan.c
index f22d53a77..73bc2f9af 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_EMC2302_Fan/WHS_Fan.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_EMC2302_Fan/WHS_Fan.c
@@ -233,7 +233,7 @@ uint8_t WHS_Check_Fan_Controller_Type(WHS_fan_num fan_number)
         if((Fan_Driver_Info[fan_number].Product_ID != 0x36) || (status != OK))
         {
             I2CFanViaSwitch_ADD_Buf[fan_number] = 0x00;
-            ReportWithPackageFilter(WasteFilter,"unable to recognize the an controller", __FILE__,__LINE__, status, RpMessage, fan_number, 0);
+            ReportWithPackageFilter(WasteFilter,"unable to recognize the WHS fan controller", __FILE__,__LINE__, status, RpMessage, fan_number, 0);
             status = ERROR;
         }
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/IO_Extender_Ports_TCA9555/WHS_IO.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/IO_Extender_Ports_TCA9555/WHS_IO.c
index 3e36d3bba..ea45c89fe 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/IO_Extender_Ports_TCA9555/WHS_IO.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/IO_Extender_Ports_TCA9555/WHS_IO.c
@@ -689,7 +689,7 @@ bool WHS_Control_blub(bool value)
     }
     else
     {
-        #warning Add warning message
+        LOG_ERROR (WHS_HW_Ver, "wrong whs hw version");
         status = ERROR;
     }
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c b/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c
index 779d7e82f..ae862505a 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c
@@ -354,7 +354,7 @@ uint32_t MotorRunCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO
         {
             angle = Calculate_Arm_Distance(Arm_Drier_Center,temp);
 
-            if ((angle<400)||(angle>16000))
+            if (abs(angle)<400)
             {
                 BusyFlag = NOTBUSY;
             }
@@ -376,7 +376,7 @@ uint32_t MotorRunCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) //TODO
                Report("motor timeout",__FILE__,__LINE__,MotorId,RpMessage,MotorTimeout[MotorId],0);
                Busy = BUSY;
             }
-            Report("MotorRunCallBackFunctionMotorRunCallBackFunction curr prev return",__FILE__,encoder,temp,RpWarning,AccumulatedArmMovement,0);
+            Report("MotorRunCallBackFunction curr prev return",__FILE__,encoder,temp,RpWarning,AccumulatedArmMovement,0);
            //call the module callback
            if (MotorCallback[MotorId])
            {
@@ -1128,10 +1128,10 @@ float Calculate_Arm_Angle(uint32_t Drier_Center,uint32_t Current_Angle)
 int Calculate_Arm_Distance(uint32_t Drier_Center,uint32_t Current_Angle)
 {
     int angle;
-    if (Current_Angle >= Drier_Center)
+    //if (Current_Angle >= Drier_Center)
         angle = Current_Angle - Drier_Center;
-    else
-        angle = Current_Angle + 0x3FFF - Drier_Center;
+    //else
+    //    angle = Current_Angle + 0x3FFF - Drier_Center;
     ReportWithPackageFilter(DiagnosticsFilter,"Calculate_Arm_Distance",__FILE__,angle,Current_Angle,RpMessage,Drier_Center,0);
     return angle;
 }
@@ -1163,7 +1163,7 @@ uint32_t LoadingArmReset_Callback(uint32_t MotorId, uint32_t ReadValue)
     {
         ReportWithPackageFilter(DiagnosticsFilter,"LoadingArmReset_Callback OK",__FILE__,__LINE__,Arm_Drier_Center,RpMessage,ReadValue,0);
         MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0);
-        if ((angle<400)||(angle>16000))
+        if (abs(angle)<400)
         //if (fabs(angle)<0.2)
         {
             ReportWithPackageFilter(DiagnosticsFilter,"drier center proximity",__FILE__,temp,Arm_Drier_Center,RpMessage,angle,0);
diff --git a/Software/Embedded_SW/Embedded/Drivers/RFID/RFID_Task.c b/Software/Embedded_SW/Embedded/Drivers/RFID/RFID_Task.c
deleted file mode 100644
index f5101d9e7..000000000
--- a/Software/Embedded_SW/Embedded/Drivers/RFID/RFID_Task.c
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * RFID_Task.c
- *
- *  Created on: Oct 23, 2019
- *      Author: shlomo
- */
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "include.h"
-#include "inc/hw_memmap.h"
-
-#include <Drivers/RFID/RFID_Task.h>
-Task_Handle RFID_Task_Handle;
-Mailbox_Handle          RFID_ReadingMsgQ = NULL;
-typedef enum
-{
-    RFID_Initialization,
-}RFID_ReadingMessages;
-
-typedef struct RFID_ReadingMessage{
-    RFID_ReadingMessages messageId;
-    PANEL_BUTTON_OR_CRAT_ID CartId;
-}RFID_ReadingMessageStruc;
-
-void RFID_Task(UArg arg0, UArg arg1)
-{
-    RFID_ReadingMessageStruc RFID_ReadingMessage;
-    RFID_ReadingMsgQ = Mailbox_create(sizeof(RFID_ReadingMessageStruc), 10, NULL,NULL);
-    RFID_Task_Handle = Task_self();
-//    double dbl_setpoint_Q = 0.0;
-
-    while(1)
-    {
-        Mailbox_pend(RFID_ReadingMsgQ , &RFID_ReadingMessage, BIOS_WAIT_FOREVER);
-
-        switch (RFID_ReadingMessage.messageId)
-        {
-            case RFID_Initialization:
-                    break;
-            default:
-                break;
-        }
-    }
-}
diff --git a/Software/Embedded_SW/Embedded/Drivers/RFID/RFID_Task.h b/Software/Embedded_SW/Embedded/Drivers/RFID/RFID_Task.h
deleted file mode 100644
index e69de29bb..000000000
--- a/Software/Embedded_SW/Embedded/Drivers/RFID/RFID_Task.h
+++ /dev/null
diff --git a/Software/Embedded_SW/Embedded/Drivers/flash_ram/MCU_E2Prom.c b/Software/Embedded_SW/Embedded/Drivers/flash_ram/MCU_E2Prom.c
index f4b72bb0f..750d02f6b 100644
--- a/Software/Embedded_SW/Embedded/Drivers/flash_ram/MCU_E2Prom.c
+++ b/Software/Embedded_SW/Embedded/Drivers/flash_ram/MCU_E2Prom.c
@@ -212,8 +212,10 @@ void MainCardStoredItemRequestFunc(MessageContainer* requestContainer)
         response.maincardstoreddata = &MainCardStoredDataInstance;
         MainCardStoredDataInstance.maincards_n = &Serial[0];
         MainCardStoredDataInstance.n_storeditem = MAX_EEPROM_STORAGE;
+        MainCardStoredDataInstance.storeditem = my_malloc(MAX_EEPROM_STORAGE*sizeof(MainCardStoredItem *));
         for (i = 0;i<MAX_EEPROM_STORAGE;i++)
         {
+            main_card_stored_item__init(&StoredData[i]);
             StoredData[i].has_address = true;
             StoredData[i].has_data = true;
             StoredData[i].address = i;
diff --git a/Software/Embedded_SW/Embedded/Embedded.cfg b/Software/Embedded_SW/Embedded/Embedded.cfg
index 7fdf21819..d636d53d1 100644
--- a/Software/Embedded_SW/Embedded/Embedded.cfg
+++ b/Software/Embedded_SW/Embedded/Embedded.cfg
@@ -174,17 +174,17 @@ MillisecLowTaskParams.instance.name = "MilliSecondLow";
 MillisecLowTaskParams.stackSize = 4096;
 MillisecLowTaskParams.priority = 12;
 Program.global.millisecLow = Task.create("&MillisecLowTask", MillisecLowTaskParams);
-
-var RFID_TaskParams = new Task.Params();
+/*
+var RFID_TaskParams = new Task.Params(); --- moved to I2C_RFID_Disp_Task.c
 RFID_TaskParams.instance.name = "RFID";
 RFID_TaskParams.stackSize = 4096;
-RFID_TaskParams.priority = 11;
+RFID_TaskParams.priority = 10;
 Program.global.millisecLow = Task.create("&RFID_Task", RFID_TaskParams);
-
+*/
 var ADCProcessTaskParams = new Task.Params();
 ADCProcessTaskParams.instance.name = "adcProcess";
 ADCProcessTaskParams.stackSize = 512;
-ADCProcessTaskParams.priority = 10;
+ADCProcessTaskParams.priority = 9;
 Program.global.adcProcess = Task.create("&ADCProcessTask", ADCProcessTaskParams);
 
 var FPGALoad = new Task.Params();
diff --git a/Software/Embedded_SW/Embedded/Main.c b/Software/Embedded_SW/Embedded/Main.c
index c1d83abb3..6c40b7916 100644
--- a/Software/Embedded_SW/Embedded/Main.c
+++ b/Software/Embedded_SW/Embedded/Main.c
@@ -91,9 +91,11 @@
 
 #include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
 #include <Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h>
+#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
 #include <Drivers/I2C_Communication/RFID_NFC/NFC.h>
 
 #include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
+#include <Drivers/I2C_Communication/I2C_RFID_Dispenser_Task/I2C_RFID_Disp_Task.h>
 #include <Utilities/delay.h>
 //*****************************************************************************
 //
@@ -182,8 +184,8 @@ Void errHook(Error_Block *eb)
             System_printf("assertion violation: ");
             f_write(LogFileHandle,"\r\nassertion violation: ",22,&Bytes );
         }
+        CloseLogFile();
     }
-    CloseLogFile();
 
 
 #ifdef WATCHDOG
@@ -222,6 +224,7 @@ void Init_EVB()
 
 int main(void)
 {
+    FRESULT Fresult = FR_OK;
     // Enable interrupts to the processor.
     //
     ROM_IntMasterDisable();
@@ -255,6 +258,39 @@ int main(void)
         LOG_ERROR (iFResult, "Error during init Flash File System");
         assert(iFResult);
     }
+    Fresult = f_mkdir("0://SysInfo");
+    if ((Fresult != FR_OK)&&(Fresult != FR_EXIST))
+    {
+#ifdef WATCHDOG
+        ROM_WatchdogResetDisable(WATCHDOG0_BASE);
+        uint32_t timeout = 0xFFFFFFFFU;
+        ROM_WatchdogReloadSet(WATCHDOG0_BASE, timeout);
+#endif
+        STATUS_GREEN_LED_ON;
+        ACTIVITY_GREEN_LED_ON;
+        COMM_GREEN_LED_ON;
+
+        STATUS_RED_LED_ON;
+        ACTIVITY_RED_LED_ON;
+        COMM_RED_LED_ON;
+        //FFS error
+        Report("FFS error - rebuilding", __FILE__,__LINE__,Fresult, RpMessage,0, 0);
+        FRESULT iFResult = Init_Flash_File_System(true);// true
+        Fresult = f_mkdir("0://SysInfo");
+
+        ACTIVITY_GREEN_LED_OFF;
+        COMM_GREEN_LED_OFF;
+
+        STATUS_RED_LED_OFF;
+        ACTIVITY_RED_LED_OFF;
+        COMM_RED_LED_OFF;
+#ifdef WATCHDOG
+        ROM_WatchdogResetEnable(WATCHDOG0_BASE);
+        timeout = 120000000*3;
+        ROM_WatchdogReloadSet(WATCHDOG0_BASE, timeout);
+#endif
+    }
+
     OpenLogFile();
     Data = MCU_E2PromEmbeddedVersionRead();
     memcpy (&Version,&Data,4);
@@ -271,12 +307,13 @@ int main(void)
    Init_All_I2C();
 #ifndef EVALUATION_BOARD
    Block_Main_MUX_TCA9548A_0xE4();//unused, same i2c as head and whs cards
-   I2C_BUSY_DELAY = 3000; //temporary workaround!!!
+
    Check_Head_Type_Via_EEPROM();//without trigger
    Check_WHS_Type_Via_EEPROM();//without trigger
-   I2C_BUSY_DELAY = 100; //temporary workaround!!!
+   I2C_2_3_BUSY_DELAY  = 100;  //I2C2_BASE, I2C3_BASE
 
    //WHS_Type = WHS_TYPE_NEW; //for testing
+   //Head_Type = HEAD_TYPE_ARC; //for testing
 
    if ((Head_Type >= HEAD_TYPE_UNKNOWN_WITH_CARD)&&(WHS_Type == WHS_TYPE_NEW))
    {
@@ -284,7 +321,7 @@ int main(void)
    }
    if ((Head_Type > HEAD_TYPE_FLAT_WITHOUT_CARD)||(WHS_Type == WHS_TYPE_NEW))
    {
-       I2C_ReadingTask_Init();
+       I2C_ReadingTask_Init();//I2C Task for HEAD + WHS
    }
    if (Head_Type > HEAD_TYPE_FLAT_WITHOUT_CARD)
    {
@@ -295,7 +332,14 @@ int main(void)
    STATUS_GREEN_LED_ON;
    FPGA_Init();
 
-   Check_IFS_Availability();//Init_IFS();//must be done after FPGA_Init
+   I2C_DispRFIDTask_Init();//I2C Task for dispensers & RFID
+
+    #if defined(USE_RFID_LOGIC) || defined(USE_RFID_STUB)
+       Trigger_RFID_Init();  //Init_IFS();//must be done after FPGA_Init
+    #else
+       Check_IFS_Availability();
+    #endif
+
 #endif
 
 	MAP_FPULazyStackingEnable();
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index e754b0df2..eceb92d63 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -631,8 +631,16 @@ JobEndReasonEnum getEndReason(uint32_t AlarmId)
     switch (AlarmItem[AlarmId].AlarmSource)
     {
         case ALARM_SOURCE_TYPE__TemperatureAlarm:
-            usnprintf(AlarmReasonStr, 100, "Temperature problem in heater %d",AlarmItem[AlarmId].DeviceId);
-            return JOB_TEMPERATURE_ALARM;
+            if (AlarmItem[AlarmId].Severity == DEBUG_LOG_CATEGORY__Critical)
+            {
+                usnprintf(AlarmReasonStr, 100, "Safety Critical Alarm %d",AlarmItem[AlarmId].EventType);
+                return JOB_SAFETY_CRITICAL_ALARM;
+            }
+            else
+            {
+                usnprintf(AlarmReasonStr, 100, "Temperature problem in heater %d",AlarmItem[AlarmId].DeviceId);
+                return JOB_TEMPERATURE_ALARM;
+            }
         case ALARM_SOURCE_TYPE__LimitSwitchAlarm:
             usnprintf(AlarmReasonStr, 100, "Dispenser %d empty",AlarmItem[AlarmId].DeviceId);
             return JOB_OUT_OF_DYE;
@@ -987,13 +995,20 @@ JobEndReasonEnum AlarmHandlingPrepareJob(void *CurrentJob)
     }
     if ((IFS_Availability[1] == IFS_RECOGNIZED_INIT_PASSED)&&(IFS_Availability[2] == IFS_RECOGNIZED_INIT_PASSED)) //ifs installed -check cartridges
     {
-
-        tempFoundReason = Waste_CheckState(&AlarmId);
-        if (tempFoundReason!=JOB_OK)
-        {
-            FoundReason = tempFoundReason;
-            ReportWithPackageFilter(AlarmFilter,"waste handling alarm preventing job", __FILE__,__LINE__,AlarmItem[Alarm_i].EventType, AlarmItem[Alarm_i].Severity, AlarmItem[Alarm_i].DeviceId, 0);
-        }
+            tempFoundReason = Waste_CheckState(&AlarmId);
+            if (tempFoundReason!=JOB_OK)
+            {
+                for (Alarm_i = 0;Alarm_i<NumOfSystemAlarms;Alarm_i++)
+                {
+                    if (AlarmId == (AlarmItem[Alarm_i].EventType))
+                    {
+                        FoundReason = tempFoundReason;
+                        AlarmId = Alarm_i;
+                        ReportWithPackageFilter(AlarmFilter,"waste handling alarm preventing job", __FILE__,AlarmId,AlarmItem[Alarm_i].EventType, AlarmItem[Alarm_i].Severity, AlarmItem[Alarm_i].DeviceId, 0);
+                        break;
+                    }
+                }
+            }
     }
     if ((AlarmState[Alarm_i].Status == true)&&(FoundReason == JOB_OK))
     {
@@ -1025,6 +1040,18 @@ JobEndReasonEnum AlarmHandlingPrepareJob(void *CurrentJob)
     if (FoundReason == JOB_OK)
     {
         FoundReason = Winder_Check_Cone();
+        if (FoundReason!=JOB_OK)
+        {
+            for (Alarm_i = 0;Alarm_i<NumOfSystemAlarms;Alarm_i++)
+            {
+                if (AlarmItem[Alarm_i].EventType == EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST)
+                {
+                    AlarmId = Alarm_i;
+                    ReportWithPackageFilter(AlarmFilter,"cone not in place - preventing job", __FILE__,AlarmId,AlarmItem[Alarm_i].EventType, AlarmItem[Alarm_i].Severity, AlarmItem[Alarm_i].DeviceId, 0);
+                    break;
+                }
+            }
+        }
     }
     if (FoundReason != JOB_OK)
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 1057ddbc3..3de362af9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -524,11 +524,13 @@ int TemperatureSum[MAX_HEAD_CARD_TEMP_SENS_ID];
 int TemperatureMin[MAX_HEAD_CARD_TEMP_SENS_ID];
 int TemperatureMax[MAX_HEAD_CARD_TEMP_SENS_ID];
 int TemperatureCount[MAX_HEAD_CARD_TEMP_SENS_ID];
-int TemperatureCalc[MAX_HEAD_CARD_TEMP_SENS_ID];
+int TemperatureCalc[MAX_HEAD_CARD_TEMP_SENS_ID] = {2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1};
 void MillisecUpdateTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId,int temperature)
 {
    //if(TemperatureCount[SensorId]++>=10)
    //    TemperatureCount[SensorId] = 0;
+    if ((temperature>28000)||(temperature<900))
+        return;
     TemperatureCount[SensorId]++;
     if (TemperatureMax[SensorId]<temperature) TemperatureMax[SensorId]=temperature;
     if (TemperatureMin[SensorId]>temperature) TemperatureMin[SensorId]=temperature;
@@ -540,7 +542,11 @@ int MillisecCalculateTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId)
     TemperatureSum[SensorId]-=TemperatureMax[SensorId];
     TemperatureSum[SensorId]-=TemperatureMin[SensorId];
     calc = TemperatureSum[SensorId] / (TemperatureCount[SensorId]-2);
-
+    if (TemperatureSum[SensorId]>10)
+    {
+        if(abs(TemperatureMax[SensorId]-TemperatureMin[SensorId])>2000)
+                Report("Millisec Temp spike",__FILE__,SensorId,(int)TemperatureMax[SensorId],RpWarning,(int) TemperatureMin[SensorId],0);
+    }
     TemperatureSum[SensorId] = 0;
     TemperatureCount[SensorId] = 0;
     TemperatureMin[SensorId] = 30000;
@@ -703,14 +709,14 @@ uint32_t MillisecLowLoop(uint32_t tick)
 
         Trigger_InputsReading();
 
-        #ifdef USE_RFID
+        #ifdef USE_RFID_LOGIC
             //every 100 m sec
             for(readerID = READER_1; readerID < Max_Readers ; readerID++)
             {
                 if(DiscoverRFIDTagEvery100mSec[readerID] == true)
                 {
                     RFIDCallEvery100mSec(readerID);
-                    break;
+                    //break;
                 }
             }
         #endif
@@ -821,12 +827,13 @@ uint32_t MillisecLowLoop(uint32_t tick)
         //call IFS state machine
         midTankStateMachine();
 
-        #ifdef USE_RFID
-                RFIDCycleEvery1Sec();
-        #endif
+
     }
     if (Tensecond_Tick)
     {
+	#ifdef USE_RFID_LOGIC
+	        RFIDCycleEvery1Sec();
+	#endif
     }
     if (OneMinute_Tick)
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index cfd4db0e6..11a48b587 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -545,23 +545,15 @@ void CopyTemperaturesToMonitor(void)
         headblowervoltage2 = HeadBlowersGetRPM(HEAD_FAN_LEFT);
         DiagnosticsMonitor.headblowervoltage1 = &headblowervoltage1;
         DiagnosticsMonitor.headblowervoltage2 = &headblowervoltage2;
-#warning machine to machine test
+//#warning machine to machine test
         DiagnosticsMonitor.headzone4temperature =
                 HeaterTemperature[HEATER_TYPE__HeaterZone4];
-        DiagnosticsMonitor.headzone6temperature =
-                HeaterTemperature[HEATER_TYPE__HeaterZone6];
-        DiagnosticsMonitor.headzone8temperature =
-                HeaterTemperature[HEATER_TYPE__HeaterZone8];
         DiagnosticsMonitor.headzone9temperature =
                 HeaterTemperature[HEATER_TYPE__HeaterZone9];
         DiagnosticsMonitor.headzone10temperature =
                 HeaterTemperature[HEATER_TYPE__HeaterZone10];
         DiagnosticsMonitor.n_headzone4temperature =
                 HeaterCounterIndex[HEATER_TYPE__HeaterZone4];
-        DiagnosticsMonitor.n_headzone6temperature =
-                HeaterCounterIndex[HEATER_TYPE__HeaterZone6];
-        DiagnosticsMonitor.n_headzone8temperature =
-                HeaterCounterIndex[HEATER_TYPE__HeaterZone8];
         DiagnosticsMonitor.n_headzone9temperature =
                 HeaterCounterIndex[HEATER_TYPE__HeaterZone9];
         DiagnosticsMonitor.n_headzone10temperature =
@@ -1013,8 +1005,6 @@ void DiagnosticOneSecCollection(void)
         DiagnosticLoadTemperature(HEATER_TYPE__HeaterZone12, MillisecGetTemperatures(HEAD_PT100_ZONE_12_0X8A_1));
 #warning MACHINE TO MACHINE TEST
         DiagnosticLoadTemperature(HEATER_TYPE__HeaterZone4, MillisecGetTemperatures(HEAD_PT100_ZONE_4_0X82_1));
-        DiagnosticLoadTemperature(HEATER_TYPE__HeaterZone6, MillisecGetTemperatures(HEAD_PT100_ZONE_6_0X84_1));
-        DiagnosticLoadTemperature(HEATER_TYPE__HeaterZone8, MillisecGetTemperatures(HEAD_PT100_ZONE_8_0X86_1));
         DiagnosticLoadTemperature(HEATER_TYPE__HeaterZone9, MillisecGetTemperatures(HEAD_PT100_ZONE_9_0X88_0));
         DiagnosticLoadTemperature(HEATER_TYPE__HeaterZone10, MillisecGetTemperatures(HEAD_PT100_ZONE_10_0X88_1));
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h
index 7ec1962f3..e0afad820 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h
@@ -54,6 +54,7 @@ uint32_t SetBlowerStateRequestFunc(MessageContainer* requestContainer);
 
 void DiaglosticChangeBlowerData();
 
+void HomingStopReporting(void);
 
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
index c7f8740d1..086525bc1 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -46,6 +46,16 @@ uint32_t Diagnostics_Dryer_UnLoading(void);
 uint32_t Diagnostics_Dryer_Loading(void);
 int kval_upper_value;
 
+void HomingStopReporting(void)
+{
+    int i;
+    for (i = 0; i< NUM_OF_MOTORS;i++)
+    {
+        HomingToken[i][0] == 0;
+    }
+    LOG_ERROR(0,"HomingStopReporting");
+}
+
 /********************************************************************************
  * Motor Homing
  ********************************************************************************/
@@ -475,7 +485,7 @@ uint32_t Diagnostics_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_
 
     angle = Calculate_Arm_Distance(D_DrierPrevLocation,temp);
 
-    if ((angle<14000  )||(BusyFlag == BUSY)) // OK - take another round
+    if ((abs (angle)<14000  )||(BusyFlag == BUSY)) // OK - take another round
     {
         responseContainer.has_error = true;
         responseContainer.error = ERROR_CODE__GENERAL_ERROR;
@@ -611,7 +621,7 @@ uint32_t Diagnostics_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue
     ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback",__FILE__,(int)angle,(int)ReadValue,RpMessage,temp,0);
 
     MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,D_numberOfCycles);
-    if ((angle>14000  )&&(ReadValue == NOTBUSY)) // OK - take another round
+    if ((abs(angle)>14000  )&&(ReadValue == NOTBUSY)) // OK - take another round
     {
         D_DrierPrevLocation = temp;
         //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index 193a35e12..6c4293f6f 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -446,12 +446,19 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest)
         LOG_ERROR (0, "Wrong Data Allocation");
         return ERROR;
     }
-    if (InitFailures > 3)
+    if (InitFailures > REPEATED_INIT_FAILURE_LIMIT)
     {
         Report("HWConfigurationInit repeated failure",__FILE__,__LINE__,(int)InitFailures,RpWarning,(int)EEPROM_INIT_FAILURE_COUNTER,0);
         AlarmHandlingSetAlarm (EVENT_TYPE__POWER_UP_BIT_FAILURE,ON);
         return ERROR;
     }
+    else
+    {
+        if (InitFailures > 2)
+        {
+            Report("HWConfigurationInit repeated failure going up",__FILE__,__LINE__,(int)InitFailures,RpWarning,(int)EEPROM_INIT_FAILURE_COUNTER,0);
+        }
+    }
 
     HardwareConfiguration *request =    UploadRequest->hardwareconfiguration;
 
@@ -628,7 +635,6 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest)
     return OK;
 }
 char Serial[21];
-MidTankData MidTankInfo[MAX_SYSTEM_DISPENSERS];
 
 uint32_t MachineCalibrationDataRequestFunc(MessageContainer* requestContainer)
 {
@@ -636,6 +642,7 @@ uint32_t MachineCalibrationDataRequestFunc(MessageContainer* requestContainer)
     MachineCalibrationDataRequest *request = machine_calibration_data_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
     uint32_t *DancersData = my_malloc(MAX_SYSTEM_DANCERS*sizeof(uint32_t));
     MidTankData ** MidTankDataCollect = my_malloc(sizeof(MidTankData *)*MAX_SYSTEM_DISPENSERS);
+    MidTankData MidTankInfo[MAX_SYSTEM_DISPENSERS];
 
     int i;
     for (i = 0;i<MAX_SYSTEM_DANCERS;i++)
@@ -655,7 +662,8 @@ uint32_t MachineCalibrationDataRequestFunc(MessageContainer* requestContainer)
     //MCU_E2PromSerialNumRead(&Serial);
 
     MachineCalibrationDataResponse response = MACHINE_CALIBRATION_DATA_RESPONSE__INIT;
-    machine_calibration_data__init(&response.machinecalibrationdata);
+    response.machinecalibrationdata = my_malloc(sizeof(MachineCalibrationData));
+    machine_calibration_data__init(response.machinecalibrationdata);
     response.machinecalibrationdata->n_dancermiddlepoint = MAX_SYSTEM_DANCERS;
     response.machinecalibrationdata->dancermiddlepoint = DancersData;
     response.machinecalibrationdata->machineserialnumber = Serial;
@@ -885,7 +893,9 @@ void FlashInitAndLoad(void)
     }
     Report("Hardware Initialized", __FILE__,__LINE__,Bytes, RpMessage, GENHWCFG_MAP_IN_FLASH, 0);
 
-    StoreDancerConfigMessage();
+    StoreDancerConfigMessage(0);
+    StoreDancerConfigMessage(1);
+    StoreDancerConfigMessage(2);
     Report("Dancer Initialized", __FILE__,DancersCfg[0].zeropoint,DancersCfg[1].zeropoint, RpMessage, DancersCfg[2].zeropoint, 0);
     /*Bytes = AlarmHandlingLoadFile();
     if (Bytes>0)
diff --git a/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.c b/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.c
index 4c248e94b..bf68b64f0 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.c
@@ -245,7 +245,7 @@ return OK;
  * */
 
 char WasteEmptyingToken[36+1] = {0};
-uint32_t CartridgeStateUpdate(CartridgeSlot Slot,CartridgeState State, double percentage)
+uint32_t CartridgeStateUpdate(CartridgeSlot Slot, int32_t index, CartridgeState State, double percentage)
 {
     if (WasteEmptyingToken[0] == 0)
         return ERROR;
@@ -271,6 +271,9 @@ uint32_t CartridgeStateUpdate(CartridgeSlot Slot,CartridgeState State, double pe
     CartData.has_slot = true;
     CartData.slot = Slot;
 
+    CartData.has_index = true;
+    CartData.index = index;
+
     /*
         struct  _CartridgeStatus
         {
@@ -327,9 +330,9 @@ uint32_t StartInkFillingStatusRequestFunc(MessageContainer* requestContainer)
     StartInkFillingStatusRequest* request = start_ink_filling_status_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
     ustrncpy (WasteEmptyingToken, requestContainer->token,36);
 
-    CartridgeStateUpdate(CARTRIDGE_SLOT__Ink,CARTRIDGE_STATE__None,0);
-    CartridgeStateUpdate(CARTRIDGE_SLOT__WasteMiddle,CARTRIDGE_STATE__None,0);
-    CartridgeStateUpdate(CARTRIDGE_SLOT__WasteLower,CARTRIDGE_STATE__None,0);
+    CartridgeStateUpdate(CARTRIDGE_SLOT__Ink,0, CARTRIDGE_STATE__None,0);
+    CartridgeStateUpdate(CARTRIDGE_SLOT__WasteMiddle, 0, CARTRIDGE_STATE__None,0);
+    CartridgeStateUpdate(CARTRIDGE_SLOT__WasteLower, 1, CARTRIDGE_STATE__None,0);
     start_ink_filling_status_request__free_unpacked(request,NULL);
     return status;
 //    case MESSAGE_TYPE__StartWasteEmptyingRequest:
diff --git a/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.h b/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.h
index ed3ec205d..a9539e0fa 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.h
+++ b/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.h
@@ -19,7 +19,7 @@ void SetMachineStatus (MachineState State);
 int MachineUpdateResponseFunc(void);
 uint32_t StartInkFillingStatusRequestFunc(MessageContainer* requestContainer);
 
-uint32_t CartridgeStateUpdate(CartridgeSlot Slot,CartridgeState State, double percentage);
+uint32_t CartridgeStateUpdate(CartridgeSlot Slot, int32_t index, CartridgeState State, double percentage);
 
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_Blowers.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_Blowers.c
index 021dd243f..946958a08 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_Blowers.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_Blowers.c
@@ -92,8 +92,8 @@ void HeadBlowersInit()
     HeadBlowerControl[0].m_params.MAX = 200;
     HeadBlowerControl[0].m_params.MIN = -200;
     HeadBlowerControl[0].m_params.Kd = 0;
-    HeadBlowerControl[0].m_params.Kp = 350;
-    HeadBlowerControl[0].m_params.Ki = 25;
+    HeadBlowerControl[0].m_params.Kp = 800;
+    HeadBlowerControl[0].m_params.Ki = 80;
     HeadBlowerControl[0].m_params.IntegralErrorMultiplier = 10;
     HeadBlowerControl[0].m_params.ProportionalErrorMultiplier = 10;
     HeadBlowerControl[0].m_params.epsilon = 0;
@@ -110,8 +110,8 @@ void HeadBlowersInit()
     HeadBlowerControl[1].m_params.MAX = 200;
     HeadBlowerControl[1].m_params.MIN = -200;
     HeadBlowerControl[1].m_params.Kd = 0;
-    HeadBlowerControl[1].m_params.Kp = 350;
-    HeadBlowerControl[1].m_params.Ki = 25;
+    HeadBlowerControl[1].m_params.Kp = 800;
+    HeadBlowerControl[1].m_params.Ki = 80;
     HeadBlowerControl[1].m_params.IntegralErrorMultiplier = 10;
     HeadBlowerControl[1].m_params.ProportionalErrorMultiplier = 10;
     HeadBlowerControl[1].m_params.epsilon = 0;
@@ -212,108 +212,148 @@ uint32_t HeadBlowersOffGet()
     ReportWithPackageFilter(HeatersFilter,"HeadBlowersOffGet",__FILE__,__LINE__,HeadBlowersEnable,RpError, 0,0);
     return HeadBlowersEnable;
 }
+static int8_t count1 = 0, count2 = 0, count3 = 0, count4 = 0, count5 = 0;
+static int8_t count6 = 0, count7 = 0, count8 = 0, count9 = 0, count10 = 0;
+void setArcHeadAlarms(uint32_t AlarmId, bool value)
+{
+    AlarmHandlingSetAlarm(AlarmId, value);
+    if (value == true)
+    {
+        switch(AlarmId)
+        {
+            /*case EVENT_TYPE__DYEING_HEAD_ARC_LID_IS_OPEN:
+                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count9,RpError, 0,0);
+                break;
+            case EVENT_TYPE__DYEING_HEAD_TUNNEL_LID_IS_OPEN:
+                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count9,RpError, 0,0);
+                break;
+            case EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_STOPPED:
+                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count9,RpError, 0,0);
+                break;
+            case EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_STOPPED:
+                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count9,RpError, 0,0);
+                break;*/
+            case EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_RPM_TOO_LOW:
+                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count3,RpError, HeadBlowersGetRPM(HEAD_FAN_RIGHT),0);
+                break;
+            case EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_RPM_TOO_LOW:
+                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count4,RpError, HeadBlowersGetRPM(HEAD_FAN_LEFT),0);
+                break;
+            case EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_HIGH:
+                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count5,RpError, PressureSensorGetPressure(HEAD_FAN_RIGHT),0);
+                break;
+            case EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_HIGH:
+                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count7,RpError, PressureSensorGetPressure(HEAD_FAN_LEFT),0);
+                break;
+            case EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_LOW:
+                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count6,RpError, PressureSensorGetPressure(HEAD_FAN_RIGHT),0);
+                break;
+            case EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_LOW:
+                ReportWithPackageFilter(HeatersFilter,"ArcHeadAlarms on",__FILE__,AlarmId,count8,RpError, PressureSensorGetPressure(HEAD_FAN_LEFT),0);
+                break;
+        }
+    }
+}
 
 void ArcHeadAlarms(void)
 {
     double currentFlow = 0.0;
-    static int8_t count1 = 0, count2 = 0, count3 = 0, count4 = 0, count5 = 0;
-    static int8_t count6 = 0, count7 = 0, count8 = 0, count9 = 0, count10 = 0;
 
     if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_ARC) != LIMIT) {
         if (++count9 == ARC_ALARM_LIMIT)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_ARC_LID_IS_OPEN, true);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_ARC_LID_IS_OPEN, true);
         count9 = (count9 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count9);
     } else {
         if (--count9 == 0)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_ARC_LID_IS_OPEN, false);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_ARC_LID_IS_OPEN, false);
         count9 = (count9 < 0)?(0):(count9);
     }
 
     if (FPGA_Read_limit_Switches(I2C_HEADCARD_COVER_LS_TUNNEL_ARC) != LIMIT) {
         if (++count10 == ARC_ALARM_LIMIT)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_TUNNEL_LID_IS_OPEN, true);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_TUNNEL_LID_IS_OPEN, true);
         count10 = (count10 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count10);
     } else {
         if (--count10 == 0)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_TUNNEL_LID_IS_OPEN, false);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_TUNNEL_LID_IS_OPEN, false);
         count10 = (count10 < 0)?(0):(count10);
     }
 
     if (Head_Fan_Tach[0] == 0x1FFE) {
         if (++count1 == ARC_ALARM_LIMIT)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_STOPPED, true);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_STOPPED, true);
         count1 = (count1 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count1);
     } else {
         if (--count1 == 0)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_STOPPED, false);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_STOPPED, false);
         count1 = (count1 < 0)?(0):(count1);
     }
     if (Head_Fan_Tach[1] == 0x1FFE) {
         if (++count2 == ARC_ALARM_LIMIT)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_STOPPED, true);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_STOPPED, true);
         count2 = (count2 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count2);
     } else {
         if (--count2 == 0)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_STOPPED, false);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_STOPPED, false);
         count2 = (count2 < 0)?(0):(count2);
     }
 
     if (HeadBlowersGetRPM(HEAD_FAN_RIGHT) < 2000) {
         if (++count3 == ARC_ALARM_LIMIT)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_RPM_TOO_LOW, true);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_RPM_TOO_LOW, true);
         count3 = (count3 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count3);
     } else {
         if (--count3 == 0)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_RPM_TOO_LOW, false);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FAN_RPM_TOO_LOW, false);
         count3 = (count3 < 0)?(0):(count3);
     }
     if (HeadBlowersGetRPM(HEAD_FAN_LEFT) < 2000) {
         if (++count4 == ARC_ALARM_LIMIT)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_RPM_TOO_LOW, true);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_RPM_TOO_LOW, true);
         count4 = (count4 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count4);
     } else {
         if (--count4 == 0)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_RPM_TOO_LOW, false);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FAN_RPM_TOO_LOW, false);
         count4 = (count4 < 0)?(0):(count4);
     }
 
     currentFlow = PressureSensorGetPressure(HEAD_FAN_RIGHT);
     if (currentFlow > 5.0) {
         if (++count5 == ARC_ALARM_LIMIT)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_HIGH, true);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_HIGH, true);
         count5 = (count5 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count5);
     } else {
         if (--count5 == 0)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_HIGH, false);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_HIGH, false);
         count5 = (count5 < 0)?(0):(count5);
     }
     if (currentFlow < 1.0) {
         if (++count6 == ARC_ALARM_LIMIT)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_LOW, true);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_LOW, true);
         count6 = (count6 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count6);
     } else {
         if (--count6 == 0)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_LOW, false);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_1_FLOW_TOO_LOW, false);
         count6 = (count6 < 0)?(0):(count6);
     }
 
     currentFlow = PressureSensorGetPressure(HEAD_FAN_LEFT);
     if (currentFlow > 5.0) {
         if (++count7 == ARC_ALARM_LIMIT)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_HIGH, true);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_HIGH, true);
         count7 = (count7 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count7);
     } else {
         if (--count7 == 0)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_HIGH, false);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_HIGH, false);
         count7 = (count7 < 0)?(0):(count7);
     }
     if (currentFlow < 1.0) {
         if (++count8 == ARC_ALARM_LIMIT)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_LOW, true);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_LOW, true);
         count8 = (count8 > ARC_ALARM_LIMIT)?(ARC_ALARM_LIMIT):(count8);
     } else {
         if (--count8 == 0)
-            AlarmHandlingSetAlarm(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_LOW, false);
+            setArcHeadAlarms(EVENT_TYPE__DYEING_HEAD_BLOWER_2_FLOW_TOO_LOW, false);
         count8 = (count8 < 0)?(0):(count8);
     }
 }
@@ -411,6 +451,7 @@ uint32_t  HeadBlowerCommandRequestMessage(int blowerId, float flow)
         ReportWithPackageFilter(HeatersFilter,"flow not in range",__FILE__, flow, blowerId,RpError, 0,0);
         return ERROR;
     }
+    ReportWithPackageFilter(HeatersFilter,"HeadBlower configured",__FILE__,blowerId,(int)(flow*100),RpError, PressureSensorV0[blowerId],0);
     headBlowersFlow[blowerId] = flow;
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index 84013add6..def9c5530 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -1053,7 +1053,7 @@ uint32_t HeaterMaxTempCBFunction(uint32_t IfIndex, uint32_t readValue)
     }
     if (abs(readValue - HeaterPreviousRead[index])>2000)
     {
-        ReportWithPackageFilter(HeatersFilter,"Temperature Spike",__FILE__,index,HeaterPreviousRead[index],RpWarning,readValue, index);
+        ReportWithPackageFilter(HeatersFilter,"Temperature Spike AC",__FILE__,index,HeaterPreviousRead[index],RpWarning,readValue, index);
 #ifdef IGNORE_SPIKE
         HeaterPreviousRead[index] = readValue;
 #else
@@ -1345,18 +1345,18 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
             DeActivateHeater(HEATER_TYPE__DryerMainHeater);
             HeaterRecalculateSharedHeatersParams(HEATER_TYPE__DryerMainHeater,0);
             HeaterRecalculateSharedHeatersParams(HEATER_TYPE__DryerSecondaryHeater,0);
-            if (HeaterSpikeRead[index] == 0)
-            {
-                HeaterSpikeRead[index] = readValue;
-            }
-            else
-            {
-                ReportWithPackageFilter(HeatersFilter,"restoring read Temperature Spike",__FILE__,index,HeaterPreviousRead[index],RpWarning,HeaterSpikeRead[index], index);
-                HeaterPreviousRead[index] = HeaterSpikeRead[index];
-                HeaterSpikeRead[index] = 0;
-            }
-
         }
+        if (HeaterSpikeRead[index] == 0)
+        {
+            HeaterSpikeRead[index] = readValue;
+        }
+        else
+        {
+            ReportWithPackageFilter(HeatersFilter,"restoring read Temperature Spike",__FILE__,index,HeaterPreviousRead[index],RpWarning,HeaterSpikeRead[index], index);
+            HeaterPreviousRead[index] = HeaterSpikeRead[index];
+            HeaterSpikeRead[index] = 0;
+        }
+
         return ERROR;
 #endif
     }
@@ -1580,20 +1580,20 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
 #ifdef IGNORE_SPIKE
         HeaterPreviousRead[index] = readValue;
 #else
-        //if (readValue > HeaterCmd[index].targettemperatue)
+        if (readValue > HeaterCmd[index].targettemperatue)
         {
             DeActivateHeater(index);
             HeaterRecalculateHeaterParams(index, 0);
-            if (HeaterSpikeRead[index] == 0)
-            {
-                HeaterSpikeRead[index] = readValue;
-            }
-            else
-            {
-                ReportWithPackageFilter(HeatersFilter,"restoring read Temperature Spike",__FILE__,index,HeaterPreviousRead[index],RpWarning,HeaterSpikeRead[index], index);
-                HeaterPreviousRead[index] = HeaterSpikeRead[index];
-                HeaterSpikeRead[index] = 0;
-            }
+        }
+        if (HeaterSpikeRead[index] == 0)
+        {
+            HeaterSpikeRead[index] = readValue;
+        }
+        else
+        {
+            ReportWithPackageFilter(HeatersFilter,"restoring read Temperature Spike",__FILE__,index,HeaterPreviousRead[index],RpWarning,HeaterSpikeRead[index], index);
+            HeaterPreviousRead[index] = HeaterSpikeRead[index];
+            HeaterSpikeRead[index] = 0;
         }
 #endif
     }
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
index d86d7ab2d..0f8108dad 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
@@ -5,9 +5,6 @@
 #include "drivers/motors/motor.h"
 #include "ids_ex.h"
 
-#define LUBRICANT_DISPENSER 7
-#define CLEANER_DISPENSER 6
-
 extern uint32_t DispenserPrepareSpeed ;
 extern double   DispenserPreparePressure ;
 extern uint32_t DispenserPrepareTimeout ;
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
index 0ae017c13..59b9a56be 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_Cleaning.c
@@ -107,6 +107,16 @@ uint32_t IDS_Cleaning_Move_Actuators()
    CleaningStageCounter++;
    return OK;
 }
+
+uint32_t IDS_Cleaning_stop_cleaner_motor()
+{
+    TimerMotors_t MotorId = HARDWARE_MOTOR_TYPE__MOTO_DH_LID;
+    if (Head_Type != HEAD_TYPE_ARC)
+       return OK;
+
+    MotorStop(MotorId,Soft_Stop);
+    return OK;
+}
 /*uint32_t IDS_Cleaning_Move_Rockers (int LeftRockerSpeed,int RightRockerSpeed)
 {
     uint32_t status = OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
index 57d3ff5e2..0a8060496 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
@@ -118,5 +118,6 @@ void DispenserDataRequestFunc(MessageContainer* requestContainer);
 void IDS_Start_Pid_Testing(int DispenserId);
 void IDS_Stop_Pid_Testing(int DispenserId);
 uint32_t cleaningMotorCBFunction (uint32_t deviceID, uint32_t Parameter1);
+uint32_t IDS_Cleaning_stop_cleaner_motor();
 
 #endif /* MODULES_IDS_IDS_EX_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index fcd1c05a2..a28d00a91 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -43,7 +43,6 @@ typedef struct
 }DispenserControlConfig_t;
 HardwarePidControl *DispensersControl;
 HardwarePidControl DispensersCtrl[MAX_SYSTEM_DISPENSERS];
-#define MAX_DYE_DISPENSERS 6
 #define IDS_PRESEGMENT_TIME_STEP 50
 #define INITIAL_CLEANER_SPEED 300
 
@@ -251,6 +250,7 @@ JobDescriptionFileBrushStop *FirstBrushStop = NULL;
          DispenserUsedInJob[Dispenser_i] = false;
      }
      n_segments = 0;
+#ifndef LIGHT_COLORS
      if (MotorsCfg[DispenserIdToMotorId[CLEANER_DISPENSER]].hardwaremotortype == DispenserIdToMotorId[CLEANER_DISPENSER])
      {
          DispenserUsedInJob[CLEANER_DISPENSER] = true;
@@ -261,7 +261,7 @@ JobDescriptionFileBrushStop *FirstBrushStop = NULL;
          DispenserUsedInJob[CLEANER_DISPENSER] = false;
          EnableCleaning = false;
      }
-
+#endif
      if (JobTicket->enablelubrication == true)
      {
          //DispenserUsedInJob[LUBRICANT_DISPENSER] = true;
@@ -522,7 +522,20 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue)
   char IdMessage[100];
   int SegmentPrepareWFCFCounter = 0;
   bool pressureReady = false,endOfPrepareWCF = false;
-
+int BuildIdsJobPrintString(char *dest,char *desc, int info)
+{
+    int len = 0,i;
+    int DispenserSpeed[MAX_SYSTEM_DISPENSERS] = {0,0,0,0,0,0,0,0};
+    if (dest == NULL)
+        return 0;
+    for (i = 0; i<MAX_SYSTEM_DISPENSERS; i++)
+    {
+        if (DispenserUsedInJob[i])
+            DispenserSpeed[i] = CurrentDispenserSpeed[i];
+    }
+    len = usnprintf(dest, 80,"%s %d K %d,C %d,M %d,Y %d,TI %d",desc,(int)info,DispenserSpeed[0],DispenserSpeed[1],DispenserSpeed[2],DispenserSpeed[3],DispenserSpeed[4]);
+  return len;
+}
 bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,double pressure)
 {
     TimerMotors_t HW_Motor_Id = DispenserIdToMotorId[DispenserId];
@@ -565,7 +578,7 @@ bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,doubl
      double segmentfirst_speed;
      double pressure;
      ModuleStateEnum result = ModuleDone;
-     bool DispenserReady[MAX_DYE_DISPENSERS] = {false,false,false,false,false,false};
+     bool DispenserReady[MAX_SYSTEM_DISPENSERS] = {false,false,false,false,false,false,false,false};
 
      //REPORT_MSG((int)DispenserPrepareControlId, "Prepare Callback");
 
@@ -591,10 +604,10 @@ bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,doubl
              {
                  DispenserTotalPrepareSteps[i]+=(CurrentDispenserSpeed[i]*PRESSURE_READ_TIME_GAP/eOneSecond);
                  HW_Motor_Id = DispenserIdToMotorId[i];
-                 if ((DispenserTotalPrepareSteps[i]>TargetNumberOfStepsPreRun)&&(DispenserTotalPrepareSteps[i]<(TargetNumberOfStepsPreRun+InitialDispenserSpeed)))
+                 /*if ((DispenserTotalPrepareSteps[i]>TargetNumberOfStepsPreRun)&&(DispenserTotalPrepareSteps[i]<(TargetNumberOfStepsPreRun+InitialDispenserSpeed)))
                  {
                      ReportWithPackageFilter(IDSFilter,"IDS dispenser pre-run ended",__FILE__,i,(int)DispenserTotalPrepareSteps[i],RpWarning,(int)(pressure*100),0);
-                 }
+                 }*/
                  if (DispenserTotalPrepareSteps[i]<TargetNumberOfStepsPreRun)
                  {
                      AdjustDispenserSpeedToPressure(i,MaximalPressurePreRun,pressure);
@@ -617,6 +630,7 @@ bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,doubl
              else
                  DispenserReady[i] = true;
          }
+#ifndef LIGHT_COLORS
          pressure = GetDispenserPressure(CLEANER_DISPENSER);
          if (pressure > 0.02)
          {
@@ -626,7 +640,7 @@ bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,doubl
                  ReportWithPackageFilter(IDSFilter,"IDS stop cleaner",__FILE__,CLEANER_DISPENSER,(int)(pressure*100),RpWarning,(int)CurrentDispenserSpeed[CLEANER_DISPENSER] ,0);
              CurrentDispenserSpeed[CLEANER_DISPENSER] = 0;
          }
-
+#endif
          if ((NumofReadyDispensers)&&(DispenserBuildTimeCounter%1000 == 0))
          {
              ReportWithPackageFilter(IDSFilter,"IDS_Prepare_Callback",__FILE__,DispenserBuildTimeCounter,NumofReadyDispensers,RpWarning,(int)NumOfActiveDispensers,0);
@@ -634,7 +648,9 @@ bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,doubl
          if (NumofReadyDispensers>=NumOfActiveDispensers)
          {
              pressureReady = true;
-             ReportWithPackageFilter(IDSFilter,"pressureReady = true;",__FILE__,DispenserBuildTimeCounter,NumofReadyDispensers,RpWarning,(int)NumOfActiveDispensers,0);
+             usnprintf(IdMessage, 80,"setting pressureReady = true %d K %d,C %d,M %d,Y %d,TI %d",(int)TargetNumberOfStepsPreRun,DispenserTotalPrepareSteps[0],DispenserTotalPrepareSteps[1],
+                       DispenserTotalPrepareSteps[2],DispenserTotalPrepareSteps[3],DispenserTotalPrepareSteps[4]);
+             ReportWithPackageFilter(IDSFilter,IdMessage,__FILE__,DispenserBuildTimeCounter,NumofReadyDispensers,RpWarning,(int)NumOfActiveDispensers,0);
          }
          if (pressureReady == true)
          {
@@ -736,9 +752,10 @@ bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,doubl
                              }
                          }
                      }//for
-                     usnprintf(IdMessage, 80,"WFCF %d K %d,C %d,M %d,Y %d,TI %d",WFCF,CurrentDispenserSpeed[0],
-                               CurrentDispenserSpeed[1],CurrentDispenserSpeed[2],CurrentDispenserSpeed[3],CurrentDispenserSpeed[4]);
-                     ReportWithPackageFilter(IDSFilter,IdMessage, __FILE__, __LINE__, DispenserBuildTimeCounter, RpWarning,   WFCF, 0);
+                     //usnprintf(IdMessage, 80,"WFCF %d K %d,C %d,M %d,Y %d,TI %d",WFCF,CurrentDispenserSpeed[0],
+                     //          CurrentDispenserSpeed[1],CurrentDispenserSpeed[2],CurrentDispenserSpeed[3],CurrentDispenserSpeed[4]);
+                     if (BuildIdsJobPrintString(IdMessage,"WFCF ",WFCF))
+                             ReportWithPackageFilter(IDSFilter,IdMessage, __FILE__, __LINE__, DispenserBuildTimeCounter, RpWarning,   WFCF, 0);
 
                  }//if n_dis
              }
@@ -760,6 +777,7 @@ bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,doubl
      //start new stage
      if (endOfPrepareWCF == true)
      {
+#ifndef LIGHT_COLORS
          if (CurrentDispenserSpeed[CLEANER_DISPENSER])
          {
              HW_Motor_Id = DispenserIdToMotorId[CLEANER_DISPENSER];
@@ -767,6 +785,7 @@ bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,doubl
              CurrentDispenserSpeed[CLEANER_DISPENSER] = 0;
              ReportWithPackageFilter(IDSFilter,"IDS stop cleaner",__FILE__,CLEANER_DISPENSER,(int)(pressure*100),RpWarning,(int)CurrentDispenserSpeed[CLEANER_DISPENSER] ,0);
          }
+#endif
          SafeRemoveControlCallback(DispenserPrepareControlId, IDS_Prepare_Callback );
          DispenserPrepareControlId = 0xFF;
          ReportWithPackageFilter(IDSFilter,"IDS_Prepare_Callback SafeRemoveControlCallback",__FILE__,DispenserBuildTimeCounter,NumofReadyDispensers,RpWarning,(int)NumOfActiveDispensers,0);
@@ -884,6 +903,7 @@ bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,doubl
          }
          else
          {
+             DispenserTotalPrepareSteps[i] = 0;
              if (AutoHoming_Config >= AutoHoming_JobEnd_PowerOn_off)
              {
                  if ((HomingActive[i] == false)&&(PrimingActive[i] == false))
@@ -940,11 +960,12 @@ bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,doubl
                  }*/
              }
          }
+#ifndef LIGHT_COLORS
          TimerMotors_t  HW_Motor_Id = DispenserIdToMotorId[CLEANER_DISPENSER];
          MotorSetSpeed((TimerMotors_t)HW_Motor_Id,INITIAL_CLEANER_SPEED ); //set the dispenser to the
          CurrentDispenserSpeed[CLEANER_DISPENSER] = INITIAL_CLEANER_SPEED;
          ReportWithPackageFilter(IDSFilter,"IDS start cleaner",__FILE__,CLEANER_DISPENSER,(int)(GetDispenserPressure(CLEANER_DISPENSER)*100),RpWarning,(int)CurrentDispenserSpeed[CLEANER_DISPENSER] ,0);
-
+#endif
 
      }
      DispenserBuildTimeCounter = 0;
@@ -1087,9 +1108,10 @@ bool AdjustDispenserSpeedToPressure(int DispenserId, double RefMaxPressure,doubl
                      }
                  }
              }//for
-             usnprintf(IdMessage, 80,"Presegment Prepare K %d,C %d,M %d,Y %d,TI %d",CurrentDispenserSpeed[0],
-                       CurrentDispenserSpeed[1],CurrentDispenserSpeed[2],CurrentDispenserSpeed[3],CurrentDispenserSpeed[4]);
-             ReportWithPackageFilter(IDSFilter,IdMessage, __FILE__, __LINE__, Dispenser_i, RpWarning,   segmentfirst_speed, 0);
+             //usnprintf(IdMessage, 80,"Presegment Prepare K %d,C %d,M %d,Y %d,TI %d",CurrentDispenserSpeed[0],
+             //          CurrentDispenserSpeed[1],CurrentDispenserSpeed[2],CurrentDispenserSpeed[3],CurrentDispenserSpeed[4]);
+             if (BuildIdsJobPrintString(IdMessage,"Presegment Prepare ",0))
+                 ReportWithPackageFilter(IDSFilter,IdMessage, __FILE__, __LINE__, Dispenser_i, RpWarning,   segmentfirst_speed, 0);
 
              DispenserBuildTimeCounter = 0;
              //DispenserPrepareControlId = AddControlCallback( IDS_PreSegmentPrepare_Callback, IDS_PRESEGMENT_TIME_STEP,TemplateDataReadCBFunction ,0, 0, 0 );
@@ -1175,8 +1197,10 @@ uint32_t IDS_Cleaning_Stop_Cleaning_Solution (callback_fptr callback);
          ReportWithPackageFilter(IDSFilter,"End of Pre-segment Handling",__FILE__,__LINE__,InterSegmentStepsCount,RpWarning,(int)lInterSegmentLength,0);
          //PreSegmentReady(Module_IDS,ModuleDone);
          SafeRemoveControlCallback(DispenserPreSegmentControlId,IDSPreSegmentStateCallbackRunner);
+#ifndef LIGHT_COLORS
          if (CurrentDispenserSpeed[CLEANER_DISPENSER]>0)
              IDS_Cleaning_Stop_Cleaning_Solution (NULL);
+#endif
          setRapidPressureRead(false);
          //if (JobEndSequence == true)
          //    DistanceToSpoolReady(Module_IDS,ModuleDone);
@@ -1289,9 +1313,10 @@ uint32_t IDS_Cleaning_Stop_Cleaning_Solution (callback_fptr callback);
                          //SendJobProgress(0.0, 0, false, IdsMessage);*/
                      }
                  }//for
-                 usnprintf(IdMessage, 80,"Presegment WFCF %d K %d,C %d,M %d,Y %d,TI %d",WFCF,CurrentDispenserSpeed[0],
-                           CurrentDispenserSpeed[1],CurrentDispenserSpeed[2],CurrentDispenserSpeed[3],CurrentDispenserSpeed[4]);
-                 ReportWithPackageFilter(IDSFilter,IdMessage, __FILE__, __LINE__, FileBrushStop->index, RpWarning,   lInterSegmentLength, 0);
+                 //usnprintf(IdMessage, 80,"Presegment WFCF %d K %d,C %d,M %d,Y %d,TI %d",WFCF,CurrentDispenserSpeed[0],
+                 //          CurrentDispenserSpeed[1],CurrentDispenserSpeed[2],CurrentDispenserSpeed[3],CurrentDispenserSpeed[4]);
+                 if (BuildIdsJobPrintString(IdMessage,"Presegment WFCF ",WFCF))
+                     ReportWithPackageFilter(IDSFilter,IdMessage, __FILE__, __LINE__, FileBrushStop->index, RpWarning,   lInterSegmentLength, 0);
              }
          }
          //startDispensersAtSegmentSpeed*1=WFCFClenerSpray(speed);
@@ -1616,9 +1641,10 @@ void IDS_StartBrushStop(int n_dispensers,  JobDispenser** Dispensers)
             }
         } //for
     }
-    usnprintf(IdMessage, 80,"IDS_StartBrushStop %d/%d K %d,C %d,M %d,Y %d,TI %d",FileBrushStop->index,SegmentNumOfBrushStops,CurrentDispenserSpeed[0],
-              CurrentDispenserSpeed[1],CurrentDispenserSpeed[2],CurrentDispenserSpeed[3],CurrentDispenserSpeed[4]);
-    ReportWithPackageFilter(IDSFilter,IdMessage, __FILE__, SegmentNumOfBrushStops, FileBrushStop->index, RpWarning,(int)FileBrushStop->offsetmeters, 0);
+    //usnprintf(IdMessage, 80,"IDS_StartBrushStop %d/%d K %d,C %d,M %d,Y %d,TI %d",FileBrushStop->index,SegmentNumOfBrushStops,CurrentDispenserSpeed[0],
+    //          CurrentDispenserSpeed[1],CurrentDispenserSpeed[2],CurrentDispenserSpeed[3],CurrentDispenserSpeed[4]);
+    if (BuildIdsJobPrintString(IdMessage,"IDS_StartBrushStop ",FileBrushStop->index))
+        ReportWithPackageFilter(IDSFilter,IdMessage, __FILE__, SegmentNumOfBrushStops, FileBrushStop->index, RpWarning,(int)FileBrushStop->offsetmeters, 0);
 
 }
 uint32_t IDSBrushStopRestartCallback(uint32_t IfIndex, uint32_t readValue)
@@ -1717,8 +1743,10 @@ uint32_t IDSSegmentState(void *SegmentDetails, int SegmentId)
 #else
      Valve_Set(VALVE_MIXCHIP_WASTECH, Mixer_Head);
 #endif
+#ifndef LIGHT_COLORS
      if (CurrentDispenserSpeed[CLEANER_DISPENSER]>0)
          IDS_Cleaning_Stop_Cleaning_Solution(NULL);
+#endif
      SegmentNumOfBrushStops = Segment->n_brushstops;
      BrushStopTime = Segment->length*1000/(double)SegmentNumOfBrushStops;  //brushstop in meters //brushstop in millisecond
      BrushStopTime = ((BrushStopTime*100)/dyeingspeed);//brushstop in seconds
@@ -1935,27 +1963,38 @@ uint32_t IDSSegmentState(void *SegmentDetails, int SegmentId)
          }
 
      }
-	IDS_StopLubrication();
+#ifndef LIGHT_COLORS
+     if (CurrentDispenserSpeed[CLEANER_DISPENSER]>0)
+         IDS_Cleaning_Stop_Cleaning_Solution(NULL);
+#endif
+     IDS_StopLubrication();
 	Init_CleaningStageCounter();//set the counter before relocating
+	IDS_Cleaning_stop_cleaner_motor();
 	HeadCard_Actuators_Relocate();
 	return OK;
 }
 uint32_t IDS_StartLubrication(void)
 {
+#ifdef LIGHT_COLORS
+#else
     IDS_Dispenser_Start_Motor_and_Open_Valve(LUBRICANT_DISPENSER,lubricant_speed,NULL);
     CurrentDispenserSpeed[LUBRICANT_DISPENSER] = lubricant_speed;
-    REPORT_MSG (lubricant_speed, "IDS_StartLubrication");
     Lubricant_2Way_Valve (START);
+#endif
+    REPORT_MSG (lubricant_speed, "IDS_StartLubrication");
     return OK;
 }
 uint32_t IDS_StopLubrication(void)
 {
     REPORT_MSG (lubricant_speed, "IDS_StopLubrication and home dispenser");
+#ifdef LIGHT_COLORS
+#else
     Lubricant_2Way_Valve (STOP);
     //IDS_Dispenser_Close_Valve_And_Stop_Motor(LUBRICANT_DISPENSER,IDS_Valve_EndValveReady);
     TimerMotors_t  HW_Motor_Id = DispenserIdToMotorId[LUBRICANT_DISPENSER];
     MotorStop(HW_Motor_Id,Hard_Hiz);
     CurrentDispenserSpeed[LUBRICANT_DISPENSER] = 0;
     //IDS_HomeDispenser (LUBRICANT_DISPENSER, 1000 , NULL);   do not fill lubricant between jobs
+#endif
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
index 329357ed4..469e82aa8 100644
--- a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
+++ b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
@@ -157,9 +157,20 @@ bool CartridgeInkFull()
 int GetCartridgeFillPercent()
 {
     double MidTank_Pressure = 0;
-
+#ifdef IFS_DEBUG
+    static int percent = 0;
+    if (midTankState == MidTankStateFilling) {
+        if (percent < 100) {
+            percent += 1;
+        }
+    } else {
+        percent = 0;
+    }
+    return percent;
+#else
     MidTank_Pressure = Read_MidTank_Pressure_Sensor(midTankCartColor);
     return (((MidTank_Pressure - midTankCapacity)* 100) / CARTRIDGE_CAPATICY);
+#endif
 }
 
 bool midTankCart_door()
@@ -284,7 +295,6 @@ void midTankStateMachine(void)
                     break;
                 }
                 //check RFID - check cartridge OK
-                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, CARTRIDGE_STATE__Inserted, 0);
 
                 // send request for validation to ppc
                 inkAuthenticationPass = 2; //not defined
@@ -306,17 +316,18 @@ void midTankStateMachine(void)
                 Report("MidTank Cartridge authentication wait...", __FILE__, __LINE__, authenticationTimeOutCounter, RpMessage, 0, 0);
                 if (authenticationTimeOutCounter > 60) {
                     midTankState = MidTankStateIdle;
-                    CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, CARTRIDGE_STATE__Error, 0);
+                    CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, midTankCartColor, CARTRIDGE_STATE__Error, 0);
                 }
                 break;
             }
             if (inkAuthenticationPass == false) {
                 Report("MidTank Cartridge authentication fail", __FILE__, __LINE__, 0, RpMessage, 0, 0);
-                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, CARTRIDGE_STATE__Error, 0);
+                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, midTankCartColor, CARTRIDGE_STATE__Error, 0);
                 midTankState = MidTankStateIdle;
                 break;
             }
             // Cartridge status received from user - assume cartridge is full
+            CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, midTankCartColor, CARTRIDGE_STATE__Inserted, 0);
 
             midTankCapacity = Get_MidTank_Pressure_Sensor(midTankCartColor);
             if ((midTankCapacity > MidTankEmptyLimit)&&(midTankCartColor!=MIDTANK_8))//enable lubricant filling in two stages
@@ -325,13 +336,13 @@ void midTankStateMachine(void)
                 cart1.color = fastBILNK;
                 Report("Mid-tank not empty", __FILE__, __LINE__, midTankCapacity, RpMessage, MidTankEmptyLimit, 0);
                 midTankState = MidTankStateIdle;
-                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, CARTRIDGE_STATE__Error, 0);
+                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, midTankCartColor, CARTRIDGE_STATE__Error, 0);
                 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;
-            CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, CARTRIDGE_STATE__Filling, 0);
+            CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, midTankCartColor, CARTRIDGE_STATE__Filling, 0);
 
             if (midTankCartColor == MIDTANK_8)
             {
@@ -356,7 +367,7 @@ void midTankStateMachine(void)
                 Report("Cartridge extracted during filling!!!", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
                 Report("Mid-tank # filling fail", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
                 midTankState = MidTankStateIdle;
-                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, CARTRIDGE_STATE__Absent, GetCartridgeFillPercent());
+                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, midTankCartColor, CARTRIDGE_STATE__Absent, GetCartridgeFillPercent());
             }
             // if time out or emptying done - idle
             if (midTankTimeoutCounter++ > CartridgeInkTimeout) {
@@ -366,7 +377,7 @@ void midTankStateMachine(void)
                 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);
-                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, CARTRIDGE_STATE__Error, GetCartridgeFillPercent());
+                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, midTankCartColor, CARTRIDGE_STATE__Error, GetCartridgeFillPercent());
                 midTankState = MidTankStateIdle;
             } else if (CartridgeInkFull()) {
                 midTankTimeoutCounter = 0;
@@ -376,9 +387,10 @@ void midTankStateMachine(void)
                 Report("Mid-tank # filling done", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
                 midTankState = MidTankStateIdle;
                 // RFID change status - inkEmpty
-                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, CARTRIDGE_STATE__FillingCompleted, 100.0);
+                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, midTankCartColor, CARTRIDGE_STATE__FillingCompleted, 100.0);
             }
-            CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, CARTRIDGE_STATE__Filling, GetCartridgeFillPercent());
+            if (midTankState == MidTankStateFilling)
+                CartridgeStateUpdate(CARTRIDGE_SLOT__Ink, midTankCartColor, CARTRIDGE_STATE__Filling, GetCartridgeFillPercent());
             break;
         default:
             Report("wrong state", __FILE__, __LINE__, midTankState, RpMessage, 0, 0);
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Calculate.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Calculate.c
index e386df88c..d26b3d814 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Calculate.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Calculate.c
@@ -5,22 +5,32 @@
 #include <PMR/Stubs/CalculateResponse.pb-c.h>
 #include "include.h"
 #include "driverlib/gpio.h"
+#include "modules/control/control.h"
 
 #include "Drivers/USB_Communication/USBCDCD.h"
 
-
-void Stub_CalculateRequest(MessageContainer* requestContainer)
+char CalculateToken[36+1] = {0};
+//char init_string[100];
+int aaa = 0;
+uint32_t CalculateControlId = 0xFF;
+uint32_t CalculateRpeat(uint32_t IfIndex, uint32_t BusyFlag)
 {
     MessageContainer responseContainer;
-
-    CalculateRequest* request = calculate_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
-
-
     CalculateResponse response = CALCULATE_RESPONSE__INIT;
-    response.sum = request->a + request->b;
+    bool last = false;
+    response.sum = aaa++;
     response.has_sum = true;
+    if (aaa >=20)
+    {
+        RemoveControlCallback(CalculateControlId, CalculateRpeat);
+        last = true;
+        aaa=0;
+    }
+    Report("CalculateRpeat",__FILE__,__LINE__,(int)aaa,RpWarning,(int)0,0);
 
-    responseContainer = createContainer(MESSAGE_TYPE__CalculateResponse, requestContainer->token, true, &response, &calculate_response__pack, &calculate_response__get_packed_size);
+    responseContainer = createContainer(MESSAGE_TYPE__CalculateResponse, CalculateToken, last, &response, &calculate_response__pack, &calculate_response__get_packed_size);
+    responseContainer.has_continuous = true;
+    responseContainer.continuous = true;
 
 //    free(request);
 
@@ -30,5 +40,21 @@ void Stub_CalculateRequest(MessageContainer* requestContainer)
     SendChars((char*)container_buffer, container_size);
     //free(container_buffer);
     //free(requestContainer);
+
+    return OK;
+}
+
+
+void Stub_CalculateRequest(MessageContainer* requestContainer)
+{
+    MessageContainer responseContainer;
+
+    CalculateRequest* request = calculate_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+    ustrncpy (CalculateToken, requestContainer->token,36);
+
+
+    Report("Stub_CalculateRequest",__FILE__,__LINE__,(int)aaa,RpWarning,(int)0,0);
+    CalculateControlId = AddControlCallback("PowerUpLoad", CalculateRpeat, eOneSecond, TemplateDataReadCBFunction,0,0, 0 );
+
     calculate_request__free_unpacked(request,NULL);
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
index 0ce6474cc..284a43e41 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
@@ -1246,7 +1246,7 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
         response.has_progress = true;
     }
     else
-    if(request->amount == 0xB24)  //Set loading arm cycles
+    if(request->amount == 0xB24)  //AttemptThreadJoggingFunc
     {
         REPORT_MSG(request->delay,"AttemptThreadJoggingFunc");
         AttemptThreadJoggingFunc(NULL);
@@ -1254,7 +1254,7 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
         response.has_progress = true;
     }
     else
-    if(request->amount == 0xB25)  //Set loading arm cycles
+    if(request->amount == 0xB25)  //Change jog speed
     {
         REPORT_MSG(request->delay,"Change jog speed");
         if (request->delay<100)
@@ -1263,6 +1263,14 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
         response.has_progress = true;
     }
     else
+    if(request->amount == 0xB26)  //Change sublimation time
+    {
+        REPORT_MSG(request->delay,"Change sublimation time");
+        setSublimationBlowTime(request->delay);
+        response.progress = JogSpeed;
+        response.has_progress = true;
+    }
+    else
     if(request->amount == 0xC3)  //suspend I2C task
     {
         if (request->delay == 0)
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c
index 63c7b6963..f4b1f7463 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Cartridge.c
@@ -9,21 +9,23 @@
 #include "include.h"
 #include "inc/hw_uart.h"
 #include "driverlib/gpio.h"
-
-#include "Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h"
 #include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
+#include "Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h"
+
+#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
 #include "Drivers/I2C_Communication/RFID_NFC/NFC.h"
 #include <Utilities/RfidTagHandling.h>
-#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
+
 #include <Drivers/I2C_Communication/I2C.h>
 #include <Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.h>
+#include <Drivers/I2C_Communication/I2C_RFID_Dispenser_Task/I2C_RFID_Disp_Task.h>
 
 
 #include "Stub_Status.h"
 
 void Stub_CartridgeReadRequest(MessageContainer* requestContainer)
 {
-    //uint32_t status = PASSED;
+    uint32_t status = PASSED;
     MessageContainer responseContainer;
 
     static bool first_time = true;
@@ -38,26 +40,69 @@ void Stub_CartridgeReadRequest(MessageContainer* requestContainer)
 
     uint8_t readerID = request->cartridgeid >> 8;
 
-    /*if(request->cartridgeid == 0x0100)
+    if(request->cartridgeid == 0x0100)
     {
-        Init_RFID_Reader(readerID);//0-2
+        Init_RFID_Reader(readerID);//0-2 - done in main.c. by Init_IFS no need to use it
     }
     else if(request->cartridgeid == 0x0101)
     {
-        Connect_to_RFID_Reder(readerID);//0-2 - conect the card and get tag id
+        Connect_to_RFID_Reder(readerID);//0-2 - conect the card and get tag id - while(1)
     }
     else if(request->cartridgeid == 0x0102)
     {
+        /*
         Active_RFID_Reader = readerID + 1;
-        if(first_time == true)
-        {
-            TagStage =  Tag_PrepareForDiscovery;
-            first_time = false;
-        }
+//        if(first_time == true)
+//        {
+//            TagStage =  Tag_PrepareForDiscovery;
+//            first_time = false;
+//        }
+        TagStage[readerID] =  Tag_PrepareForDiscovery;
+
+        RFIDTagComm_StateMachine(readerID, Auto);
+        */
+        Trigger_RFID_TestTask(readerID);
 
-        RFIDTagComm_StateMachine(Auto);
     }
-    */
+    else if(request->cartridgeid == 0x0103)//first time before 0x0102 - no need done in 102
+    {
+        Active_RFID_Reader = readerID + 1;
+        TagStage[readerID] =  Tag_PrepareForDiscovery;
+
+    }
+    else if((request->cartridgeid & 0xFF)== 0xC1)
+    {
+        Trigger_RFID_UpdateInkFill(readerID);
+    }
+    else if((request->cartridgeid & 0xFF)== 0xC2)
+    {
+        Trigger_RFID_UpdateInkUsed(readerID);
+    }
+    else if((request->cartridgeid & 0xFF)== 0xC3)
+    {
+        Trigger_RFID_UpdateInkEmpty(readerID);
+    }
+    else if((request->cartridgeid & 0xFF)== 0xC4)
+    {
+        Trigger_RFID_UpdateInkMachineID(readerID);
+    }
+    else if((request->cartridgeid & 0xFF)== 0xE1)
+    {
+        Trigger_RFID_UpdateWasteFill(readerID);
+    }
+    else if((request->cartridgeid & 0xFF)== 0xE2)
+    {
+        Trigger_RFID_UpdateWasteFull(readerID);
+    }
+    else if((request->cartridgeid & 0xFF)== 0xE3)
+    {
+        Trigger_RFID_UpdateWasteEmpty(readerID);
+    }
+    else if((request->cartridgeid & 0xFF)== 0xE4)
+    {
+        Trigger_RFID_UpdateWasteMachineID(readerID);
+    }
+
     response.cartridge = &CartridgeRead;
     CartridgeRead.has_index = false;
     CartridgeRead.has_slot = true;
@@ -82,7 +127,7 @@ void Stub_CartridgeReadRequest(MessageContainer* requestContainer)
 
 void Stub_CartridgeWriteRequest(MessageContainer* requestContainer)
 {
-    //uint32_t status = NOT_SUPPORTED;
+    uint32_t status = NOT_SUPPORTED;
     MessageContainer responseContainer;
 
     StubCartridgeWriteRequest* request = stub_cartridge_write_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Dancer.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Dancer.c
index 3d2db071e..27a7dfb31 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Dancer.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Dancer.c
@@ -70,6 +70,7 @@ void Stub_DancerPositionRequest(MessageContainer* requestContainer)
         response.has_general_status = true;
         response.detailed_status = DANCER_ENC[request->dancer_id].Det_status;
         response.has_general_status = true;
+        StoreDancerConfigMessage(Dancer_Id);
     }
     else if(Dancer_Id < NUM_OF_ROTENC*2)
     {
@@ -86,8 +87,6 @@ void Stub_DancerPositionRequest(MessageContainer* requestContainer)
         response.detailed_status = ERROR; // use the Reserved bits to send our errors
         response.has_general_status = true;
     }
-    if (Dancer_Id == 2)
-        StoreDancerConfigMessage();
     responseContainer = createContainer(MESSAGE_TYPE__StubDancerPositionResponse, requestContainer->token, true, &response, &stub_dancer_position_response__pack, &stub_dancer_position_response__get_packed_size);
 
     //free(request);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index cdb726b5e..c3f15aff8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -258,7 +258,7 @@
         {
             CallbackCounter--;
         }
-        if ((angle>14000  )&&(BusyFlag == NOTBUSY)) // OK - take another round
+        if ((abs(angle)>14000  )&&(BusyFlag == NOTBUSY)) // OK - take another round
         {
             Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0);
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 07ecf5d0b..11a92ab63 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -46,7 +46,7 @@ uint32_t ThreadEndState();
 
 bool Set_Thread_Rockers_Bypass (int value);
 
-uint32_t StoreDancerConfigMessage(void);
+uint32_t StoreDancerConfigMessage(int DancerId);
 uint32_t LoadDancerConfigMessage(void);
 
 //uint32_t MotorPidRequestMessage(HardwarePidControl* request);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 1920db03a..869485b71 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -188,40 +188,36 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request)
 }
 char DancerConfigPath[50] = "0://SysInfo//DancCfg.cfg";
 
-uint32_t StoreDancerConfigMessage()
+uint32_t StoreDancerConfigMessage(int DancerId)
 {
     uint32_t status = OK;
   //  FRESULT Fresult = FR_OK;
     //HardwareConfiguration DancerConfig;
     HardwareDancer Dancers[MAX_SYSTEM_DANCERS];
-  //  uint8_t* response_buffer;
+#ifdef FOUR_WINDERS
+    uint8_t DancerAddress[MAX_SYSTEM_DANCERS] = {EEPROM_STORAGE_DANCER_0,EEPROM_STORAGE_DANCER_1,EEPROM_STORAGE_DANCER_2,EEPROM_STORAGE_DANCER_3,EEPROM_STORAGE_DANCER_4};
+#else
+    uint8_t DancerAddress[MAX_SYSTEM_DANCERS] = {EEPROM_STORAGE_DANCER_0,EEPROM_STORAGE_DANCER_1,EEPROM_STORAGE_DANCER_2};
+#endif
   //  size_t response_size = 0;
-    int Dancer_i;
+    int Dancer_i = DancerId;
 
     //hardware_configuration__init(&DancerConfig);
     //DancerConfig.dancers = (HardwareDancer**)my_malloc(sizeof(HardwareDancer*)*MAX_SYSTEM_DANCERS);
-    for (Dancer_i = 0; Dancer_i < MAX_SYSTEM_DANCERS; Dancer_i++)
-    {
-        hardware_dancer__init(&Dancers[Dancer_i]);
-        //DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i];
-        Dancers[Dancer_i].has_zeropoint = true;
-        Dancers[Dancer_i].hardwaredancertype = Dancer_i;
-        Dancers[Dancer_i].has_hardwaredancertype = true;
-        Dancers[Dancer_i].zeropoint=Control_Read_Dancer_Position(Dancer_i,0);
-        //DancerConfig.n_dancers++;
-        DancersCfg[Dancer_i].zeropoint = Dancers[Dancer_i].zeropoint;
-        DancersCfg[Dancer_i].zeropoint = Control_Read_Dancer_Position(Dancer_i,0);
-    }
+    hardware_dancer__init(&Dancers[Dancer_i]);
+    //DancerConfig.dancers[Dancer_i] = &Dancers[Dancer_i];
+    Dancers[Dancer_i].has_zeropoint = true;
+    Dancers[Dancer_i].hardwaredancertype = Dancer_i;
+    Dancers[Dancer_i].has_hardwaredancertype = true;
+    Dancers[Dancer_i].zeropoint=Control_Read_Dancer_Position(Dancer_i,0);
+    //DancerConfig.n_dancers++;
+    DancersCfg[Dancer_i].zeropoint = Dancers[Dancer_i].zeropoint;
+    DancersCfg[Dancer_i].zeropoint = Control_Read_Dancer_Position(Dancer_i,0);
 
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint);
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_1,DancersCfg[1].zeropoint);
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_2,DancersCfg[2].zeropoint);
-    Report("~~~~~~Store eeprom 0",__FILE__,DancersCfg[0].zeropoint,(int)DancersCfg[1].zeropoint,RpWarning,(int)DancersCfg[2].zeropoint,0);
-#ifdef FOUR_WINDERS
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_3,DancersCfg[HARDWARE_DANCER_3].zeropoint);
-    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_4,DancersCfg[HARDWARE_DANCER_4].zeropoint);
-    Report("~~~~~~Store eeprom 1",__FILE__,__LINE__,(int)DancersCfg[HARDWARE_DANCER_3].zeropoint,RpWarning,(int)DancersCfg[HARDWARE_DANCER_4].zeropoint,0);
-#endif
+
+
+    status |= MCU_E2PromProgram(DancerAddress[Dancer_i],DancersCfg[Dancer_i].zeropoint);
+    Report("~~~~~~Store eeprom dancer",__FILE__,__LINE__,Dancer_i,RpWarning,(int)DancersCfg[Dancer_i].zeropoint,0);
     /*
     response_buffer = my_malloc(hardware_configuration__get_packed_size(&DancerConfig));
     if (response_buffer)
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 8d41f3b9a..5fee72dc7 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -1338,22 +1338,22 @@ uint32_t UpdatePidDuringRun(HardwarePidControl *request)
         return ERROR;
 
 
-    if (request->derivativetime == true)
+    if (request->has_derivativetime == true)
     {
         MotorControlConfig[Motor_i].m_params.Kd = request->derivativetime;
         ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kd",__FILE__,Motor_i,(int)(request->derivativetime),RpWarning,0,0);
     }
-    if (request->proportionalgain == true)
+    if (request->has_proportionalgain == true)
     {
         MotorControlConfig[Motor_i].m_params.Kp = request->proportionalgain;
         ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kp",__FILE__,Motor_i,(int)(request->proportionalgain),RpWarning,0,0);
     }
-    if (request->integraltime == true)
+    if (request->has_integraltime == true)
     {
         MotorControlConfig[Motor_i].m_params.Ki = request->integraltime;
         ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Ki",__FILE__,Motor_i,(int)(request->integraltime),RpWarning,0,0);
     }
-    if (request->epsilon == true)
+    if (request->has_epsilon == true)
     {
         MotorControlConfig[Motor_i].m_params.epsilon = request->epsilon;
         ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun epsilon",__FILE__,Motor_i,(int)(request->epsilon*10000),RpWarning,0,0);
diff --git a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
index 9c5fe9822..490a0c0a5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
+++ b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
@@ -8,8 +8,9 @@
 #include "Modules/Waste/Waste_ex.h"
 #include "modules/heaters/heaters_ex.h"
 #include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
-#include "Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h"
 #include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
+#include "Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h"
+
 #include "Drivers/I2C_Communication/RFID_NFC/NFC.h"
 #include <Utilities/RfidTagHandling.h>
 #include "PMR/IFS/CartridgeState.pb-c.h"
@@ -27,6 +28,7 @@
 #include <Drivers/I2C_Communication/I2C_Task.h>
 #include <Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/WHS_MAX11614_A2D.h>
 #include <Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.h>
+#include <Drivers/I2C_Communication/RFID_NFC/RFID_Waste.h>
 #include <Drivers/I2C_Communication/WHS_Card/D_AD5272_Rheostat/WHS_Rheostat.h>
 
 uint8_t VocPpmAlarmLimit = 10;//PPM
@@ -42,6 +44,7 @@ uint32_t Cartridge_Cover_Control;
 button *CartLed[MaxWasteCartridges] = {0, &cart2, &cart3};
 WasteTankStateEnum WasteTankState = WasteTankStateIdle;
 
+extern char WasteEmptyingToken[36+1];
 int init = 1;
 
 bool setWasteValve (WasteCartridgeEnum WasteCartridge);
@@ -165,7 +168,7 @@ WasteCartridgeEnum cartGetActiveCart()
 {
     uint32_t CartId;
 
-    for (CartId = 0; CartId < MaxWasteCartridges; CartId++) {
+    for (CartId = WasteCartridge_middle; CartId < MaxWasteCartridges; CartId++) {
         if (CartState[CartId] == CartridgeStateACTIVE)
             return CartId;
     }
@@ -223,10 +226,13 @@ bool setWasteValve (WasteCartridgeEnum WasteCartridge)
 bool RdWasteTankEmptySensor()
 {
     //double WasteLevel = 0.0;
-
+#ifndef WHS_DEBUG
     if (WHS_Type == WHS_TYPE_UNKNOWN) {
         return WHS_GPI_WCONTAINER_WARN();
-    } else { //new WHS
+    }
+    else
+#endif
+    { //new WHS
         //WasteLevel = GetWHSWasteTankLevelMiliLiter();
         if (wasteLevel < wasteLevelEmpty)
             return 1; //Empty
@@ -237,10 +243,13 @@ bool RdWasteTankEmptySensor()
 bool RdWasteTankFullSensor()
 {
     //double WasteLevel = 0.0;
-
+#ifndef WHS_DEBUG
     if (WHS_Type == WHS_TYPE_UNKNOWN) {
         return WHS_GPI_WCONTAINER_FULL();
-    } else { //new WHS
+    }
+    else
+#endif
+    { //new WHS
         //WasteLevel = GetWHSWasteTankLevelMiliLiter();
         if (wasteLevel >= wasteLevelFull)
             return 1; //full
@@ -251,10 +260,13 @@ bool RdWasteTankFullSensor()
 bool RdWasteTankOverFlowSensor()
 {
     //double WasteLevel = 0.0;
-
+#ifndef WHS_DEBUG
     if (WHS_Type == WHS_TYPE_UNKNOWN) {
         return WHS_GPI_WASTE_OVERFULL();
-    } else {//new WHS
+    }
+    else
+#endif
+    {//new WHS
         //WasteLevel = GetWHSWasteTankLevelMiliLiter();
         if (wasteLevel > wasteLevelOverflow)
             return 1; //overflow
@@ -348,13 +360,14 @@ uint32_t Waste_DoorOpenDuringEmptying(uint32_t IfIndex, uint32_t ReadValue)
 void Waste_StateMachine_OneSecond_Call(void)
 {
     static WasteTankStateEnum prev_state = 0;
-    static int prev_wasteLevel = 0;
+    static int cart, prev_wasteLevel = 0;
+    static int insertUpdate[3] = {0, 0, 0};
 
     //call cartridge state
     if ((Is_Cartridge_Present(CART_2) != cartGetPresence(WasteCartridge_middle)) || (init)) {
         if (Is_Cartridge_Present(CART_2)) {
             cartCART_INSERTED(WasteCartridge_middle);
-            CartridgeStateUpdate(CARTRIDGE_SLOT__WasteMiddle, CARTRIDGE_STATE__Inserted, 0);
+            insertUpdate[CARTRIDGE_SLOT__WasteMiddle-1] = 1;
         } else {
             cartCART_EXTRACTED(WasteCartridge_middle);
         }
@@ -362,12 +375,20 @@ void Waste_StateMachine_OneSecond_Call(void)
     if ((Is_Cartridge_Present(CART_3) != cartGetPresence(WasteCartridge_lower)) || (init)) {
         if (Is_Cartridge_Present(CART_3)) {
             cartCART_INSERTED(WasteCartridge_lower);
-            CartridgeStateUpdate(CARTRIDGE_SLOT__WasteLower, CARTRIDGE_STATE__Inserted, 0);
+            insertUpdate[CARTRIDGE_SLOT__WasteLower-1] = 1;
         } else {
             cartCART_EXTRACTED(WasteCartridge_lower);
         }
     }
 
+    //update ppc
+    for (cart = WasteCartridge_middle; cart <= WasteCartridge_lower; cart++) {
+        if ((insertUpdate[cart]) && (WasteEmptyingToken[0])) {
+            CartridgeStateUpdate(cart, cart-1, CARTRIDGE_STATE__Inserted, 0);
+            insertUpdate[cart] = 0;
+        }
+    }
+
     //call door state
     cartCart_door();
 
@@ -467,7 +488,7 @@ void Waste_StateMachine(void)
             //start  emptying
             // RFID change status - set wasteFilling
             WasteTankState = WasteTankStateEmptying;
-            CartridgeStateUpdate(activeCart, CARTRIDGE_STATE__Emptying, 0);
+            CartridgeStateUpdate(activeCart, activeCart-1, CARTRIDGE_STATE__Emptying, 0);
             break;
         case WasteTankStateEmptying:
             // if time out or emptying done - idle
@@ -478,7 +499,7 @@ void Waste_StateMachine(void)
                 WasteTankState = WasteTankStateIdle;
                 usnprintf(str, 100, "WasteTankStateEmptying Timeout. time:%d:%d volume:%d", timeout_counter/60, timeout_counter%60, WHS_GetCartridgeFill());
                 Report(str, __FILE__, __LINE__, 0, RpMessage, 0, 0);
-                CartridgeStateUpdate(activeCart, CARTRIDGE_STATE__Error, WHS_GetCartridgeFillPercent());
+                CartridgeStateUpdate(activeCart, activeCart-1, CARTRIDGE_STATE__Error, WHS_GetCartridgeFillPercent());
                 AlarmHandlingSetAlarm( EVENT_TYPE__WASTE_CONTAINER_EMPTYING_TIMEOUT,  true);
                 RemoveControlCallback(Cartridge_Cover_Control, Waste_DoorOpenDuringEmptying);
                 timeout_counter = 0;
@@ -493,10 +514,11 @@ void Waste_StateMachine(void)
                     AlarmHandlingSetAlarm( EVENT_TYPE__ALL_WASTE_CARTRIDGES_FULL,  true);
                 usnprintf(str, 100, "WasteTankStateEmptying done. time:%d:%d volume:%d", timeout_counter/60, timeout_counter%60, WHS_GetCartridgeFill());
                 Report(str, __FILE__, __LINE__, 0, RpMessage, 0, 0);
-                CartridgeStateUpdate(activeCart, CARTRIDGE_STATE__EmptyingCompleted, WHS_GetCartridgeFillPercent());
+                CartridgeStateUpdate(activeCart, activeCart-1, CARTRIDGE_STATE__EmptyingCompleted, WHS_GetCartridgeFillPercent());
                 timeout_counter = 0;
             }
-            CartridgeStateUpdate(activeCart, CARTRIDGE_STATE__Emptying, WHS_GetCartridgeFillPercent());
+            if (WasteTankState == WasteTankStateEmptying)
+                CartridgeStateUpdate(activeCart, activeCart-1, CARTRIDGE_STATE__Emptying, WHS_GetCartridgeFillPercent());
 #ifdef WHS_DEBUG
             wasteLevel -= 100;
             timeout_counter += 10;
@@ -517,7 +539,7 @@ void Waste_StateMachine(void)
                     AlarmHandlingSetAlarm( EVENT_TYPE__ALL_WASTE_CARTRIDGES_FULL,  true);
                 Report("WasteTankStatePaused done", __FILE__, __LINE__, 0, RpMessage, 0, 0);
                 timeout_counter = 0;
-                CartridgeStateUpdate(activeCart, CARTRIDGE_STATE__Error, WHS_GetCartridgeFillPercent());
+                CartridgeStateUpdate(activeCart, activeCart-1, CARTRIDGE_STATE__Error, WHS_GetCartridgeFillPercent());
             }
             break;
         default:
diff --git a/Software/Embedded_SW/Embedded/Software Release Notes.txt b/Software/Embedded_SW/Embedded/Software Release Notes.txt
index 4e1f7104f..9b3b4a919 100644
--- a/Software/Embedded_SW/Embedded/Software Release Notes.txt
+++ b/Software/Embedded_SW/Embedded/Software Release Notes.txt
@@ -1,3 +1,30 @@
+Embedded SW Release note - Version 1.5.2(3) - Pack 3
+=============================================================
+tweaks to the loading arm reset function (thread load, PPC request)
+main card EEPROM read fixed
+Fans - support for new component (arc head and whs)
+light colors preparation - suppressed
+fix cleaning stop on job abort
+Dancer middle point EEPROM support writing single dancer
+Change spikes handling (fix taking the different temperature after 2 seconds
+change and fix some arc head blowers alarm and logs
+
+
+Embedded SW Release note - Version 1.5.2(1) - Pack 3
+=============================================================
+Rebuild Flash file system on Initialization failure.
+Protobuf - Procedure request, Read all hardware request, AttemptThreadJoggingRequest.
+Align pressure reading setup with electric scripts.
+fix alarm handling bugs (#4085, #4097), fix job failures report.
+add diagnostic temperature meters to diagnostic data (zone 4,9,10).
+add stub to control jog speed.
+fix Change Pid During Job stub handling.
+IFS and waste cartridge requests.
+Blowers V0 initialization and handling fixed and improved.
+Dancer resetting - enable for single dancer
+support initial BTSR tests
+Store cleaning parameters from the last job for cleaning jobs (#4112)
+
 Embedded SW Release note - Version 1.5.1(4) - Pack 3
 =============================================================
 fix tunnel pressure calculation
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Initialization/InitSequence.c b/Software/Embedded_SW/Embedded/StateMachines/Initialization/InitSequence.c
index 98326a1da..4e6a4b112 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Initialization/InitSequence.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Initialization/InitSequence.c
@@ -112,12 +112,12 @@ uint32_t InitSequenceCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
 
     MCU_E2PromProgram(EEPROM_INIT_FAILURE_COUNTER,InitFailures+1);
 
-    if (InitFailures > 6)
+    if (InitFailures >= REPEATED_INIT_FAILURE_LIMIT+3)
     {
         InitFailures = 0;
         MCU_E2PromProgram(EEPROM_INIT_FAILURE_COUNTER,0);
     }
-    if (InitFailures > 3)
+    if (InitFailures > REPEATED_INIT_FAILURE_LIMIT)
     {
         Report("HWConfigurationInit repeated failure",__FILE__,__LINE__,(int)InitFailures,RpWarning,(int)EEPROM_INIT_FAILURE_COUNTER,0);
         SetMachineState(MACHINE_STATE_HW_CONFIG_FAILED);
@@ -917,6 +917,7 @@ uint32_t PowerUpUpdate(MACHINE_STATE_STAGES_ENUM stage)
         {
             RemoveControlCallback(PowerUpLoadControlId, PowerUpUpdateCallBackFunction);
             PowerUpLoadControlId = 0xFF;
+            PowerUpStopReporting();
         }
     }
     return status;
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Initialization/PowerOffSequence.c b/Software/Embedded_SW/Embedded/StateMachines/Initialization/PowerOffSequence.c
index 3a0c94e27..b5ebdee17 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Initialization/PowerOffSequence.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Initialization/PowerOffSequence.c
@@ -275,9 +275,8 @@ uint32_t PowerOffHeatersOff(void)
     if (HandleProcessParameters(&ProcessParametersClear,false)!= OK)
     {
         LOG_ERROR (PowerOffMachineState, "Turn Off Heaters  failed");
-        //return ERROR;
     }
-
+    HeadBlowersOff(1);
     PowerOffMachineState++;
 
     return OK;
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Initialization/PowerOffSequence.h b/Software/Embedded_SW/Embedded/StateMachines/Initialization/PowerOffSequence.h
index 10dd4c71e..d7352492a 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Initialization/PowerOffSequence.h
+++ b/Software/Embedded_SW/Embedded/StateMachines/Initialization/PowerOffSequence.h
@@ -39,6 +39,7 @@ void setPowerOffDisableCleaning (bool value);
 
 uint32_t PowerDownUpdateFunc(MessageContainer* requestContainer);
 uint32_t AbortPowerDownFunc(MessageContainer* requestContainer);
+void PowerDownStopReporting(void);
 
 
 #endif /* STATEMACHINES_INITIALIZATION_POWEROFFSEQUENCE_H_ */
diff --git a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
index 82601a162..a2a0d6f67 100644
--- a/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
+++ b/Software/Embedded_SW/Embedded/StateMachines/Printing/JobSTM.c
@@ -105,7 +105,7 @@ JobSegment *TSegment;
 JobBrushStop *TbrushStop;
 JobDispenser *Tdispenser;
 JobSpool *Tspool;
-HeadCleaningParameters *CleaningParameters;
+HeadCleaningParameters *CleaningParameters = NULL;
 bool CopyConfigured[MAX_SYSTEM_MODULES];
 bool CleaningJobActive = false, JoggingJobActive = false;
 char ErrorMsg[100];
@@ -596,10 +596,9 @@ uint32_t ThreadCleaningJob(int speed)
                 TSegment = my_malloc(sizeof(JobSegment));
                 Tspool = my_malloc(sizeof(JobSpool));
                 TbrushStop = my_malloc(sizeof(JobBrushStop));
-                CleaningParameters = my_malloc(sizeof(CleaningParameters));
-                CleaningParameters->has_cleanerflow = true;
-                CleaningParameters->cleanerflow = 800;
                 Ticket.headcleaningparameters = CleaningParameters;
+                if (Ticket.headcleaningparameters)
+                    Report("Clean job cleaning parameters", __FILE__, __LINE__, Ticket.headcleaningparameters->cleanerflow, RpWarning, Ticket.headcleaningparameters->archeadcleaningmotorspeed, 0);
                 TSegment->length = 10.0;
                 Ticket.length += (TSegment->length*n_segments);
                 TSegment->n_brushstops = 1;
@@ -649,8 +648,8 @@ void FreeCleaningJobData(void *JobDetails)
 {
     JobTicket* JobTicket = JobDetails;
 
-    if (JobTicket->headcleaningparameters)
-        free (JobTicket->headcleaningparameters);
+    //if (JobTicket->headcleaningparameters)
+    //    free (JobTicket->headcleaningparameters);
     if (JobTicket->spool)
     {
         free (JobTicket->spool);
@@ -962,6 +961,18 @@ void JobRequestFunc(MessageContainer* requestContainer)
                             usnprintf(ErrorMsg, 100, "spool parameters error");
                         }
                     }
+                    if (Ticket->headcleaningparameters)
+                    {
+                        ///store last updated cleaning parameters
+                        if (CleaningParameters == NULL)
+                            CleaningParameters = my_malloc(sizeof(CleaningParameters));
+                        CleaningParameters->has_cleanerflow = Ticket->headcleaningparameters->has_cleanerflow;
+                        CleaningParameters->cleanerflow = Ticket->headcleaningparameters->cleanerflow;
+                        CleaningParameters->has_archeadcleaningmotorspeed = Ticket->headcleaningparameters->has_archeadcleaningmotorspeed;
+                        CleaningParameters->archeadcleaningmotorspeed = Ticket->headcleaningparameters->archeadcleaningmotorspeed;
+                        Report("Save job cleaning parameters", __FILE__, __LINE__, CleaningParameters->cleanerflow, RpWarning, CleaningParameters->archeadcleaningmotorspeed, 0);
+
+                    }
 
                 }
                 else