/*############################################################################################*/
/*############################################################################################*/
/**
 * \file    RFIDManager.c
 * \author  Thibaut LALUQUE 3D-Ouest
 * \date    25/11/2014
 * \version 1.0
 * \brief 	Source file for RFID management.
 * \details All Right Reserved.
 */
/*############################################################################################*/
/*############################################################################################*/


/*############################################################################################*/
/*## INCLUDES ################################################################################*/
/*############################################################################################*/
#include "RFIDManager.h"
/*############################################################################################*/
/*## END INCLUDES ############################################################################*/
/*############################################################################################*/


/*############################################################################################*/
/*## LOCAL DEFINES ###########################################################################*/
/*############################################################################################*/

/*############################################################################################*/
/*## END LOCAL DEFINES ######################################################################*/
/*############################################################################################*/


/*############################################################################################*/
/*## LOCAL FONCTIONS DECLARATION #############################################################*/
/*############################################################################################*/
void RFIDDealer (void);
void RFIDIntLpmStateMachine(void);
void RFIDExtLpmStateMachine(void);
void RFIDBridgeStateMachine (void);
void RFIDReadStateMachine (void);
void RFIDWriteStateMachine (void);
/*############################################################################################*/
/*## END LOCAL FONCTIONS DECLARATION #########################################################*/
/*############################################################################################*/


/*############################################################################################*/
/*## LOCAL VARIABLES #########################################################################*/
/*############################################################################################*/
tRFIDPowerMode RFIDInternalPowerMode = RFID_POWERED;
tRFIDPowerMode RFIDExternalPowerMode = RFID_POWERED;
/*############################################################################################*/
/*## END LOCAL VARIABLES #####################################################################*/
/*############################################################################################*/

/*############################################################################################*/
/*## GLOBAL VARIABLES ########################################################################*/
/*############################################################################################*/
tRFIDCtrl RFIDCtrl;

tSRDEeprom SRDEeprom;
/*############################################################################################*/
/*## END GLOBAL VARIABLES ####################################################################*/
/*############################################################################################*/


/*############################################################################################*/
/*## LOCAL FONCTIONS #########################################################################*/
/*############################################################################################*/

/*############################################################################################*/
/*## END LOCAL FONCTIONS #####################################################################*/
/*############################################################################################*/


/*############################################################################################*/
/*## EXTERNAL FONCTIONS ######################################################################*/
/*############################################################################################*/
void RFIDManagerInit (void) {

	CR95HFInit();
	RFIDInternalPowerMode = RFID_POWERED;
	RFIDExternalPowerMode = RFID_POWERED;

	RFIDCtrl.state = RFID_STATE_STANDBY;

	RFIDCtrl.lpm = RFID_WORKING;

	RFIDCtrl.msRef = 0;

}

void RFIDManager (void) {

	/*=== Wait for RFID ready ===*/
	if(RFIDCtrl.state == RFID_STATE_STANDBY) {

		/*=== If no request for the moment ===*/
		if(RFIDCtrl.request.all == 0) {

			if(RFIDCtrl.msRef == 0) {

				/*-- Try to put all device in LPM --*/
				if(RFIDInternalPowerMode == RFID_POWERED) {
					RFIDCtrl.state = RFID_STATE_LOW_POWER_INT;
					RFIDCtrl.subState = RFID_State_IntLpm_Start;
					//RFIDCtrl.result = RFID_RESULT_OK;

				} else if (CR95ExtGetConnectionState() == CR95_EXT_CONNECTED) {
					if(RFIDExternalPowerMode == RFID_POWERED) {
						RFIDCtrl.state = RFID_STATE_LOW_POWER_EXT;
						RFIDCtrl.subState = RFID_State_ExtLpm_Start;
						//RFIDCtrl.result = RFID_RESULT_OK;
					}
				} else {
					RFIDExternalPowerMode = RFID_HIBERNATE;
				}

				if(RFIDExternalPowerMode == RFID_HIBERNATE && RFIDInternalPowerMode == RFID_HIBERNATE) {
					RFIDCtrl.lpm = RFID_READY_FOR_LPM;
				}

			} else {

				if(TickMilliSecElapsedFrom(RFIDCtrl.msRef) > 1000) {
					RFIDCtrl.msRef = 0;
				}

			}

		/*=== Deal request ===*/
		} else {

			RFIDCtrl.lpm = RFID_WORKING;

			if(RFIDCtrl.request.bits.read) {
				RFIDCtrl.state = RFID_STATE_READ;
				RFIDCtrl.subState = RFID_State_Read_Start;
				RFIDCtrl.result = RFID_RESULT_OK;
			}

			else if(RFIDCtrl.request.bits.write) {
				RFIDCtrl.state = RFID_STATE_WRITE;
				RFIDCtrl.subState = RFID_State_Write_Start;
				RFIDCtrl.result = RFID_RESULT_OK;
			}

			else if(RFIDCtrl.request.bits.bridge) {
				RFIDCtrl.state = RFID_STATE_BRIDGE;
				RFIDCtrl.subState = RFID_State_Bridge_Start;
				RFIDCtrl.result = RFID_RESULT_OK;
			}
		}
	} else {
		RFIDCtrl.lpm = RFID_WORKING;
	}

	RFIDDealer();

	/*=== Deal CR95 Manager ===*/
	CR95HFManager();
}


void RFIDDealer (void) {

	switch(RFIDCtrl.state) {

	case RFID_STATE_STANDBY :
		//Wait state -> Do nothing
		break;

	case RFID_STATE_BRIDGE :
		RFIDBridgeStateMachine();
		break;

	case RFID_STATE_READ :
		RFIDReadStateMachine();
		break;

	case RFID_STATE_WRITE :
		RFIDWriteStateMachine();
		break;

	case RFID_STATE_LOW_POWER_INT :
		RFIDIntLpmStateMachine();
		break;

	case RFID_STATE_LOW_POWER_EXT :
		RFIDExtLpmStateMachine();
		break;

	default :
		RFIDCtrl.request.all = 0;
		RFIDCtrl.state = RFID_STATE_STANDBY;
		break;

	}
}

void RFIDIntLpmStateMachine(void) {

	switch(RFIDCtrl.subState) {

	case RFID_State_IntLpm_Start :
		CR95SelectDevice(CR95_SELECT_INTERNAL);
		RFIDCtrl.subState = RFID_State_IntLpm_Echo;

	case RFID_State_IntLpm_Echo :
		CR95HFStartEcho();
		RFIDCtrl.subState = RFID_State_IntLpm_WaitEcho;
		break;

	case RFID_State_IntLpm_WaitEcho :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_IntLpm_FieldOff;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_ECHO_ERROR) {
				RFIDCtrl.subState = RFID_State_IntLpm_FieldOff;
				//RFIDCtrl.result = RFID_RESULT_INTLPM_ERROR;
			}
		}
		break;

	case RFID_State_IntLpm_FieldOff :
		CR95HFStartFieldOff();
		RFIDCtrl.subState = RFID_State_IntLpm_WaitFieldOff;
		break;

	case RFID_State_IntLpm_WaitFieldOff :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_IntLpm_Hibernate;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_FIELDOFF_ERROR) {
				RFIDCtrl.subState = RFID_State_IntLpm_Hibernate;
				//RFIDCtrl.result = RFID_RESULT_INTLPM_ERROR;
			}
		}
		break;

	case RFID_State_IntLpm_Hibernate :
		CR95HFStartHibernate();
		RFIDCtrl.subState = RFID_State_IntLpm_WaitHibernate;
		break;

	case RFID_State_IntLpm_WaitHibernate :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_IntLpm_Finish;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_HIBERNATE_ERROR) {
				RFIDCtrl.subState = RFID_State_IntLpm_Finish;
				//RFIDCtrl.result = RFID_RESULT_INTLPM_ERROR;
			}
		}
		break;

	case RFID_State_IntLpm_Finish :
		RFIDInternalPowerMode = RFID_HIBERNATE;
		RFIDCtrl.state = RFID_STATE_STANDBY;
		break;

	default :
		RFIDCtrl.subState = RFID_State_IntLpm_Start;
		break;
	}
}

void RFIDExtLpmStateMachine(void) {

	switch(RFIDCtrl.subState) {

	case RFID_State_ExtLpm_Start :
		CR95SelectDevice(CR95_SELECT_EXTERNAL);
		RFIDCtrl.subState = RFID_State_ExtLpm_Echo;

	case RFID_State_ExtLpm_Echo :
		CR95HFStartEcho();
		RFIDCtrl.subState = RFID_State_ExtLpm_WaitEcho;
		break;

	case RFID_State_ExtLpm_WaitEcho :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK ) {
				RFIDCtrl.subState = RFID_State_ExtLpm_FieldOff;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_ECHO_ERROR) {
				RFIDCtrl.subState = RFID_State_ExtLpm_FieldOff;
				//RFIDCtrl.result = RFID_RESULT_EXTLPM_ERROR;
			}
		}
		break;

	case RFID_State_ExtLpm_FieldOff :
		CR95HFStartFieldOff();
		RFIDCtrl.subState = RFID_State_ExtLpm_WaitFieldOff;
		break;

	case RFID_State_ExtLpm_WaitFieldOff :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK ) {
				RFIDCtrl.subState = RFID_State_ExtLpm_Hibernate;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_FIELDOFF_ERROR) {
				RFIDCtrl.subState = RFID_State_ExtLpm_Hibernate;
				//RFIDCtrl.result = RFID_RESULT_EXTLPM_ERROR;
			}
		}
		break;

	case RFID_State_ExtLpm_Hibernate :
		CR95HFStartHibernate();
		RFIDCtrl.subState = RFID_State_ExtLpm_WaitHibernate;
		break;

	case RFID_State_ExtLpm_WaitHibernate :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_ExtLpm_Finish;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_HIBERNATE_ERROR) {
				RFIDCtrl.subState = RFID_State_ExtLpm_Finish;
				//RFIDCtrl.result = RFID_RESULT_EXTLPM_ERROR;
			}
		}
		break;

	case RFID_State_ExtLpm_Finish :
		RFIDExternalPowerMode = RFID_HIBERNATE;
		RFIDCtrl.state = RFID_STATE_STANDBY;
		break;

	default :
		RFIDCtrl.subState = RFID_State_ExtLpm_Start;
		break;
	}
}

void RFIDBridgeStateMachine(void) {
	static unsigned long msRef = 0;

	switch(RFIDCtrl.subState) {

	case RFID_State_Bridge_Start :
		RFIDCtrl.subState = RFID_State_Bridge_Antenna;

	case RFID_State_Bridge_Antenna :
		CR95SelectDevice(CR95_SELECT_EXTERNAL);
		CR95UartDisableReceiver();
		char data[1] = {0x55};
		CR95UartSendData(data,1);	//Try to wake up external
		msRef = TickGetMsCounter();
		RFIDCtrl.subState = RFID_State_Bridge_WaitAntenna;
		break;

	case RFID_State_Bridge_WaitAntenna :
		if(TickMilliSecElapsedFrom(msRef)>10) {
			if(CR95ExtGetConnectionState() == CR95_EXT_CONNECTED) {
				CR95SelectDevice(CR95_SELECT_EXTERNAL);
				RFIDExternalPowerMode = RFID_POWERED;
			} else {
				CR95SelectDevice(CR95_SELECT_INTERNAL);
				RFIDInternalPowerMode = RFID_POWERED;
			}
			RFIDCtrl.subState = RFID_State_Bridge_Echo;
		}
		break;


	case RFID_State_Bridge_Echo :
		CR95HFStartEcho();
		RFIDCtrl.subState = RFID_State_Bridge_WaitEcho;
		break;

	case RFID_State_Bridge_WaitEcho :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK ) {
				RFIDCtrl.subState = RFID_State_Bridge_FieldOn;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_ECHO_ERROR) {
				RFIDCtrl.subState = RFID_State_Bridge_SendError;
				RFIDCtrl.result = RFID_RESULT_BRIDGE_ERROR;
			}
		}
		break;

	case RFID_State_Bridge_FieldOn :
		CR95HFStartField15693();
		RFIDCtrl.subState = RFID_State_Bridge_WaitFieldOn;
		break;

	case RFID_State_Bridge_WaitFieldOn :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_Bridge_Transfert;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_FIELD15693_ERROR) {
				RFIDCtrl.subState = RFID_State_Bridge_SendError;
				RFIDCtrl.result = RFID_RESULT_BRIDGE_ERROR;
			}
		}
		break;

	case RFID_State_Bridge_Transfert :
		CR95HFStartBridge(RFIDCtrl.bridgeCommand, RFIDCtrl.bridgeCommandLength);
		RFIDCtrl.subState = RFID_State_Bridge_WaitTransfert;
		break;

	case RFID_State_Bridge_WaitTransfert :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_Bridge_SendResult;
			} else {
				RFIDCtrl.subState = RFID_State_Bridge_SendError;
				RFIDCtrl.result = RFID_RESULT_BRIDGE_ERROR;
			}
		}
		break;

	case RFID_State_Bridge_SendResult :
		UsbManagerSendCR95Result(CR95HFCtrl.receiveBuf, CR95HFCtrl.rxPos);
		RFIDCtrl.subState = RFID_State_Bridge_Finish;
		break;

	case RFID_State_Bridge_SendError :
		UsbManagerSendCR95Error();
		RFIDCtrl.subState = RFID_State_Bridge_Finish;
		break;

	case RFID_State_Bridge_Finish :
		RFIDCtrl.request.bits.bridge = 0;
		RFIDCtrl.msRef = TickGetMsCounter();
		RFIDCtrl.state = RFID_STATE_STANDBY;
		break;

	default :
		RFIDCtrl.subState = RFID_State_Bridge_Start;
		break;
	}
}

void RFIDReadStateMachine(void) {
	static unsigned short addr = 0;
	static unsigned long msRef = 0;

	switch(RFIDCtrl.subState) {

	case RFID_State_Read_Start :
		addr = 0;
		RFIDCtrl.subState = RFID_State_Read_Antenna;

	case RFID_State_Read_Antenna :
		CR95SelectDevice(CR95_SELECT_EXTERNAL);
		CR95UartDisableReceiver();
		char data[1] = {0x55};
		CR95UartSendData(data,1);	//Try to wake up external
		msRef = TickGetMsCounter();
		RFIDCtrl.subState = RFID_State_Read_WaitAntenna;
		break;

	case RFID_State_Read_WaitAntenna :
		if(TickMilliSecElapsedFrom(msRef)>10) {
			if(CR95ExtGetConnectionState() == CR95_EXT_CONNECTED) {
				CR95SelectDevice(CR95_SELECT_EXTERNAL);
				RFIDExternalPowerMode = RFID_POWERED;
			} else {
				CR95SelectDevice(CR95_SELECT_INTERNAL);
				RFIDInternalPowerMode = RFID_POWERED;
			}
			RFIDCtrl.subState = RFID_State_Read_Echo;
		}
		break;

	case RFID_State_Read_Echo :
		CR95HFStartEcho();
		RFIDCtrl.subState = RFID_State_Read_WaitEcho;
		break;

	case RFID_State_Read_WaitEcho :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_Read_FieldOn;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_ECHO_ERROR) {
				RFIDCtrl.subState = RFID_State_Read_Finish;
				RFIDCtrl.result = RFID_RESULT_READ_ERROR;
			}
		}
		break;

	case RFID_State_Read_FieldOn :
		CR95HFStartField15693();
		RFIDCtrl.subState = RFID_State_Read_WaitFieldOn;
		break;

	case RFID_State_Read_WaitFieldOn :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_Read_Read;
				//RFIDCtrl.subState =RFID_State_Read_FieldOn;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_FIELD15693_ERROR) {
				RFIDCtrl.subState = RFID_State_Read_Finish;
				RFIDCtrl.result = RFID_RESULT_READ_ERROR;
			}
		}
		break;

	case RFID_State_Read_Read :
		CR95HFStartRead(addr);
		RFIDCtrl.subState = RFID_State_Read_WaitRead;
		break;

	case RFID_State_Read_WaitRead :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				*(((unsigned long*)&SRDEeprom) + addr ) = CR95HFCtrl.deviceReg;
				if(addr < (SRD_MEMORY_NBR_BLOCK-1)) {
					addr++;
					RFIDCtrl.subState = RFID_State_Read_Read;
				} else {
					RFIDCtrl.subState = RFID_State_Read_Finish;
				}
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_READ_ERROR) {
				RFIDCtrl.subState = RFID_State_Read_Finish;
				RFIDCtrl.result = RFID_RESULT_READ_ERROR;
			}
		}
		break;


	case RFID_State_Read_Finish :
		if(RFIDCtrl.result != RFID_RESULT_OK && ButtonReadGetState() == BUTTON_PRESSED) {
			RFIDCtrl.result = RFID_RESULT_OK;
			RFIDCtrl.subState = RFID_State_Read_Echo;
		} else {
			RFIDCtrl.request.bits.read = 0;
			RFIDCtrl.state = RFID_STATE_STANDBY;
		}
		break;

	default :
		RFIDCtrl.subState = RFID_State_Bridge_Start;
		break;
	}
}


void RFIDWriteStateMachine(void) {
	static unsigned short addr = 0;
	static unsigned long msRef = 0;

	switch(RFIDCtrl.subState) {

	case RFID_State_Write_Start :
		addr = 0;
		RFIDCtrl.subState = RFID_State_Write_Antenna;

	case RFID_State_Write_Antenna :
		CR95SelectDevice(CR95_SELECT_EXTERNAL);
		CR95UartDisableReceiver();
		char data[1] = {0x55};
		CR95UartSendData(data,1);	//Try to wake up external
		msRef = TickGetMsCounter();
		RFIDCtrl.subState = RFID_State_Write_WaitAntenna;
		break;

	case RFID_State_Write_WaitAntenna :
		if(TickMilliSecElapsedFrom(msRef)>10) {
			if(CR95ExtGetConnectionState() == CR95_EXT_CONNECTED) {
				CR95SelectDevice(CR95_SELECT_EXTERNAL);
				RFIDExternalPowerMode = RFID_POWERED;
			} else {
				CR95SelectDevice(CR95_SELECT_INTERNAL);
				RFIDInternalPowerMode = RFID_POWERED;
			}
			RFIDCtrl.subState = RFID_State_Write_Echo;
		}
		break;

	case RFID_State_Write_Echo :
		CR95HFStartEcho();
		RFIDCtrl.subState = RFID_State_Write_WaitEcho;
		break;

	case RFID_State_Write_WaitEcho :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_Write_FieldOn;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_ECHO_ERROR) {
				RFIDCtrl.subState = RFID_State_Write_Finish;
				RFIDCtrl.result = RFID_RESULT_WRITE_ERROR;
			}
		}
		break;

	case RFID_State_Write_FieldOn :
		CR95HFStartField15693();
		RFIDCtrl.subState = RFID_State_Write_WaitFieldOn;
		break;

	case RFID_State_Write_WaitFieldOn :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_Write_ReadSerial;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_FIELD15693_ERROR) {
				RFIDCtrl.subState = RFID_State_Write_Finish;
				RFIDCtrl.result = RFID_RESULT_WRITE_ERROR;
			}
		}
		break;

	case RFID_State_Write_ReadSerial :
		CR95HFStartRead(addr);
		RFIDCtrl.subState = RFID_State_Write_WaitReadSerial;
		break;

	case RFID_State_Write_WaitReadSerial :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				*(((unsigned long*)&SRDEeprom) + addr ) = CR95HFCtrl.deviceReg;
				if(addr < 7) {
					addr++;
					RFIDCtrl.subState = RFID_State_Write_ReadSerial;
				} else {
					RFIDCtrl.subState = RFID_State_Write_IsWritable;
				}
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_READ_ERROR) {
				RFIDCtrl.subState = RFID_State_Write_Finish;
				RFIDCtrl.result = RFID_RESULT_WRITE_ERROR;
			}
		}
		break;

	case RFID_State_Write_IsWritable :
		if(SRDEeprom.statusModule.all != 0) {
			RFIDCtrl.subState = RFID_State_Write_Finish;
			RFIDCtrl.result = RFID_RESULT_WRITE_ERROR;
		} else {
			RFIDCtrl.subState = RFID_State_Write_Lock;
		}
		break;

	case RFID_State_Write_Lock :
		SRDEeprom.ctrlModule.all = 0x01;
		CR95HFStartWrite(0,*((unsigned long*)&SRDEeprom) );
		RFIDCtrl.subState = RFID_State_Write_WaitLock;
		break;

	case RFID_State_Write_WaitLock :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_Write_Write;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_WRITE_ERROR) {
				RFIDCtrl.subState = RFID_State_Write_Finish;
				RFIDCtrl.result = RFID_RESULT_WRITE_ERROR;
			}
		}
		break;

	case RFID_State_Write_Write :

		SRDEeprom.debug = 0;
		SRDEeprom.activationDepth = DivingParameters.depth;
		SRDEeprom.filterMSB = DivingParameters.filterMSB;
		SRDEeprom.filterLSB = DivingParameters.filterLSB;

		unsigned short checksum = ChecksumByteOnShortCompute(&SRDEeprom.serial0,10);
		checksum += DivingParameters.depth;
		checksum += DivingParameters.filterMSB;
		checksum += DivingParameters.filterLSB;

		SRDEeprom.checksumGenMSB = (unsigned char)((checksum >> 8) & 0x00FF);
		SRDEeprom.checksumGenLSB = (unsigned char)((checksum >> 0) & 0x00FF);

		CR95HFStartWrite(1,*( ((unsigned long*)&SRDEeprom) + 1) );
		RFIDCtrl.subState = RFID_State_Write_WaitWrite;
		break;


	case RFID_State_Write_WaitWrite :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_Write_WriteChecksum;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_WRITE_ERROR) {
				RFIDCtrl.subState = RFID_State_Write_Unlock;	//Make sure to unlock
				RFIDCtrl.result = RFID_RESULT_WRITE_ERROR;
			}
		}
		break;

	case RFID_State_Write_WriteChecksum :
		CR95HFStartWrite(7,*( ((unsigned long*)&SRDEeprom) + 7) );
		RFIDCtrl.subState = RFID_State_Write_WaitWriteChecksum;
		break;

	case RFID_State_Write_WaitWriteChecksum :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				RFIDCtrl.subState = RFID_State_Write_Unlock;
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_WRITE_ERROR) {
				RFIDCtrl.subState = RFID_State_Write_Unlock;	//Make sure to unlock
				RFIDCtrl.result = RFID_RESULT_WRITE_ERROR;
			}
		}
		break;

	case RFID_State_Write_Unlock :
		SRDEeprom.ctrlModule.all = 0x00;
		CR95HFStartWrite(0,*((unsigned long*)&SRDEeprom) );
		RFIDCtrl.subState = RFID_State_Write_WaitUnlock;
		break;

	case RFID_State_Write_WaitUnlock :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				if(RFIDCtrl.result == RFID_RESULT_WRITE_ERROR) {
					RFIDCtrl.subState = RFID_State_Write_Finish;
				} else {
					RFIDCtrl.subState = RFID_State_Write_Read;
					addr = 0;
				}
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_WRITE_ERROR) {
				RFIDCtrl.subState = RFID_State_Write_Finish;
				RFIDCtrl.result = RFID_RESULT_WRITE_ERROR;
			}
		}
		break;

	case RFID_State_Write_Read :
		CR95HFStartRead(addr);
		RFIDCtrl.subState = RFID_State_Write_WaitRead;
		break;

	case RFID_State_Write_WaitRead :
		if(CR95HFManagerIsReady() == CR95HF_IS_READY) {
			if(CR95HFCtrl.resultCode == CR95HF_RETURN_OK) {
				*(((unsigned long*)&SRDEeprom) + addr ) = CR95HFCtrl.deviceReg;
				if(addr < (SRD_MEMORY_NBR_BLOCK-1)) {
					addr++;
					RFIDCtrl.subState = RFID_State_Write_Read;
				} else {
					RFIDCtrl.subState = RFID_State_Write_Finish;
				}
			} else if (CR95HFCtrl.resultCode == CR95HF_RETURN_READ_ERROR) {
				RFIDCtrl.subState = RFID_State_Write_Finish;
				RFIDCtrl.result = RFID_RESULT_WRITE_ERROR;
			}
		}
		break;


	case RFID_State_Write_Finish :
		if(RFIDCtrl.result != RFID_RESULT_OK && ButtonWriteGetState() == BUTTON_PRESSED) {
			RFIDCtrl.result = RFID_RESULT_OK;
			RFIDCtrl.subState = RFID_State_Write_Echo;
		} else {
			RFIDCtrl.request.bits.write = 0;
			RFIDCtrl.state = RFID_STATE_STANDBY;
		}
		break;

	default :
		RFIDCtrl.subState = RFID_State_Write_Start;
		break;
	}
}



signed short RFIDManagerSendBridgeCommand (char* command, unsigned short commandSize) {

	//if(RFIDCtrl.state == RFID_STATE_STANDBY) {
	unsigned short i;

		for(i=0 ; i<commandSize ; i++) {
			RFIDCtrl.bridgeCommand[i] = *(command+i);
		}

		RFIDCtrl.bridgeCommandLength = commandSize;

		RFIDCtrl.request.bits.bridge = 1;

		return 0;

	//} else {
	//	return -1;		//Error
	//}
}


/*############################################################################################*/
/*## END EXTERNAL FONCTIONS ##################################################################*/
/*############################################################################################*/
