#include <stdlib.h>
#include <string.h>
#include "gfambrtuslv.h"
#include "gfambrtuslv_priv.h"
#include "driverlib/sw_crc.h"
#include "inc/hw_memmap.h"

/////////////////////////////////////////////////////////////////////////////

static GFA_MODBUS_RTU_SLAVE g_mbSlv;

/////////////////////////////////////////////////////////////////////////////

static bool _PeekStateChange(LPGFA_MODBUS_RTU_SLAVE pSlv)
{
	if(pSlv->oldstate != pSlv->state)
	{
		if(pSlv->appItf.pfnStateChanged)
			(*pSlv->appItf.pfnStateChanged)(pSlv->state, pSlv->oldstate);
		pSlv->oldstate = pSlv->state;
		return true;
	}
	return false;
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

HMBRTUSLV GfaModbusRTUSlvCreate(LPCGFA_MODBUS_RTU_SLAVE_PARAMETERS pslp)
{
	if(!pslp)
		return NULL;
	if(!pslp->slaveID || pslp->slaveID > MODBUS_MAX_SLAVE_ID)
		return NULL;
	if(!pslp->hFifoRX || !pslp->hFifoTX || !pslp->regMap.pRegs || !pslp->regMap.nCountRegs)
		return NULL;

	LPGFA_MODBUS_RTU_SLAVE pSlv = &g_mbSlv;
	memset(pSlv, 0, sizeof(GFA_MODBUS_RTU_SLAVE));

	pSlv->state		= MB_RTU_SLV_Idle;
	pSlv->oldstate	= MB_RTU_SLV_Void;
	pSlv->slaveID	= pslp->slaveID;
	pSlv->hFifoRX	= pslp->hFifoRX;
	pSlv->hFifoTX	= pslp->hFifoTX;

	memcpy(&pSlv->regMap, &pslp->regMap, sizeof(GFA_MODBUS_REGISTER));
	memcpy(&pSlv->appItf, &pslp->appItf, sizeof(GFA_MODBUS_SLAVE_APP_INTERFACE));
	return (HMBRTUSLV)pSlv;
}

/////////////////////////////////////////////////////////////////////////////

void GfaModbusRTUSlvRelease(HMBRTUSLV hMbSlv)
{
}

/////////////////////////////////////////////////////////////////////////////

bool GfaModbusRTUSlvSetID(HMBRTUSLV hMbSlv, uint8_t newID)
{
	LPGFA_MODBUS_RTU_SLAVE pSlv = (LPGFA_MODBUS_RTU_SLAVE)hMbSlv;

	if(!newID || newID > MODBUS_MAX_SLAVE_ID)
		return false;

	if(pSlv->newSlaveID != newID)
		pSlv->newSlaveID = newID;

	return true;
}

/////////////////////////////////////////////////////////////////////////////

bool GfaModbusRTUSlvStateMachine(HMBRTUSLV hMbSlv)
{
	static int nReEnterSTM = 0;
	uint8_t b;
	size_t nRead;
	LPGFA_MODBUS_RTU_SLAVE pSlv = (LPGFA_MODBUS_RTU_SLAVE)hMbSlv;

	++nReEnterSTM;

	switch(pSlv->state)
	{
	case MB_RTU_SLV_Idle:
		nReEnterSTM = 0;
		_PeekStateChange(pSlv);
		if(pSlv->newSlaveID)
		{
			pSlv->slaveID = pSlv->newSlaveID;
			pSlv->newSlaveID = 0;
		}
		if(!GfaMbFifoMatchFlags(pSlv->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true))
			break;
		pSlv->state			= MB_RTU_SLV_RxSlvID;
		pSlv->nCbToRead		= 0;
		pSlv->nCbToWrite	= 0;
		pSlv->nDataPtr		= 0;
		pSlv->bCRCOk		= false;
		GfaMbFifoClearFlags(pSlv->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true);
		// fall through

	case MB_RTU_SLV_RxSlvID:
		_PeekStateChange(pSlv);
		if(!GfaMbFifoPop(pSlv->hFifoRX, &b, true))
		{
			if(GfaMbFifoMatchFlags(pSlv->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true))
				pSlv->state	= MB_RTU_SLV_Idle;
			break;
		}
		if(pSlv->slaveID != b) // ignore broadcasts as well
		{
			GfaMbFifoSetFlags(pSlv->hFifoRX, MB_RTU_FLAG_IGNORE_FRAME, true);
			GfaMbFifoReset(pSlv->hFifoRX, true);
			pSlv->state		= MB_RTU_SLV_Idle;
			++pSlv->diag.nBusMsgCount;
			break;
		}
		pSlv->adu.slaveID	= b;
		pSlv->state			= MB_RTU_SLV_RxFunc;
		++pSlv->diag.nBusMsgCount;
		// fall through

	case MB_RTU_SLV_RxFunc:
		_PeekStateChange(pSlv);
		if(!GfaMbFifoPop(pSlv->hFifoRX, &b, true))
		{
			if(GfaMbFifoMatchFlags(pSlv->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true))
				pSlv->state	= MB_RTU_SLV_Idle;
			break;
		}
		else if(!GfaModbusRequestFunctionKnown(b))
		{
			pSlv->adu.pdu.func = b;
			GfaMbFifoSetFlags(pSlv->hFifoRX, MB_RTU_FLAG_IGNORE_FRAME, true);
			GfaMbFifoReset(pSlv->hFifoRX, true);
			GfaModbusRequestCreateExceptionResponse(&pSlv->adu.pdu, MB_ERROR_ILLEGAL_FUNCTION);
			GfaBufSetCRC(&pSlv->adu, 3, &pSlv->adu.pdu.b[1]);
			pSlv->nCbToWrite = 5;
			pSlv->state	= MB_RTU_SLV_TxStart;
			++pSlv->diag.nExcErrCount;
			break;
		}
		pSlv->adu.pdu.func	= b;
		pSlv->state			= MB_RTU_SLV_RxDataInfo;
		pSlv->nCbToRead		= GfaModbusRequestGetDataInfoLength(pSlv);
		// fall through

	case MB_RTU_SLV_RxDataInfo:
		_PeekStateChange(pSlv);
		if(pSlv->nCbToRead)
		{
			if(!(nRead = GfaMbFifoRead(pSlv->hFifoRX, &pSlv->adu.pdu.b[pSlv->nDataPtr], pSlv->nCbToRead, true)))
			{
				if(GfaMbFifoMatchFlags(pSlv->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true))
					pSlv->state	= MB_RTU_SLV_Idle;
				break;
			}
			pSlv->nDataPtr	+= nRead;
			pSlv->nCbToRead	-= nRead;
		}

		if(pSlv->nCbToRead)
			break;
		else
		{
			if(pSlv->adu.pdu.func == MB_FUNC_DIAGNOSTIC)
			{
				pSlv->nSubFunc = GfaBufGetUnaligned_uint16(pSlv->adu.pdu.b);

				if(!GfaModbusRequestSubFunctionKnown(pSlv->nSubFunc))
				{
					pSlv->adu.pdu.func = b;
					GfaMbFifoSetFlags(pSlv->hFifoRX, MB_RTU_FLAG_IGNORE_FRAME, true);
					GfaMbFifoReset(pSlv->hFifoRX, true);
					GfaModbusRequestCreateExceptionResponse(&pSlv->adu.pdu, MB_ERROR_ILLEGAL_FUNCTION);
					GfaBufSetCRC(&pSlv->adu, 3, &pSlv->adu.pdu.b[1]);
					pSlv->nCbToWrite = 5;
					pSlv->state	= MB_RTU_SLV_TxStart;
					++pSlv->diag.nExcErrCount;
					break;
				}
			}
			pSlv->state		= MB_RTU_SLV_RxDataPayload;
			pSlv->nCbToRead	= GfaModbusRequestGetDataPayloadLength(pSlv);
			// fall through
		}

	case MB_RTU_SLV_RxDataPayload:
		_PeekStateChange(pSlv);
		if(pSlv->nCbToRead)
		{
			if(!(nRead = GfaMbFifoRead(pSlv->hFifoRX, &pSlv->adu.pdu.b[pSlv->nDataPtr], pSlv->nCbToRead, true)))
			{
				if(GfaMbFifoMatchFlags(pSlv->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true))
					pSlv->state	= MB_RTU_SLV_Idle;
				break;
			}
			pSlv->nDataPtr	+= nRead;
			pSlv->nCbToRead	-= nRead;
		}

		if(pSlv->nCbToRead)
			break;
		else
		{
			pSlv->state		= MB_RTU_SLV_RxCRC;
			pSlv->nCbToRead	= 2;
			// fall through
		}

	case MB_RTU_SLV_RxCRC:
		_PeekStateChange(pSlv);
		if(pSlv->nCbToRead)
		{
			if(!(nRead = GfaMbFifoRead(pSlv->hFifoRX, &pSlv->adu.pdu.b[pSlv->nDataPtr], pSlv->nCbToRead, true)))
			{
				if(GfaMbFifoMatchFlags(pSlv->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true))
					pSlv->state	= MB_RTU_SLV_Idle;
				break;
			}
			pSlv->nDataPtr	+= nRead;
			pSlv->nCbToRead	-= nRead;
		}

		if(pSlv->nCbToRead)
			break;
		else
		{
			pSlv->bCRCOk	= GfaBufVerifyCRC(&pSlv->adu, pSlv->nDataPtr, &pSlv->adu.pdu.b[pSlv->nDataPtr - 2]);
			pSlv->state		= MB_RTU_SLV_RxComplete;
			// fall through
		}

	case MB_RTU_SLV_RxComplete:
		_PeekStateChange(pSlv);
		if(pSlv->bCRCOk)
		{
			bool bIsExeption;
			size_t nCbData = 0;
			++pSlv->diag.nSlvMsgCount;

			if(GfaModbusRequestHandler(pSlv, &nCbData, &bIsExeption))
			{
				GfaBufSetCRC(&pSlv->adu, nCbData + 2, &pSlv->adu.pdu.b[nCbData]);
				pSlv->nCbToWrite	= nCbData + 4;
				pSlv->state			= MB_RTU_SLV_TxStart;
				if(bIsExeption)
					++pSlv->diag.nExcErrCount;
				GfaMbFifoRxFinalize(pSlv->hFifoRX, true);
				// fall through
			}
			else
			{
				pSlv->state = MB_RTU_SLV_Idle;	// nothing to send
				break;
			}
		}
		else
		{
			++pSlv->diag.nCrcErrCount;		// CRC Error!!!
			pSlv->state = MB_RTU_SLV_Idle;	// silently ignore frame
			break;
		}

	case MB_RTU_SLV_TxStart:
		_PeekStateChange(pSlv);
		if(	GfaMbFifoEmpty(pSlv->hFifoTX, true) &&
			GfaMbFifoTxPrepare(pSlv->hFifoTX, true))
		{
			GfaMbFifoSetFlags(pSlv->hFifoTX, MB_RTU_FLAG_TRANSMIT_IN_PROGRESS, true);
			GfaMbFifoWrite(pSlv->hFifoTX, &pSlv->adu, pSlv->nCbToWrite, true);

			if(pSlv->appItf.pfnPreTransmit)
				(*pSlv->appItf.pfnPreTransmit)(&pSlv->adu);
			GfaMbFifoTxStart(pSlv->hFifoTX, false);
			pSlv->state = MB_RTU_SLV_TxWaitEnd;
			// fall through
		}
		else
			break;

	case MB_RTU_SLV_TxWaitEnd:
		_PeekStateChange(pSlv);
		if(!GfaMbFifoMatchFlags(pSlv->hFifoTX, MB_RTU_FLAG_TRANSMIT_IN_PROGRESS, true))
		{
			if(pSlv->appItf.pfnPostTransmit)
				(*pSlv->appItf.pfnPostTransmit)(&pSlv->adu);
			pSlv->state = MB_RTU_SLV_Idle;
		}
		break;

	default:
		return false;
	}

	if(pSlv->nMaxReEnterSTM < nReEnterSTM)
		pSlv->nMaxReEnterSTM = nReEnterSTM;
	return true;
}