//  Copyright © 2019 Jean-Luc Deltombe (LX3JL). All rights reserved.

// urfd -- The universal reflector
// Copyright © 2021 Thomas A. Early N7TAE
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.

#include <string.h>

#include "CRC.h"
#include "YSFFich.h"
#include "YSFPayload.h"
#include "YSFClient.h"
#include "YSFUtils.h"
#include "WiresXCmdHandler.h"
#include "Global.h"

////////////////////////////////////////////////////////////////////////////////////////
// constructor

CWiresxCmdHandler::CWiresxCmdHandler()
{
	m_seqNo = 0;
	keep_running = true;
}


////////////////////////////////////////////////////////////////////////////////////////
// destructor

CWiresxCmdHandler::~CWiresxCmdHandler()
{
	// kill thread
	Close();

	// empty queue
	m_CmdQueue.Lock();
	while ( !m_CmdQueue.empty() )
	{
		m_CmdQueue.pop();
	}
	m_CmdQueue.Unlock();
}

////////////////////////////////////////////////////////////////////////////////////////
// initialization

bool CWiresxCmdHandler::Init(void)
{
	// fill our wiresx info
	m_ReflectorWiresxInfo.SetCallsign(g_Refl.GetCallsign());
	m_ReflectorWiresxInfo.SetNode(g_Refl.GetCallsign());
	m_ReflectorWiresxInfo.SetName("Reflector");

	// reset stop flag
	keep_running = true;

	try
	{
		m_Future = std::async(std::launch::async, &CWiresxCmdHandler::Thread, this);
	}
	catch(const std::exception &e)
	{
		std::cerr << "ERROR: could not start WiresX Command Handler: " << e.what() << std::endl;
		return false;
	}


	// done
	return true;
}

void CWiresxCmdHandler::Close(void)
{
	keep_running = false;
	if ( m_Future.valid() )
	{
		m_Future.get();
	}
}

////////////////////////////////////////////////////////////////////////////////////////
// thread

void CWiresxCmdHandler::Thread()
{
	while (keep_running)
	{
		Task();
	}
}

////////////////////////////////////////////////////////////////////////////////////////
// task

void CWiresxCmdHandler::Task(void)
{
	CWiresxInfo Info;
	CWiresxCmd  Cmd;
	uint32_t  uiNodeTxFreq;
	uint32_t  uiNodeRxFreq;
	char cModule;
	bool bCmd;

	// anything to do ?
	bCmd = false;
	m_CmdQueue.Lock();
	{
		// any cmd in our queue ?
		if ( !m_CmdQueue.empty() )
		{
			// yes, get a command
			Cmd = m_CmdQueue.front();
			// check that the command is at least one second old
			// so that the so delayed processing of the command
			// introduce the delay the radio needs to switch
			// from tx to rx
			if ( Cmd.GetTime().time() >= WIRESX_REPLY_DELAY )
			{
				m_CmdQueue.pop();
				bCmd = true;
			}
		}
	}
	m_CmdQueue.Unlock();


	// handle it
	if ( bCmd )
	{
		// fill our info object
		Info = m_ReflectorWiresxInfo;
		g_LYtr.FindFrequencies(Cmd.GetCallsign(), &uiNodeTxFreq, &uiNodeRxFreq);
		Info.SetFrequencies(uiNodeTxFreq, uiNodeRxFreq);

		// find our client and the module it's currentlink linked to
		cModule = ' ';
		CClients *clients = g_Refl.GetClients();
		std::shared_ptr<CClient>client = clients->FindClient(Cmd.GetCallsign(), Cmd.GetIp(), EProtocol::ysf);
		if ( client )
		{
			cModule = client->GetReflectorModule();
		}
		g_Refl.ReleaseClients();

		// and crack the cmd
		switch ( Cmd.GetCmd() )
		{
		case WIRESX_CMD_DX_REQ:
			std::cout << "Wires-X DX_REQ command from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << std::endl;
			// reply
			ReplyToWiresxDxReqPacket(Cmd.GetIp(), Info, cModule);
			break;
		case WIRESX_CMD_ALL_REQ:
		case WIRESX_CMD_SEARCH_REQ:
			std::cout << "Wires-X ALL_REQ command from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << std::endl;
			// reply
			ReplyToWiresxAllReqPacket(Cmd.GetIp(), Info, Cmd.GetArg());
			break;
		case WIRESX_CMD_CONN_REQ:
			cModule = 'A' + (char)(Cmd.GetArg() - 1);
			if (g_Refl.IsValidModule(cModule))
			{
				std::cout << "Wires-X CONN_REQ command to link on module " << cModule << " from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << std::endl;
				// acknowledge
				ReplyToWiresxConnReqPacket(Cmd.GetIp(), Info, cModule);
				// change client's module
				CClients *clients = g_Refl.GetClients();
				std::shared_ptr<CClient>client = clients->FindClient(Cmd.GetCallsign(), Cmd.GetIp(), EProtocol::ysf);
				if ( client )
				{
					client->SetReflectorModule(cModule);
				}
				g_Refl.ReleaseClients();
			}
			else
			{
				std::cout << "Wires-X CONN_REQ command with illegal argument from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << std::endl;
			}
			break;
		case WIRESX_CMD_DISC_REQ:
			std::cout << "Wires-X DISC_REQ command from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << std::endl;
			// and reply
			ReplyToWiresxDiscReqPacket(Cmd.GetIp(), Info);
			// change client's module
			{
				CClients *clients = g_Refl.GetClients();
				std::shared_ptr<CClient>client = clients->FindClient(Cmd.GetCallsign(), Cmd.GetIp(), EProtocol::ysf);
				if ( client != nullptr )
				{
					client->SetReflectorModule(' ');
				}
				g_Refl.ReleaseClients();
			}
			break;
		case WIRESX_CMD_UNKNOWN:
		default:
			std::cout << "Wires-X non implemented command from " << Cmd.GetCallsign() << " at " << Cmd.GetIp() << " cmd=" << Cmd.GetCmd() << " arg=" << Cmd.GetArg() << std::endl;
			break;
		}
	}
	else
	{
		// if nothing to do, sleep a bit
		std::this_thread::sleep_for(std::chrono::milliseconds(100));
	}

}

////////////////////////////////////////////////////////////////////////////////////////
// packet encoding

bool CWiresxCmdHandler::ReplyToWiresxDxReqPacket(const CIp &Ip, const CWiresxInfo &WiresxInfo, char Module)
{
	uint8_t DX_RESP[] = {0x5DU, 0x51U, 0x5FU, 0x26U};
	bool ok;
	uint8_t data[150U];
	uint8_t RoomId;
	bool IsLinked;

	// linked module
	// module A == 0
	IsLinked = (Module != ' ');
	RoomId = (uint8_t)(Module - 'A');

	// fill data buffer
	memset(data, 0x00U, 150U);
	memset(data, ' ', 128U);
	// seq no
	data[0U] = m_seqNo;
	// command
	memcpy(data +  1U, DX_RESP, 4U);
	// node info
	memcpy(data +  5U, WiresxInfo.GetId(), 5U);
	memcpy(data + 10U, WiresxInfo.GetNode(), 10U);
	memcpy(data + 20U, WiresxInfo.GetName(), 14U);
	// linked room
	if (!IsLinked)
	{
		data[34U] = '1';
		data[35U] = '2';
		// no room linked
		data[57U] = '0';
		data[58U] = '0';
		data[59U] = '0';
	}
	else
	{
		data[34U] = '1';
		data[35U] = '5';
		// linked room
		char item[16U];
		// refl->m_id
		::sprintf(item, "%05d", 4001U + RoomId);
		memcpy(data + 36U, item, 5U);
		// refl->name
		memset(item, ' ', 16U);
		memcpy(item, "MODULE", 6U); // K2IE fix for U/C only radios
		item[7] = 'A' + RoomId;
		memcpy(data + 41U, item, 16U);
		// refl->count
		::sprintf(item, "%03d", RoomId + 1);
		memcpy(data + 57U, item, 3U);
		// other
		memset(data + 60U, ' ', 10U);
		// refl->m_desc
		memcpy(data + 70U, "Description   ", 14U);
	}

	// frequencies
	{
		unsigned int offset;
		char sign;
		if (WiresxInfo.GetTxFrequency() >= WiresxInfo.GetRxFrequency())
		{
			offset = WiresxInfo.GetTxFrequency() - WiresxInfo.GetRxFrequency();
			sign = '-';
		}
		else
		{
			offset = WiresxInfo.GetRxFrequency() - WiresxInfo.GetTxFrequency();
			sign = '+';
		}
		unsigned int freqHz = WiresxInfo.GetTxFrequency() % 1000000U;
		//unsigned int freqkHz = (freqHz + 500U) / 1000U;

		char freq[30U];
		::sprintf(freq, "%05u.%06u%c%03u.%06u", WiresxInfo.GetTxFrequency() / 1000000U, freqHz, sign, offset / 1000000U, offset % 1000000U);

		memcpy(data + 84U, freq, 23U);
	}

	// EOD & CRC
	data[127U] = 0x03U;
	data[128U] = CCRC::addCRC(data, 128U);

	// and encode the reply
	CBuffer Data;
	Data.Set(data, 129U);
	ok = EncodeAndSendWiresxPacket(Ip, Data, WiresxInfo);

	// done
	m_seqNo++;
	return ok;
}

bool CWiresxCmdHandler::ReplyToWiresxAllReqPacket(const CIp &Ip, const CWiresxInfo &WiresxInfo, int Start)
{
	bool ok = false;
	uint8_t ALL_RESP[]  = {0x5DU, 0x46U, 0x5FU, 0x29U};
	uint8_t data[1100U];

	// fill data buffer
	memset(data, 0x00U, 1100U);
	// seq no
	data[0U] = m_seqNo;
	// command
	memcpy(data + 1U, ALL_RESP, 4U);
	data[5U] = '2';
	data[6U] = '1';
	// node info
	memcpy(data +  7U, WiresxInfo.GetId(), 5U);
	memcpy(data + 12U, WiresxInfo.GetNode(), 10U);

	// number of entries
	const std::string modules(g_Conf.GetString(g_Conf.j.modules.modules));
	uint NB_OF_MODULES = modules.size();
	uint total = NB_OF_MODULES;
	uint n = NB_OF_MODULES - Start;
	if (n > 20U)
		n = 20U;
	::sprintf((char*)(data + 22U), "%03u%03u", n, total);
	data[28U] = 0x0DU;

	// entries
	uint offset = 29U;
	for ( uint i = 0; i < n; i++ )
	{
		char item[16U];
		// module A == 0
		char RoomMod = modules[i + Start];
		int RoomId = RoomMod - 'A';

		// prepare
		memset(data + offset, ' ', 50U);
		data[offset + 0U] = '5';

		// refl->m_id
		::sprintf(item, "%05d", 4001U + RoomId);
		memcpy(data + offset + 1U, item, 5U);
		// refl->name
		memset(item, ' ', 16U);
		memcpy(item, "MODULE", 6U); // K2IE fix for U/C only radios
		item[7] = RoomMod;
		memcpy(data + offset + 6U, item, 16U);
		// refl->count
		::sprintf(item, "%03d", RoomId + 1);
		memcpy(data + offset + 22U, item, 3U);
		// other
		memset(data + offset + 25U, ' ', 10U);
		// refl->m_desc
		memcpy(data + offset + 35U, "Description   ", 14U);
		data[offset + 49U] = 0x0DU;
		// next
		offset += 50U;
	}

	// the following is a workaround for FT2D which
	// do not accept less than 20 items frames.
	// first send a 'patched' frame
	if ( n < 20 )
	{
		// FT2D workaround
		uint offset2 = offset;
		// patch the remaining
		uint k = 1029U - offset2;
		memset(data+offset2, ' ', k);
		offset2 += k;

		// EOD + CRC
		data[offset2 + 0U] = 0x03U;
		data[offset2 + 1U] = CCRC::addCRC(data, offset2 + 1U);
		offset2 += 2U;

		// and encode the reply
		CBuffer Data;
		Data.Set(data, offset2 + 2U);
		ok = EncodeAndSendWiresxPacket(Ip, Data, WiresxInfo);
	}


	// and next repeat with normal frame
	{
		// EOD + CRC
		data[offset + 0U] = 0x03U;
		data[offset + 1U] = CCRC::addCRC(data, offset + 1U);
		offset += 2U;

		// patch the remaining
		//uint k = 1031U - offset;
		//memset(data+offset, ' ', k);
		//offset += k;

		// and encode the reply
		CBuffer Data;
		Data.Set(data, offset + 2U);
		ok = EncodeAndSendWiresxPacket(Ip, Data, WiresxInfo);
	}


	// done
	m_seqNo++;
	return ok;
}

bool CWiresxCmdHandler::ReplyToWiresxConnReqPacket(const CIp &Ip, const CWiresxInfo &WiresxInfo, char Module)
{
	uint8_t CONN_RESP[] = {0x5DU, 0x41U, 0x5FU, 0x26U};
	bool ok = false;
	uint8_t data[110U];
	uint RoomId;

	// linked room
	// Module A == 0
	RoomId = (uint8_t)(Module - 'A');

	// prepare buffer
	memset(data, 0x00U, 110U);
	memset(data, ' ', 90U);

	// seq no
	data[0U] = m_seqNo;
	// command
	memcpy(data + 1U, CONN_RESP, 4U);
	// node info
	memcpy(data +  5U, WiresxInfo.GetId(), 5U);
	memcpy(data + 10U, WiresxInfo.GetNode(), 10U);
	memcpy(data + 20U, WiresxInfo.GetName(), 14U);
	data[34U] = '1';
	data[35U] = '5';
	// entry info
	{
		char item[16U];

		// refl->m_id
		::sprintf(item, "%05d", 4001U + RoomId);
		memcpy(data + 36U, item, 5U);
		// refl->name
		memset(item, ' ', 16U);
		memcpy(item, "MODULE", 6U); // K2IE fix for U/C only radios
		item[7] = 'A' + RoomId;
		memcpy(data + 41U, item, 16U);
		// refl->count
		::sprintf(item, "%03d", RoomId + 1);
		memcpy(data + 57U, item, 3U);
		// refl->m_desc
		memcpy(data + 70U, "Description   ", 14U);
	}
	data[84U] = '0';
	data[85U] = '0';
	data[86U] = '0';
	data[87U] = '0';
	data[88U] = '0';

	// EOD + CRC
	data[89U] = 0x03U;
	data[90U] = CCRC::addCRC(data, 90U);

	// and encode the reply
	CBuffer Data;
	Data.Set(data, 91U);
	ok = EncodeAndSendWiresxPacket(Ip, Data, WiresxInfo);

	// done
	m_seqNo++;
	return ok;
}

bool CWiresxCmdHandler::ReplyToWiresxDiscReqPacket(const CIp &Ip, const CWiresxInfo &WiresxInfo)
{
	uint8_t DISC_RESP[] = {0x5DU, 0x41U, 0x5FU, 0x26U};
	bool ok = false;
	uint8_t data[110U];

	// prepare buffer
	memset(data, 0x00U, 110U);
	memset(data, ' ', 90U);

	// seq no
	data[0U] = m_seqNo;
	// command
	memcpy(data + 1U, DISC_RESP, 4U);
	// node info
	memcpy(data +  5U, WiresxInfo.GetId(), 5U);
	memcpy(data + 10U, WiresxInfo.GetNode(), 10U);
	memcpy(data + 20U, WiresxInfo.GetName(), 14U);
	// data
	data[34U] = '1';
	data[35U] = '2';

	data[57U] = '0';
	data[58U] = '0';
	data[59U] = '0';

	// EOD + CRC
	data[89U] = 0x03U;
	data[90U] = CCRC::addCRC(data, 90U);

	// and encode the reply
	CBuffer Data;
	Data.Set(data, 91U);
	ok = EncodeAndSendWiresxPacket(Ip, Data, WiresxInfo);

	// done
	m_seqNo++;
	return ok;
}



////////////////////////////////////////////////////////////////////////////////////////
///// packet encoding helpers

bool CWiresxCmdHandler::EncodeAndSendWiresxPacket(const CIp &Ip, const CBuffer &DataOrg, const CWiresxInfo &WiresxInfo)
{
	uint8_t DEFAULT_FICH[] = {0x20U, 0x00U, 0x01U, 0x00U};
	uint8_t NET_HEADER[] = "YSFD                    ALL      ";
	CYSFFICH fich;
	CYSFPayload payload;
	uint8_t buffer[200U];


	CBuffer Data(DataOrg);

	// seq no
	uint8_t seqNo = 0U;

	// calculate bt and adjust length
	uint length = (uint)Data.size();
	uint8_t bt = 0;
	if (length > 260U)
	{
		bt = 1U;
		bt += (length - 260U) / 259U;
		length += bt;
	}
	if (length > 20U)
	{
		uint blocks = (length - 20U) / 40U;
		if ((length % 40U) > 0U)
			blocks++;
		length = blocks * 40U + 20U;
	}
	else
	{
		length = 20U;
	}
	if ( length > (uint)Data.size() )
	{
		Data.Append((uint8_t)0x20U, (int)(length - (uint)Data.size()));
	}

	// ft
	uint8_t ft = WiresxCalcFt(length, 0U);

	// Write the header
	{
		//header
		memcpy(buffer, NET_HEADER, 34U);
		memcpy(buffer + 4U, WiresxInfo.GetCallsign(), 10U);
		memcpy(buffer + 14U, WiresxInfo.GetNode(), 10U);
		// sync
		memcpy(buffer + 35U, YSF_SYNC_BYTES, YSF_SYNC_LENGTH_BYTES);
		// Fich
		fich.load(DEFAULT_FICH);
		fich.setFI(YSF_FI_HEADER);
		fich.setBT(bt);
		fich.setFT(ft);
		fich.encode(buffer + 35U + 5U);
		// payload
		payload.writeDataFRModeData1(WiresxInfo.GetCsd1(), buffer + 35U);
		payload.writeDataFRModeData2(WiresxInfo.GetCsd2(), buffer + 35U);
		// seqno
		buffer[34U] = seqNo;
		seqNo += 2U;
		// and post it
		SendPacket(Ip, buffer);
	}

	// write the payload
	fich.setFI(YSF_FI_COMMUNICATIONS);
	uint offset = 0;
	for ( uint8_t bn = 0; bn <= bt; bn++ )
	{
		for ( uint8_t fn = 0; fn <= ft; fn++ )
		{
			// fich
			fich.setFT(ft);
			fich.setFN(fn);
			fich.setBT(bt);
			fich.setBN(bn);
			fich.encode(buffer + 35U + 5U);
			// seq no
			buffer[34U] = seqNo;
			seqNo += 2U;
			// data
			//if ( (bn == 0) && (fn == 0) )
			if ( fn == 0 )
			{
				payload.writeDataFRModeData1(WiresxInfo.GetCsd1(), buffer + 35);
				payload.writeDataFRModeData2(WiresxInfo.GetCsd2(), buffer + 35);
			}
			//else if ( (bn == 0) && (fn == 1) )
			else if ( fn == 1 )
			{
				if ( bn == 0 )
				{
					payload.writeDataFRModeData1(WiresxInfo.GetCsd3(), buffer + 35);
					payload.writeDataFRModeData2(Data.data() + offset, buffer + 35);
					offset += 20;
				}
				else
				{
					uint8_t temp[20U];
					temp[0U] = 0x00U;
					memcpy(temp + 1U, Data.data() + offset, 19U);
					payload.writeDataFRModeData2(temp, buffer + 35U);
					offset += 19U;
				}
			}
			else
			{
				payload.writeDataFRModeData1(Data.data() + offset, buffer + 35);
				offset += 20;
				payload.writeDataFRModeData2(Data.data() + offset, buffer + 35);
				offset += 20;
			}
			// and post it
			SendPacket(Ip, buffer);
			// and some delay before next packet
			std::this_thread::sleep_for(std::chrono::milliseconds(100));
		}
	}

	// Write the trailer
	{
		// fich
		fich.setFI(YSF_FI_TERMINATOR);
		fich.setFN(ft);
		fich.setBN(bt);
		fich.encode(buffer + 35U + 5U);
		// payload
		payload.writeDataFRModeData1(WiresxInfo.GetCsd1(), buffer + 35U);
		payload.writeDataFRModeData2(WiresxInfo.GetCsd2(), buffer + 35U);
		// seq no
		buffer[34U] = seqNo | 0x01U;
		// and post it
		SendPacket(Ip, buffer);
	}

	// done
	return true;
}


uint8_t CWiresxCmdHandler::WiresxCalcFt(uint length, uint offset) const
{
	length -= offset;
	if (length > 220U) return 7U;
	if (length > 180U) return 6U;
	if (length > 140U) return 5U;
	if (length > 100U) return 4U;
	if (length > 60U)  return 3U;
	if (length > 20U)  return 2U;
	return 1U;
}


void CWiresxCmdHandler::SendPacket(const CIp &Ip, uint8_t *Buffer)
{
	//CBuffer packet(Buffer, 155);
	//DebugTestDecodePacket(packet);

	// and push in queue
	m_PacketQueue.Lock();
	{
		m_PacketQueue.push(CWiresxPacket(CBuffer(Buffer, 155), Ip));
	}
	m_PacketQueue.Unlock();
}


////////////////////////////////////////////////////////////////////////////////////////
// debug

#ifdef DEBUG_DUMPFILE
bool CWiresxCmdHandler::DebugTestDecodePacket(const CBuffer &Buffer)
{
	uint8_t tag[] = { 'Y','S','F','D' };
	static uint8_t command[4098];
	static int len;
	CYSFFICH Fich;
	CYSFPayload payload;
	CBuffer dump;
	bool valid = false;

	if ( (Buffer.size() == 155) && (Buffer.Compare(tag, sizeof(tag)) == 0) )
	{
		// decode YSH fich
		if ( Fich.decode(&(Buffer.data()[40])) )
		{
			std::cout << (int)Fich.getDT() << ","
					  << (int)Fich.getFI() << ","
					  << (int)Fich.getBN() << ","
					  << (int)Fich.getBT() << ","
					  << (int)Fich.getFN() << ","
					  << (int)Fich.getFT() << " : ";

			switch ( Fich.getFI() )
			{
			case YSF_FI_HEADER:
				len = 0;
				memset(command, 0x00, sizeof(command));
				std::cout << "Header" << std::endl;
				break;
			case YSF_FI_TERMINATOR:
				std::cout << "Trailer" << std::endl;
				std::cout << "length of payload : " << len << std::endl;
				dump.Set(command, len);
				dump.DebugDump(g_Refl.m_DebugFile);
				dump.DebugDumpAscii(g_Refl.m_DebugFile);
				break;
			case YSF_FI_COMMUNICATIONS:
				if ( Fich.getDT() == YSF_DT_DATA_FR_MODE )
				{
					valid = payload.readDataFRModeData1(&(Buffer.data()[35]), command + len);
					len += 20;
					valid &= payload.readDataFRModeData2(&(Buffer.data()[35]), command + len);
					len += 20;
					std::cout << "decoded ok" << std::endl;
				}
				break;
			}
		}
		else
		{
			std::cout << "invalid fich in packet" << std::endl;
		}
	}
	else
	{
		std::cout << "invalid size packet" << std::endl;
	}
	return valid;
}
#endif