/**
* @file  cusdr_protocol2_io.cpp
* @brief openHPSDR Protocol 2 transport helper
*/

#define LOG_DATAIO

#include "cusdr_protocol2_io.h"

#ifdef LOG_DATAIO
#   define PROTOCOL2_DEBUG qDebug().nospace() << "Protocol2::\t"
#else
#   define PROTOCOL2_DEBUG nullDebug()
#endif


namespace {

static const quint16 kProtocol2DdcSpecificPort = 1025;
static const quint16 kProtocol2HighPriorityToPcPort = 1025;
static const quint16 kProtocol2DdcAudioPort = 1028;
static const quint16 kProtocol2DucIqPort = 1029;
static const quint16 kProtocol2DdcBasePort = 1035;
static const int kProtocol2GeneralPacketSize = 60;
static const int kProtocol2HighPriorityPacketSize = 1444;
static const int kProtocol2StatusPacketMinSize = 31;

quint32 alexFilterWordForFrequency(Settings *set, long frequency, int currentBand) {

	if (!set || !set->getAlexPresence())
		return 0;

	quint32 alexWord = 0;
	const quint16 alexConfig = set->getAlexConfig();
	const QList<long> hpfLo = set->getHPFLoFrequencies();
	const QList<long> hpfHi = set->getHPFHiFrequencies();
	const QList<long> lpfLo = set->getLPFLoFrequencies();
	const QList<long> lpfHi = set->getLPFHiFrequencies();
	const QList<int> alexStates = set->getAlexStates();

	if (currentBand >= 0 && currentBand < alexStates.size()) {

		const int rxAntenna = alexStates.at(currentBand) & 0x03;
		if (rxAntenna >= 1 && rxAntenna <= 3)
			alexWord |= (1u << (23 + rxAntenna));
	}

	if ((alexConfig & 0x02) != 0) {
		alexWord |= (1u << 12);
	}
	else if (hpfLo.size() >= 6 && hpfHi.size() >= 6) {

		if (frequency >= hpfLo.at(0) && frequency <= hpfHi.at(0))
			alexWord |= (1u << 6);
		else if (frequency >= hpfLo.at(1) && frequency <= hpfHi.at(1))
			alexWord |= (1u << 5);
		else if (frequency >= hpfLo.at(2) && frequency <= hpfHi.at(2))
			alexWord |= (1u << 4);
		else if (frequency >= hpfLo.at(3) && frequency <= hpfHi.at(3))
			alexWord |= (1u << 1);
		else if (frequency >= hpfLo.at(4) && frequency <= hpfHi.at(4))
			alexWord |= (1u << 2);
		else if (frequency >= hpfLo.at(5) && frequency <= hpfHi.at(5)) {

			if ((alexConfig & 0x04) != 0)
				alexWord |= (1u << 3);
			else
				alexWord |= (1u << 12);
		}
		else {
			alexWord |= (1u << 12);
		}
	}

	if (lpfLo.size() >= 7 && lpfHi.size() >= 7) {

		if (frequency >= lpfLo.at(0) && frequency <= lpfHi.at(0))
			alexWord |= (1u << 23);
		else if (frequency >= lpfLo.at(1) && frequency <= lpfHi.at(1))
			alexWord |= (1u << 22);
		else if (frequency >= lpfLo.at(2) && frequency <= lpfHi.at(2))
			alexWord |= (1u << 21);
		else if (frequency >= lpfLo.at(3) && frequency <= lpfHi.at(3))
			alexWord |= (1u << 20);
		else if (frequency >= lpfLo.at(4) && frequency <= lpfHi.at(4))
			alexWord |= (1u << 31);
		else if (frequency >= lpfLo.at(5) && frequency <= lpfHi.at(5))
			alexWord |= (1u << 30);
		else
			alexWord |= (1u << 29);
	}

	return alexWord;
}

}


Protocol2DataPath::Protocol2DataPath(Settings *settings, THPSDRParameter *ioData, QObject *parent)
	: QObject(parent)
	, set(settings)
	, io(ioData)
	, m_commandSocket(0)
	, m_statusSocket(0)
	, m_heartbeatTimer(0)
	, m_running(false)
	, m_generalSequence(0)
	, m_ddcSequence(0)
	, m_highPrioritySequence(0)
{
	m_legacyControlBytes = QByteArray(5, 0x00);
}

Protocol2DataPath::~Protocol2DataPath() {
	stop();
}

bool Protocol2DataPath::isActive() const {
	return set->getCurrentMetisCard().protocolVersion >= 2;
}

bool Protocol2DataPath::initSockets() {

	if (!isActive())
		return false;

	closeSockets();

	const QHostAddress localAddress(set->getHPSDRDeviceLocalAddr());
	const quint16 discoveryPort = set->getMetisPort();
	const int receivers = qMax(1, set->getNumberOfReceivers());

	m_commandSocket = new QUdpSocket(this);
	if (!m_commandSocket->bind(localAddress, discoveryPort, QUdpSocket::DontShareAddress)) {
		PROTOCOL2_DEBUG << "command socket bind failed on port " << discoveryPort;
		closeSockets();
		return false;
	}

	m_statusSocket = new QUdpSocket(this);
	if (!m_statusSocket->bind(localAddress, kProtocol2HighPriorityToPcPort, QUdpSocket::DontShareAddress)) {
		PROTOCOL2_DEBUG << "status socket bind failed on port " << kProtocol2HighPriorityToPcPort;
		closeSockets();
		return false;
	}
	CHECKED_CONNECT(
		m_statusSocket,
		SIGNAL(readyRead()),
		this,
		SLOT(readStatusData()));

	m_ddcSampleBuffers.clear();
	m_ddcSockets.clear();
	for (int rx = 0; rx < receivers; ++rx) {

		QUdpSocket *socket = new QUdpSocket(this);
		const quint16 port = kProtocol2DdcBasePort + rx;
		if (!socket->bind(localAddress, port, QUdpSocket::DontShareAddress)) {
			PROTOCOL2_DEBUG << "DDC socket bind failed on port " << port;
			delete socket;
			closeSockets();
			return false;
		}

		CHECKED_CONNECT(
			socket,
			SIGNAL(readyRead()),
			this,
			SLOT(readDdcData()));

		m_ddcSockets.append(socket);
		m_ddcSampleBuffers.append(QList<Protocol2IQSample>());
	}

	m_heartbeatTimer = new QTimer(this);
	m_heartbeatTimer->setInterval(100);
	CHECKED_CONNECT(
		m_heartbeatTimer,
		SIGNAL(timeout()),
		this,
		SLOT(sendHeartbeat()));

	m_partialLegacyFrame.clear();
	m_partialLegacyFrame.reserve(BUFFER_SIZE);
	m_legacyControlBytes.fill(0x00, 5);
	m_running = false;

	PROTOCOL2_DEBUG << "sockets initialized for " << receivers << " receiver(s).";
	return true;
}

void Protocol2DataPath::stop() {

	m_running = false;

	if (m_heartbeatTimer)
		m_heartbeatTimer->stop();

	if (m_commandSocket)
		sendHighPriorityPacket(false);

	closeSockets();
}

void Protocol2DataPath::sendInitFramesToNetworkDevice(int rx) {
	Q_UNUSED(rx)
}

void Protocol2DataPath::networkDeviceStartStop(char value) {

	if (!m_commandSocket)
		return;

	if (value == 0) {
		m_running = false;
		if (m_heartbeatTimer)
			m_heartbeatTimer->stop();
		sendHighPriorityPacket(false);
		return;
	}

	sendGeneralPacket();
	sendDdcSpecificPacket();
	sendHighPriorityPacket(true);

	m_running = true;
	if (m_heartbeatTimer)
		m_heartbeatTimer->start();
}

void Protocol2DataPath::readStatusData() {

	while (m_statusSocket && m_statusSocket->hasPendingDatagrams()) {

		m_statusDatagram.resize(m_statusSocket->pendingDatagramSize());
		m_statusSocket->readDatagram(m_statusDatagram.data(), m_statusDatagram.size());

		if (m_statusDatagram.size() >= kProtocol2StatusPacketMinSize)
			updateLegacyControlBytes(m_statusDatagram);
	}
}

void Protocol2DataPath::readDdcData() {

	QUdpSocket *socket = qobject_cast<QUdpSocket *>(sender());
	const int rx = m_ddcSockets.indexOf(socket);

	if (rx < 0)
		return;

	while (socket->hasPendingDatagrams()) {

		QByteArray datagram;
		datagram.resize(socket->pendingDatagramSize());
		socket->readDatagram(datagram.data(), datagram.size());

		if (datagram.size() < 16)
			continue;

		const int bitsPerSample = ((quint8)datagram.at(12) << 8) | (quint8)datagram.at(13);
		const int sampleCount = ((quint8)datagram.at(14) << 8) | (quint8)datagram.at(15);
		const int payloadBytes = 16 + sampleCount * 6;

		if (bitsPerSample != 24 || datagram.size() < payloadBytes)
			continue;

		for (int i = 0; i < sampleCount; ++i) {

			const char *sample = datagram.constData() + 16 + i * 6;
			Protocol2IQSample iqSample;
			iqSample.i = read24BitSample(sample);
			iqSample.q = read24BitSample(sample + 3);
			m_ddcSampleBuffers[rx].append(iqSample);
		}
	}

	tryProduceLegacyFrames();
}

void Protocol2DataPath::sendHeartbeat() {

	if (!m_running)
		return;

	sendHighPriorityPacket(true);
}

void Protocol2DataPath::closeSockets() {

	if (m_heartbeatTimer) {
		delete m_heartbeatTimer;
		m_heartbeatTimer = 0;
	}

	foreach (QUdpSocket *socket, m_ddcSockets) {
		socket->close();
		delete socket;
	}
	m_ddcSockets.clear();
	m_ddcSampleBuffers.clear();

	if (m_statusSocket) {
		m_statusSocket->close();
		delete m_statusSocket;
		m_statusSocket = 0;
	}

	if (m_commandSocket) {
		m_commandSocket->close();
		delete m_commandSocket;
		m_commandSocket = 0;
	}

	m_partialLegacyFrame.clear();
}

void Protocol2DataPath::sendGeneralPacket() {

	QByteArray datagram(kProtocol2GeneralPacketSize, 0x00);

	datagram[0] = (m_generalSequence >> 24) & 0xFF;
	datagram[1] = (m_generalSequence >> 16) & 0xFF;
	datagram[2] = (m_generalSequence >> 8) & 0xFF;
	datagram[3] = m_generalSequence & 0xFF;
	datagram[4] = 0x00;
	datagram[5] = (kProtocol2DdcSpecificPort >> 8) & 0xFF;
	datagram[6] = kProtocol2DdcSpecificPort & 0xFF;
	datagram[7] = (kProtocol2DucIqPort >> 8) & 0xFF;
	datagram[8] = kProtocol2DucIqPort & 0xFF;
	datagram[9] = (HIGH_PRIORITY_FROM_HOST_PORT >> 8) & 0xFF;
	datagram[10] = HIGH_PRIORITY_FROM_HOST_PORT & 0xFF;
	datagram[11] = (kProtocol2HighPriorityToPcPort >> 8) & 0xFF;
	datagram[12] = kProtocol2HighPriorityToPcPort & 0xFF;
	datagram[13] = (kProtocol2DdcAudioPort >> 8) & 0xFF;
	datagram[14] = kProtocol2DdcAudioPort & 0xFF;
	datagram[15] = (kProtocol2DucIqPort >> 8) & 0xFF;
	datagram[16] = kProtocol2DucIqPort & 0xFF;
	datagram[17] = (kProtocol2DdcBasePort >> 8) & 0xFF;
	datagram[18] = kProtocol2DdcBasePort & 0xFF;
	datagram[21] = (HIGH_PRIORITY_FROM_HOST_PORT >> 8) & 0xFF;
	datagram[22] = HIGH_PRIORITY_FROM_HOST_PORT & 0xFF;
	datagram[24] = 0x02;
	datagram[26] = 0x10;
	datagram[27] = 20;
	datagram[28] = 32;
	datagram[59] = set->getAlexPresence() ? 0x01 : 0x00;

	if (m_commandSocket->writeDatagram(datagram, set->getCurrentMetisCard().ip_address, DEVICE_PORT) >= 0)
		++m_generalSequence;
	else
		PROTOCOL2_DEBUG << "general packet write failed.";
}

void Protocol2DataPath::sendDdcSpecificPacket() {

	const int receivers = qMax(1, set->getNumberOfReceivers());
	QByteArray datagram(17 + receivers * 6, 0x00);

	datagram[0] = (m_ddcSequence >> 24) & 0xFF;
	datagram[1] = (m_ddcSequence >> 16) & 0xFF;
	datagram[2] = (m_ddcSequence >> 8) & 0xFF;
	datagram[3] = m_ddcSequence & 0xFF;
	datagram[4] = 0x01;
	datagram[5] = io->ccTx.dither ? 0x01 : 0x00;
	datagram[6] = io->ccTx.random ? 0x01 : 0x00;

	for (int rx = 0; rx < receivers; ++rx) {

		datagram[7 + rx / 8] = datagram.at(7 + rx / 8) | (1 << (rx % 8));

		const int offset = 17 + rx * 6;
		datagram[offset + 0] = 0x00;
		datagram[offset + 1] = (io->samplerate >> 8) & 0xFF;
		datagram[offset + 2] = io->samplerate & 0xFF;
		datagram[offset + 3] = 0x00;
		datagram[offset + 4] = 0x00;
		datagram[offset + 5] = 24;
	}

	if (m_commandSocket->writeDatagram(datagram, set->getCurrentMetisCard().ip_address, kProtocol2DdcSpecificPort) >= 0)
		++m_ddcSequence;
	else
		PROTOCOL2_DEBUG << "DDC specific packet write failed.";
}

void Protocol2DataPath::sendHighPriorityPacket(bool run) {

	QByteArray datagram(kProtocol2HighPriorityPacketSize, 0x00);
	const QList<long> frequencies = set->getCtrFrequencies();
	const int receivers = qMax(1, set->getNumberOfReceivers());
	const int currentReceiver = set->getCurrentReceiver();
	const int currentBand = (int)set->getCurrentHamBand(currentReceiver);
	const long currentVfoFrequency = set->getVfoFrequency(currentReceiver);
	const quint32 alexWord = alexFilterWordForFrequency(set, currentVfoFrequency, currentBand);

	datagram[0] = (m_highPrioritySequence >> 24) & 0xFF;
	datagram[1] = (m_highPrioritySequence >> 16) & 0xFF;
	datagram[2] = (m_highPrioritySequence >> 8) & 0xFF;
	datagram[3] = m_highPrioritySequence & 0xFF;
	datagram[4] = run ? 0x01 : 0x00;

	for (int rx = 0; rx < receivers; ++rx) {

		const quint32 encodedFrequency = encodeFrequency(frequencies.value(rx, frequencies.value(0, 0)));
		const int offset = 9 + rx * 4;

		datagram[offset + 0] = (encodedFrequency >> 24) & 0xFF;
		datagram[offset + 1] = (encodedFrequency >> 16) & 0xFF;
		datagram[offset + 2] = (encodedFrequency >> 8) & 0xFF;
		datagram[offset + 3] = encodedFrequency & 0xFF;
	}

	if (set->getAlexPresence()) {

		datagram[1432] = (alexWord >> 24) & 0xFF;
		datagram[1433] = (alexWord >> 16) & 0xFF;
		datagram[1434] = (alexWord >> 8) & 0xFF;
		datagram[1435] = alexWord & 0xFF;
	}

	if (m_commandSocket->writeDatagram(datagram, set->getCurrentMetisCard().ip_address, HIGH_PRIORITY_FROM_HOST_PORT) >= 0)
		++m_highPrioritySequence;
	else
		PROTOCOL2_DEBUG << "high priority packet write failed.";
}

void Protocol2DataPath::updateLegacyControlBytes(const QByteArray &statusDatagram) {

	const uchar statusBits = (uchar)statusDatagram.at(4);
	const uchar overloadBits = (uchar)statusDatagram.at(5);

	char control0 = 0x00;
	if (statusBits & 0x01) control0 |= 0x01;
	if (statusBits & 0x04) control0 |= 0x02;
	if (statusBits & 0x02) control0 |= 0x04;

	m_legacyControlBytes[0] = control0;
	m_legacyControlBytes[1] = (overloadBits & 0x01) ? 0x01 : 0x00;
	m_legacyControlBytes[2] = 0x00;
	m_legacyControlBytes[3] = 0x00;
	m_legacyControlBytes[4] = (char)(set->getCurrentMetisCard().firmwareVersion & 0xFF);
}

void Protocol2DataPath::tryProduceLegacyFrames() {

	const int receivers = qMax(1, set->getNumberOfReceivers());
	const int halfFrameSamples = samplesPerLegacyHalfFrame();

	if (halfFrameSamples <= 0)
		return;

	while (true) {

		bool hasEnoughData = true;
		for (int rx = 0; rx < receivers; ++rx) {
			if (rx >= m_ddcSampleBuffers.size() || m_ddcSampleBuffers[rx].size() < halfFrameSamples) {
				hasEnoughData = false;
				break;
			}
		}

		if (!hasEnoughData)
			break;

		QByteArray halfFrame(BUFFER_SIZE / 2, 0x00);
		int pos = 0;
		halfFrame[pos++] = SYNC;
		halfFrame[pos++] = SYNC;
		halfFrame[pos++] = SYNC;
		for (int i = 0; i < m_legacyControlBytes.size(); ++i)
			halfFrame[pos++] = m_legacyControlBytes.at(i);

		for (int sampleIndex = 0; sampleIndex < halfFrameSamples; ++sampleIndex) {
			for (int rx = 0; rx < receivers; ++rx) {

				const Protocol2IQSample sample = m_ddcSampleBuffers[rx].takeFirst();
				halfFrame[pos++] = (sample.i >> 16) & 0xFF;
				halfFrame[pos++] = (sample.i >> 8) & 0xFF;
				halfFrame[pos++] = sample.i & 0xFF;
				halfFrame[pos++] = (sample.q >> 16) & 0xFF;
				halfFrame[pos++] = (sample.q >> 8) & 0xFF;
				halfFrame[pos++] = sample.q & 0xFF;
			}

			halfFrame[pos++] = 0x00;
			halfFrame[pos++] = 0x00;
		}

		m_partialLegacyFrame += halfFrame;
		if (m_partialLegacyFrame.size() == BUFFER_SIZE) {
			if (!io->iq_queue.isFull())
				io->iq_queue.enqueue(m_partialLegacyFrame);
			m_partialLegacyFrame.clear();
		}
	}
}

int Protocol2DataPath::samplesPerLegacyHalfFrame() const {

	const int receivers = qMax(1, set->getNumberOfReceivers());
	const int bytesPerSample = receivers * 6 + 2;

	return (BUFFER_SIZE / 2 - 8) / bytesPerSample;
}

quint32 Protocol2DataPath::encodeFrequency(long frequency) const {
	return (quint32)frequency;
}

qint32 Protocol2DataPath::read24BitSample(const char *data) const {

	qint32 value = ((qint32)(quint8)data[0] << 16) |
				   ((qint32)(quint8)data[1] << 8) |
				   (qint32)(quint8)data[2];

	if (value & 0x00800000)
		value |= 0xFF000000;

	return value;
}