Update RFID code + Use new task for RFID and dispensers

34 files changed, 2850 insertions, 157 deletions

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 d0193f970..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,1};
+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/RFIDTagHandling.c b/Software/Embedded_SW/Embedded/Common/Utilities/RFIDTagHandling.c
index 6d15afc6a..6465903fa 100644
--- a/Software/Embedded_SW/Embedded/Common/Utilities/RFIDTagHandling.c
+++ b/Software/Embedded_SW/Embedded/Common/Utilities/RFIDTagHandling.c
@@ -5,6 +5,8 @@
  *      Author: shlomo
  */
 #include "include.h"
+
+#include "Modules/Waste/Waste.h"
 #include "Modules/IFS/ifs.h"
 #include "Modules/AlarmHandling/AlarmHandling.h"
 #include "PMR/Diagnostics/CartridgeValidationRequest.pb-c.h"
diff --git a/Software/Embedded_SW/Embedded/Communication/Connection.c b/Software/Embedded_SW/Embedded/Communication/Connection.c
index af60541aa..72108eeea 100644
--- a/Software/Embedded_SW/Embedded/Communication/Connection.c
+++ b/Software/Embedded_SW/Embedded/Communication/Connection.c
@@ -56,6 +56,21 @@ void StopRecurringReports(void)
 
     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 +98,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;
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 0d93b3dbe..3cd3969be 100644
--- a/Software/Embedded_SW/Embedded/DataDef.h
+++ b/Software/Embedded_SW/Embedded/DataDef.h
@@ -29,6 +29,10 @@
 #define BTSR_NO_FEEDER_TFU
 #define BTSR_NO_PULLER_TFU
 #endif
+
+#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
@@ -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
@@ -74,11 +78,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 +309,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 +341,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,
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/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
index fc4bad71d..2053ac569 100644
--- 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
@@ -1,8 +1,10 @@
 /*
- * RFID_Task.c
+ * I2C_RFID_Disp_Task.c
  *
  *  Created on: Oct 23, 2019
  *      Author: shlomo
+ *
+ * Task for I2C4_BASE
  */
 
 #include <stdint.h>
@@ -10,36 +12,434 @@
 #include "include.h"
 #include "inc/hw_memmap.h"
 
-#include <Drivers/I2C_Communication/I2C_RFID_Dispenser_Task/I2C_RFID_Disp_Task.h>
-Task_Handle RFID_Task_Handle;
+#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;
-    PANEL_BUTTON_OR_CRAT_ID CartId;
+    uint32_t TargetID; //readerID / dispenserId
+    uint32_t parameter1;
 }RFID_ReadingMessageStruc;
 
-void RFID_Task(UArg arg0, UArg arg1)
+void Trigger_RFID_Init(/*RFID_READER_ID readerID*/)
 {
     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;
+
+    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:
-                    break;
+                    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 = 11;
+    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
index 6d16701fc..5be55ce11 100644
--- 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
@@ -1,5 +1,5 @@
 /*
- * RFID_Task.h
+ * I2C_RFID_Disp_Task.h
  *
  *  Created on: 2 Nov 2020
  *      Author: avi
@@ -9,7 +9,32 @@
 #define DRIVERS_RFID_TASK_RFID_TASK_H_
 
 void Trigger_RFID_Init();
-void Trigger_Dispenser_ReadData(int dispenserId);;
+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);
 
 
 
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/Embedded.cfg b/Software/Embedded_SW/Embedded/Embedded.cfg
index 7fdf21819..1fef48fd7 100644
--- a/Software/Embedded_SW/Embedded/Embedded.cfg
+++ b/Software/Embedded_SW/Embedded/Embedded.cfg
@@ -174,13 +174,13 @@ 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;
 Program.global.millisecLow = Task.create("&RFID_Task", RFID_TaskParams);
-
+*/
 var ADCProcessTaskParams = new Task.Params();
 ADCProcessTaskParams.instance.name = "adcProcess";
 ADCProcessTaskParams.stackSize = 512;
diff --git a/Software/Embedded_SW/Embedded/Main.c b/Software/Embedded_SW/Embedded/Main.c
index 36883df23..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>
 //*****************************************************************************
 //
@@ -305,10 +307,10 @@ 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
@@ -319,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)
    {
@@ -330,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/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 1057ddbc3..5f6cdf1c6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -703,14 +703,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 +821,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/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/Waste/Waste_maint.c b/Software/Embedded_SW/Embedded/Modules/Waste/Waste_maint.c
index c1b54b4d6..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