#include "ModbusSlave.h"
#include "Debug.h"

#include "ModbusRtuCodec.h"
#include "ModbusTcpCodec.h"

// #define LogShowArray ShowArray
#define LogShowArray(...)

void ModbusRtuMsgProcess(const ModbusSlave_t* mrs, byte* p, int pklen, void* sendparam)
{
	if (p == NULL)return;
	if (pklen < 8)return;

	LogShowArray("485 ", p, pklen, 16);
	if (mrs == NULL)return;
	if (mrs->SlaveAddr == NULL)return;

	// ·¢ËÍÓõĻº³åÇø
	byte tx[264];
	byte* txpy = tx + 8;

	// ½âÊý¾Ý°ü
	byte addr = p[0];
	byte cmd = p[1];

	if ((addr == (*mrs->SlaveAddr)) || (addr == 0xff))
	{
		switch (cmd)
		{
		case 1:	// ¶ÁÊä³öÏßȦ
		case 2: // ¶ÁÊäÈë
		{
			// ´¦Àíһϴó¶Ë
			ushort	regaddr = (p[2] << 8) + p[3];
			ushort	bitcnt = (p[4] << 8) + p[5];

			// Çå¿Õ»º³åÇø
			memset(txpy, 0x00, bitcnt / 8 + 1);
			// ¶Á bit ¿Õ¼ä
			if (mrs->BitAreas != NULL) AreaRead2(mrs->BitAreas, regaddr, txpy, bitcnt, true);
			int txlen = MrcResult01a02(addr, cmd, txpy, bitcnt, tx, sizeof(tx));

			LogShowArray("-> ", tx, txlen, 16);
			if (mrs->Send != NULL) mrs->Send(tx, txlen, sendparam);
		}break;
		// дÊä³öÏßȦ
		case 5:
		{
			ushort regaddr = (p[2] << 8) + p[3];
			ushort regdata = (p[4] << 8) + p[5];
			byte value = regdata > 0 ? 0x01 : 0x00;

			if (mrs->BitAreas != NULL) AreaWrite2(mrs->BitAreas, regaddr, &value, 1, true);

			LogShowArray("-> ", p, pklen, 16);
			if (mrs->Send != NULL) mrs->Send(p, pklen, sendparam);
		}break;
		// дÊä³öÏßȦS
		case 0x0f:
		{
			ushort regaddr = (p[2] << 8) + p[3];
			ushort bitcnt = (p[4] << 8) + p[5];

			if (mrs->BitAreas != NULL) AreaWrite2(mrs->BitAreas, regaddr, &p[7], bitcnt, true);
			int txlen = MrcResult0f(addr, regaddr, bitcnt, tx, sizeof(tx));

			LogShowArray("-> ", tx, txlen, 16);
			if (mrs->Send != NULL) mrs->Send(tx, txlen, sendparam);
		}break;
		case 3:	// ¶Á¶à¸ö±£³Ö¼Ä´æÆ÷
		case 4:	// ¶ÁÊäÈë¼Ä´æÆ÷
		{
			// ´¦Àíһϴó¶Ë
			ushort	regaddr = (p[2] << 8) + p[3];
			ushort	regcnt = (p[4] << 8) + p[5];

			// Çå¿Õ»º³åÇø
			memset(txpy, 0x00, regcnt * 2);
			// ¶Á byte ¿Õ¼ä
			// modbus µØÖ·¶ÔÓ¦µÄÊÇ 2×Ö½Ú£¬µØÖ·ÐèÒª X2 ¶ÔÓ¦×ֽڿռ䡣
			if (mrs->ByteAreas != NULL) AreaRead2(mrs->ByteAreas, regaddr * 2, txpy, regcnt * 2, false);
			int txlen = MrcResult03a04(addr, cmd, txpy, regcnt, tx, sizeof(tx));

			LogShowArray("-> ", tx, txlen, 16);
			if (mrs->Send != NULL) mrs->Send(tx, txlen, sendparam);
		}
		break;
		case 6:	// дµ¥¸ö¼Ä´æÆ÷
		{
			ushort regaddr = (p[2] << 8) + p[3];
			ushort regdata = (p[4] << 8) + p[5];

			// modbus µØÖ·¶ÔÓ¦µÄÊÇ 2×Ö½Ú£¬µØÖ·ÐèÒª X2 ¶ÔÓ¦×ֽڿռ䡣
			if (mrs->ByteAreas != NULL) AreaWrite2(mrs->ByteAreas, regaddr * 2, (byte*)&regdata, 2, false);

			LogShowArray("-> ", p, pklen, 16);
			if (mrs->Send != NULL) mrs->Send(p, pklen, sendparam);
		}break;
		case 0x10:
		{
			ushort regaddr = (p[2] << 8) + p[3];
			ushort regcnt = (p[4] << 8) + p[5];

			// modbus µØÖ·¶ÔÓ¦µÄÊÇ 2×Ö½Ú£¬µØÖ·ÐèÒª X2 ¶ÔÓ¦×ֽڿռ䡣
			if (mrs->ByteAreas != NULL) AreaWrite2(mrs->ByteAreas, regaddr * 2, &p[7], regcnt * 2, false);
			int txlen = MrcResult10(addr, regaddr, regcnt, tx, sizeof(tx));

			LogShowArray("-> ", tx, txlen, 16);
			if (mrs->Send != NULL) mrs->Send(tx, txlen, sendparam);
		}break;

		default:DebugPrintf("CMD 0x%02X\r\n", cmd); break;
		}
	}
}

void ModbusRtuTryProcess(CircularQueue_t* queue, const ModbusSlave_t* mrs, void* sendparam)
{
	if (queue == NULL)return;

	// Êý¾Ý°ü³¤¶È
	int pklen = MrcSlaveGetLenCircularQueue(queue);
	// ²»¹»³¤
	if (pklen == 0)return;
	// ÓÐÎÊÌâµÄ°ü
	if (pklen < 0) 
	{
		// byte data = 0;
		// CircularQueueRead(queue,&data);
		// DebugPrintf("%s seek 0x%02X\r\n",__func__, data);
		CircularQueueSeek(queue, 1); 
		return; 
	}

	// ¶ÁÊý¾Ý
	byte* p = (byte*)GlobleMalloc(pklen + 4);
	CircularQueueReads(queue, p, pklen, false);

	// ´¦ÀíÊý¾Ý
	ModbusRtuMsgProcess(mrs, p, pklen, sendparam);

	// free
	GlobleFree(p);
}

void ModbusTcpMsgProcess(const ModbusSlave_t* mrs, byte* pk, int pklen, void* sendparam)
{
	if (pk == NULL)return;
	if (pklen < 6 + 6)return;

	LogShowArray("Rev ", pk, pklen, 16);
	if (mrs == NULL)return;

	byte* p = pk + 6;

	// TCP ²»ÐèÒª byte addr = p[0];
	// µ«ÊÇÒª head£¬Ô­Ñù·µ»ØµÄ head
	MtcHead_t* head = (MtcHead_t*)pk;
	byte cmd = p[1];

	// ·¢ËÍÓõĻº³åÇø
	byte tx[272];
	byte* txpy = tx + 16;
	// int txpylen = sizeof(tx) - 16;

	switch (cmd)
	{
		// ¶ÁÊäÈë
	case 2:
	{
		ushort	regaddr = (p[2] << 8) + p[3];
		ushort	bitcnt = (p[4] << 8) + p[5];

		// Çå¿Õ»º³åÇø
		memset(txpy, 0x00, bitcnt / 8 + 1);
		// ¶Á bit ¿Õ¼ä
		if (mrs->BitAreas != NULL)AreaRead2(mrs->BitAreas, regaddr, txpy, bitcnt, true);
		int txlen = MtcResult01a02(head, cmd, txpy, bitcnt, tx, sizeof(tx));

		LogShowArray("-> ", tx, txlen, 16);
		if (mrs->Send != NULL)mrs->Send(tx, txlen, sendparam);
	}break;
	// ¶ÁÊä³öÏßȦ
	case 1:
	{
		ushort	regaddr = (p[2] << 8) + p[3];
		ushort	bitcnt = (p[4] << 8) + p[5];

		// Çå¿Õ»º³åÇø
		memset(txpy, 0x00, bitcnt / 8 + 1);
		// ¶Á bit ¿Õ¼ä
		if (mrs->BitAreas != NULL)AreaRead2(mrs->BitAreas, regaddr, txpy, bitcnt, true);
		int txlen = MtcResult01a02(head, cmd, txpy, bitcnt, tx, sizeof(tx));

		LogShowArray("-> ", tx, txlen, 16);
		if (mrs->Send != NULL)mrs->Send(tx, txlen, sendparam);
	}break;
	// дÊä³öÏßȦ
	case 5:
	{
		ushort regaddr = (p[2] << 8) + p[3];
		ushort regdata = (p[4] << 8) + p[5];

		byte value = regdata > 0 ? 0x01 : 0x00;
		if (mrs->BitAreas != NULL)AreaWrite2(mrs->BitAreas, regaddr, &value, 1, true);

		LogShowArray("-> ", pk, pklen, 16);
		if (mrs->Send != NULL)mrs->Send(pk, pklen, sendparam);
	}break;
	// дÊä³öÏßȦS
	case 0x0f:
	{
		ushort regaddr = (p[2] << 8) + p[3];
		ushort bitcnt = (p[4] << 8) + p[5];

		if (mrs->BitAreas != NULL)AreaWrite2(mrs->BitAreas, regaddr, &p[7], bitcnt, true);
		int txlen = MtcResult0f(head, regaddr, bitcnt, tx, sizeof(tx));

		LogShowArray("-> ", tx, txlen, 16);
		if (mrs->Send != NULL)mrs->Send(tx, txlen, sendparam);
	}break;


	case 3:	// ¶Á¶à¸ö±£³Ö¼Ä´æÆ÷
	case 4:	// ¶ÁÊäÈë¼Ä´æÆ÷
	{
		// ´¦Àíһϴó¶Ë
		ushort	regaddr = (p[2] << 8) + p[3];
		ushort	regcnt = (p[4] << 8) + p[5];

		// Çå¿Õ»º³åÇø
		memset(txpy, 0x00, regcnt * 2);
		// ¶Á byte ¿Õ¼ä
		// modbus µØÖ·¶ÔÓ¦µÄÊÇ 2×Ö½Ú£¬µØÖ·ÐèÒª X2 ¶ÔÓ¦×ֽڿռ䡣
		if (mrs->ByteAreas != NULL)AreaRead2(mrs->ByteAreas, regaddr * 2, txpy, regcnt * 2, false);
		int txlen = MtcResult03a04(head, cmd, txpy, regcnt, tx, sizeof(tx));

		LogShowArray("-> ", tx, txlen, 16);
		if (mrs->Send != NULL)mrs->Send(tx, txlen, sendparam);
	}
	break;
	case 6:	// дµ¥¸ö¼Ä´æÆ÷
	{
		ushort regaddr = (p[2] << 8) + p[3];
		ushort regdata = (p[4] << 8) + p[5];

		// modbus µØÖ·¶ÔÓ¦µÄÊÇ 2×Ö½Ú£¬µØÖ·ÐèÒª X2 ¶ÔÓ¦×ֽڿռ䡣
		if (mrs->ByteAreas != NULL)AreaWrite2(mrs->ByteAreas, regaddr * 2, (byte*)&regdata, 2, false);

		LogShowArray("-> ", pk, pklen, 16);
		if (mrs->Send != NULL)mrs->Send(pk, pklen, sendparam);
	}break;
	case 0x10:
	{
		ushort regaddr = (p[2] << 8) + p[3];
		ushort regcnt = (p[4] << 8) + p[5];

		// modbus µØÖ·¶ÔÓ¦µÄÊÇ 2×Ö½Ú£¬µØÖ·ÐèÒª X2 ¶ÔÓ¦×ֽڿռ䡣
		if (mrs->ByteAreas != NULL)AreaWrite2(mrs->ByteAreas, regaddr * 2, &p[7], regcnt * 2, false);
		int txlen = MtcResult10(head, regaddr, regcnt, tx, sizeof(tx));

		LogShowArray("-> ", tx, txlen, 16);
		if (mrs->Send != NULL)mrs->Send(tx, txlen, sendparam);
	}break;

	default:DebugPrintf("CMD 0x%02X\r\n", cmd); break;
	}
}

void ModbusTcpTryProcess(CircularQueue_t* queue, const ModbusSlave_t* mrs, void* sendparam)
{
	if (queue == NULL)return;

	int pklen = MtcGetLenCircularQueue(queue);
	// ²»¹»³¤
	if (pklen == 0)return;
	// ÓÐÎÊÌâµÄ°ü
	if (pklen < 0) { CircularQueueSeek(queue, 1); return; }

	// ¶ÁÊý¾Ý
	byte* pk = (byte*)GlobleMalloc(pklen + 4);
	CircularQueueReads(queue, pk, pklen, false);

	// ´¦ÀíÊý¾Ý
	ModbusTcpMsgProcess(mrs, pk, pklen, sendparam);

	// free
	GlobleFree(pk);
}

void ModbusUdpTryProcess(Stream_t* ms, const ModbusSlave_t* mrs, void* sendparam)
{
	// ÒòΪ UDP ²»´æÔÚÊý¾Ý°üÕ³Á¬ÎÊÌ⣬¶øÇÒÊÇÒ»´ÎÐÔµ½´ï¡£
	// ûÓÐʹÓà CircularQueue_t µÄÇé¿ö¡£
	// ʹÓà Stream_t  »òÕß  Buffer ¶¼¿ÉÒÔ¡£
	// UDP Êý¾Ý°üÓë TCP ÍêÈ«Ïàͬ¡£
	if (ms == NULL)return;
	// if (StreamRemian(ms) < 12)return;
	int pklen = MtcGetLenStream(ms);
	// ²»¹»³¤ »òÕß Êý¾Ý°üÓÐÎÊÌâ¡£
	if (pklen <= 0)return;

	// ´¦ÀíÊý¾Ý
	ModbusTcpMsgProcess(mrs, ms->MemStart + ms->Position, pklen, sendparam);
}