///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2020 Edouard Griffiths, F4EXB                                   //
//                                                                               //
// 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 as 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 V3 for more details.                               //
//                                                                               //
// You should have received a copy of the GNU General Public License             //
// along with this program. If not, see <http://www.gnu.org/licenses/>.          //
///////////////////////////////////////////////////////////////////////////////////

#include "dsp/samplemififo.h"
#include "metismisoudphandler.h"

MESSAGE_CLASS_DEFINITION(MetisMISOUDPHandler::MsgStartStop, Message)

MetisMISOUDPHandler::MetisMISOUDPHandler(SampleMIFifo *sampleFifo, DeviceAPI *deviceAPI) :
    m_deviceAPI(deviceAPI),
	m_socket(nullptr),
	m_metisAddress(QHostAddress::LocalHost),
	m_metisPort(9090),
    m_running(false),
	m_dataConnected(false),
	m_sampleFifo(sampleFifo),
    m_sampleCount(0),
	m_messageQueueToGUI(nullptr),
    m_mutex(QMutex::Recursive),
    m_commandBase(0),
    m_receiveSequence(0),
    m_receiveSequenceError(0)
{
    setNbReceivers(m_settings.m_nbReceivers);

    for (unsigned int i = 0; i < MetisMISOSettings::m_maxReceivers; i++) {
        m_convertBuffer[i].resize(1024, Sample{0,0});
    }

    connect(&m_inputMessageQueue, SIGNAL(messageEnqueued()), this, SLOT(handleMessages()));
}

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

void MetisMISOUDPHandler::start()
{
	qDebug("MetisMISOUDPHandler::start");

    m_rxFrame = 0;
    m_txFrame = 0;
    m_sendSequence = -1;
    m_offset = 8;
    m_receiveSequence = 0;

	if (m_running) {
	    return;
	}

    if (!m_dataConnected)
	{
        if (m_socket.bind(QHostAddress::AnyIPv4, 10001, QUdpSocket::ShareAddress))
		{
			qDebug("MetisMISOUDPHandler::start: bind host socket OK");
            connect(&m_socket, SIGNAL(readyRead()), this, SLOT(dataReadyRead()));
			m_dataConnected = true;
		}
		else
		{
			qWarning("MetisMISOUDPHandler::start: cannot bind host socket");
			m_dataConnected = false;
            return;
		}
	}

    // Start Metis
    unsigned char buffer[64];

    buffer[0] = (unsigned char) 0xEF;
    buffer[1] = (unsigned char) 0XFE;
    buffer[2] = (unsigned char) 0x04;
    buffer[3] = (unsigned char) 0x01;
    std::fill(&buffer[4], &buffer[64], 0);

    if (m_socket.writeDatagram((const char*) buffer, sizeof(buffer), m_metisAddress, m_metisPort) < 0) 
    {
        qDebug() << "MetisMISOUDPHandler::start: writeDatagram start command failed " << m_socket.errorString();
        return;
    }
    else
    {
        qDebug() << "MetisMISOUDPHandler::start: writeDatagram start command" << m_metisAddress.toString() << ":" << m_metisPort;
    }

    m_socket.flush();    

    // send 2 frames with control data

    sendNullBuffer();
    sendNullBuffer(); // TODO: on the next send frequencies

    m_running = true;
}

void MetisMISOUDPHandler::stop()
{
	qDebug("MetisMISOUDPHandler::stop");

	if (!m_running) {
	    return;
	}

    // stop Metis
    unsigned char buffer[64];

    buffer[0] = (unsigned char) 0xEF;
    buffer[1] = (unsigned char) 0XFE;
    buffer[2] = (unsigned char) 0x04;
    buffer[3] = (unsigned char) 0x00;
    std::fill(&buffer[4], &buffer[64], 0);

    if (m_dataConnected)
    {
	    disconnect(&m_socket, SIGNAL(readyRead()), this, SLOT(dataReadyRead()));
		m_dataConnected = false;
	}


    if (m_socket.writeDatagram((const char*)buffer, sizeof(buffer), m_metisAddress,m_metisPort) < 0) 
    {
        qDebug() << "MetisMISOUDPHandler::stop: writeDatagram failed " << m_socket.errorString();
        return;
    }
    else
    {
        qDebug()<<"MetisMISOUDPHandler::stop: writeDatagram stop command" << m_metisAddress.toString() << ":" << m_metisPort;    
    }
    
    m_socket.flush();   
    m_socket.close(); 
	m_running = false;
}

void MetisMISOUDPHandler::setNbReceivers(unsigned int nbReceivers)
{
    m_nbReceivers = nbReceivers;

    switch(m_nbReceivers) 
    {
    case 1: m_bMax = 512-0; break;
    case 2: m_bMax = 512-0; break;
    case 3: m_bMax = 512-4; break;
    case 4: m_bMax = 512-10; break;
    case 5: m_bMax = 512-24; break;
    case 6: m_bMax = 512-10; break;
    case 7: m_bMax = 512-20; break;
    case 8: m_bMax = 512-4; break;
    }

    for (unsigned int i = 0; i < MetisMISOSettings::m_maxReceivers; i++) {
        m_convertBuffer[i].resize(1024, Sample{0,0});
    }
}

void MetisMISOUDPHandler::applySettings(const MetisMISOSettings& settings)
{
    if (m_settings.m_nbReceivers != settings.m_nbReceivers)
    {
        QMutexLocker mutexLocker(&m_mutex);
        int nbReceivers = settings.m_nbReceivers < 1 ? 
            1 : settings.m_nbReceivers > MetisMISOSettings::m_maxReceivers ? 
                MetisMISOSettings::m_maxReceivers : settings.m_nbReceivers;
        setNbReceivers(nbReceivers);
    }

    if (m_settings.m_log2Decim != settings.m_log2Decim)
    {
        QMutexLocker mutexLocker(&m_mutex);
        m_decimators.resetCounters();
    }

    m_settings = settings;
}

void MetisMISOUDPHandler::handleMessages()
{
    Message* message;

    while ((message = m_inputMessageQueue.pop()) != 0)
    {
        if (handleMessage(*message)) {
            delete message;
        }
    }
}

bool MetisMISOUDPHandler::handleMessage(const Message& message)
{
    if (MsgStartStop::match(message))
    {
        const MsgStartStop& cmd = (const MsgStartStop&) message;

        if (cmd.getStartStop()) {
            start();
        } else {
            stop();
        }

        return true;
    }
    else
    {
        return false;
    }
    
}

void MetisMISOUDPHandler::sendMetisBuffer(int ep, unsigned char* buffer)
{
    if (m_offset == 8) // header and first HPSDR frame
    {
        m_sendSequence++;
        m_outputBuffer[0] = 0xEF;
        m_outputBuffer[1] = 0xFE;
        m_outputBuffer[2] = 0x01;
        m_outputBuffer[3] = ep;
        m_outputBuffer[4] = (m_sendSequence>>24) & 0xFF;
        m_outputBuffer[5] = (m_sendSequence>>16) & 0xFF;
        m_outputBuffer[6] = (m_sendSequence>>8) & 0xFF;
        m_outputBuffer[7] = (m_sendSequence) & 0xFF;
        std::copy(buffer, buffer+512, &m_outputBuffer[m_offset]); // copy the buffer over
        m_offset = 520;
    } 
    else // second HPSDR frame and send
    {
        std::copy(buffer, buffer+512, &m_outputBuffer[m_offset]); // copy the buffer over
        m_offset = 8;

        // send the buffer
        if (m_socket.writeDatagram((const char*) m_outputBuffer, sizeof(m_outputBuffer), m_metisAddress, m_metisPort) < 0) 
        {
            qDebug() << "MetisMISOUDPHandler::sendMetisBuffer: writeDatagram failed " << m_socket.errorString();
            return;
        }

        m_socket.flush();
    }
}

void MetisMISOUDPHandler::dataReadyRead()
{
    QHostAddress metisAddress;
    quint16 metisPort;
    unsigned char receiveBuffer[1032];
    qint64 length;

    long sequence;

    if ((length = m_socket.readDatagram((char*) &receiveBuffer, (qint64) sizeof(receiveBuffer), &metisAddress, &metisPort)) != 1032) 
    {
        qDebug() << "MetisMISOUDPHandler::dataReadyRead: readDatagram failed " << m_socket.errorString();
        return;
    }

    sendNullBuffer(); // TODO: for now a null payload is sent
    sendNullBuffer();

    if (receiveBuffer[0] == 0xEF && receiveBuffer[1] == 0xFE) 
    {
        // valid frame
        switch(receiveBuffer[2]) 
        {
        case 1: // IQ data
            switch (receiveBuffer[3]) 
            {
            case 4: // EP4 data
                break;
            case 6: // EP6 data
                sequence = ((receiveBuffer[4] & 0xFF)<<24) + ((receiveBuffer[5] & 0xFF)<<16) + ((receiveBuffer[6] & 0xFF)<<8) +(receiveBuffer[7] & 0xFF);
                if (m_receiveSequence == 0) 
                {
                    m_receiveSequence = sequence;
                } 
                else 
                {
                    m_receiveSequence++;
                    
                    if (m_receiveSequence != sequence) 
                    {
                        //qDebug()<<"Sequence error: expected "<<receive_sequence<<" got "<<sequence;
                        m_receiveSequence = sequence;
                        m_receiveSequenceError++;
                    }
                }
                processIQBuffer(&receiveBuffer[8]);
                processIQBuffer(&receiveBuffer[520]);
                break;
            default:
                qDebug() << "MetisMISOUDPHandler::dataReadyRead: invalid EP" << receiveBuffer[3];
                break;
            }
            break;
        default:
            qDebug() << "MetisMISOUDPHandler::dataReadyRead: expected data packet (1) got " << receiveBuffer[2];
            break;
        }
    } 
    else 
    {
        qDebug() << "MetisMISOUDPHandler::dataReadyRead: expected EFFE";
    }
}

void MetisMISOUDPHandler::sendNullBuffer() 
{
    unsigned char buffer[512];
    
    if (m_txFrame % (m_settings.m_sampleRateIndex+1) == 0)
    {
        int commandIndex = 2*m_commandBase; // command rotation
    
        buffer[0] = (unsigned char) 0x7F;
        buffer[1] = (unsigned char) 0x7F;
        buffer[2] = (unsigned char) 0x7F;
        buffer[3] = commandIndex; // C0
    
        int commandValue = getCommandValue(commandIndex);
    
        buffer[4]= commandValue>>24; // C1
        buffer[5]= (commandValue>>16) & 0xFF; // C2
        buffer[6]= (commandValue>>8) & 0xFF; // C3
        buffer[7]= commandValue & 0xFF; // C4

        if (m_commandBase < 15) { // base count 0 to 16
            m_commandBase++;
        } else {
            m_commandBase = 0;
        }
        
        std::fill(&buffer[8], &buffer[512], 0);
        sendMetisBuffer(2, buffer);
    }

    m_txFrame++;
}

int MetisMISOUDPHandler::getCommandValue(int commandIndex)
{
    int c1 = 0, c2 = 0, c3 = 0, c4 = 0;
    
    if (commandIndex == 0)
    {
        c1 =  m_settings.m_sampleRateIndex & 0x03;
        c3 =  m_settings.m_preamp ? 0x04 : 0;
        c3 += m_settings.m_dither ? 0x08 : 0;
        c3 += m_settings.m_random ? 0x10 : 0;
        c4 =  m_settings.m_duplex ? 0x04 : 0;
        c4 += (((m_nbReceivers-1) & 0x07)<<3);
        return (c1<<24) + (c3<<8) + c4;
    }
    else if (commandIndex == 2)
    {
        return m_settings.m_txCenterFrequency;
    }
    else if (commandIndex == 4)
    {
        return m_settings.m_rx1CenterFrequency;
    }
    else if (commandIndex == 6)
    {
        return m_settings.m_rx2CenterFrequency;
    }
    else if (commandIndex == 8)
    {
        return m_settings.m_rx3CenterFrequency;
    }
    else if (commandIndex == 10)
    {
        return m_settings.m_rx4CenterFrequency;
    }
    else if (commandIndex == 12)
    {
        return m_settings.m_rx5CenterFrequency;
    }
    else if (commandIndex == 14)
    {
        return m_settings.m_rx6CenterFrequency;
    }
    else if (commandIndex == 16)
    {
        return m_settings.m_rx7CenterFrequency;
    }
    else
    {
        return 0;
    }
}

void MetisMISOUDPHandler::processIQBuffer(unsigned char* buffer)
{
    int b = 0;
    int b_max;
    int r;
    int sampleI, sampleQ, sampleMic;


    if (buffer[b++]==0x7F && buffer[b++]==0x7F && buffer[b++]==0x7F) 
    {
        QMutexLocker mutexLocker(&m_mutex);

        // extract control bytes
        m_controlIn[0] = buffer[b++];
        m_controlIn[1] = buffer[b++];
        m_controlIn[2] = buffer[b++];
        m_controlIn[3] = buffer[b++];
        m_controlIn[4] = buffer[b++];

        // extract PTT, DOT and DASH
        m_ptt = (m_controlIn[0] & 0x01) == 0x01;
        m_dash = (m_controlIn[0] & 0x02) == 0x02;
        m_dot = (m_controlIn[0] & 0x04) == 0x04;

        switch((m_controlIn[0]>>3) & 0x1F) 
        {
        case 0:
            m_lt2208ADCOverflow = m_controlIn[1] & 0x01;
            m_IO1 = (m_controlIn[1] & 0x02) ? 0 : 1;
            m_IO2 = (m_controlIn[1] & 0x04) ? 0 : 1;
            m_IO3 = (m_controlIn[1] & 0x08) ? 0 : 1;
            // {
            //     int nbRx = (m_controlIn[4]>>3) & 0x07;
            //     nbRx++;
            //     qDebug("MetisMISOUDPHandler::processIQBuffer: nbRx: %d", nbRx);
            // }    
            if (m_mercurySoftwareVersion != m_controlIn[2]) 
            {
                m_mercurySoftwareVersion = m_controlIn[2];
                qDebug("MetisMISOUDPHandler::processIQBuffer: Mercury Software version: %d (0x%0X)",
                    m_mercurySoftwareVersion, m_mercurySoftwareVersion);
            }
            
            if (m_penelopeSoftwareVersion != m_controlIn[3]) 
            {
                m_penelopeSoftwareVersion = m_controlIn[3];
                qDebug("MetisMISOUDPHandler::processIQBuffer: Penelope Software version: %d (0x%0X)",
                    m_penelopeSoftwareVersion, m_penelopeSoftwareVersion);
            }
            
            if (m_ozySoftwareVersion != m_controlIn[4]) 
            {
                m_ozySoftwareVersion = m_controlIn[4];
                qDebug("MetisMISOUDPHandler::processIQBuffer: Ozy Software version: %d (0x%0X)",
                    m_ozySoftwareVersion, m_ozySoftwareVersion);
            }
            break;
        
        case 1:
            m_forwardPower = (m_controlIn[1]<<8) + m_controlIn[2]; // from Penelope or Hermes
            m_alexForwardPower = (m_controlIn[3]<<8) + m_controlIn[4]; // from Alex or Apollo
            break;
        
        case 2:
            m_alexForwardPower = (m_controlIn[1]<<8) + m_controlIn[2]; // from Alex or Apollo
            m_AIN3 = (m_controlIn[3]<<8) + m_controlIn[4]; // from Pennelope or Hermes
            break;
        
        case 3:
            m_AIN4 = (m_controlIn[1]<<8) + m_controlIn[2]; // from Pennelope or Hermes
            m_AIN6 = (m_controlIn[3]<<8) + m_controlIn[4]; // from Pennelope or Hermes
            break;
        }

        // extract the samples
        while (b < m_bMax) 
        {
            int samplesAdded = 0;

            // extract each of the receivers
            for (r = 0; r < m_nbReceivers; r++) 
            {
                if (SDR_RX_SAMP_SZ == 16)
                {
                    sampleQ  = (int)((signed char)buffer[b++]) << 8;
                    sampleQ += (int)((unsigned char)buffer[b++]);
                    b++;
                    sampleI  = (int)((signed char)buffer[b++]) << 8;
                    sampleI += (int)((unsigned char)buffer[b++]);
                    b++;
                }
                else
                {
                    sampleQ  = (int)((signed char)buffer[b++]) << 16;
                    sampleQ += (int)((unsigned char)buffer[b++]) << 8;
                    sampleQ += (int)((unsigned char)buffer[b++]);
                    sampleI  = (int)((signed char)buffer[b++]) << 16;
                    sampleI += (int)((unsigned char)buffer[b++]) << 8;
                    sampleI += (int)((unsigned char)buffer[b++]);                    
                }

                if (r < MetisMISOSettings::m_maxReceivers) 
                {
                    if (m_settings.m_log2Decim == 0) // no decimation - direct conversion 
                    { 
                        m_convertBuffer[r][m_sampleCount] = Sample{sampleI, sampleQ};
                        samplesAdded = 1;
                    }
                    else
                    {
                        SampleVector v(2, Sample{0, 0});
                        
                        switch (m_settings.m_log2Decim)
                        {
                            case 1:
                                samplesAdded = m_decimators.decimate2(sampleI, sampleQ, v, r);
                                break;
                            case 2:
                                samplesAdded = m_decimators.decimate4(sampleI, sampleQ, v, r);
                                break;
                            case 3:
                                samplesAdded = m_decimators.decimate8(sampleI, sampleQ, v, r);
                                break;
                            default:
                                break;
                        }

                        if (samplesAdded != 0) 
                        {
                            std::copy(
                                v.begin(),
                                v.begin() + samplesAdded,
                                &m_convertBuffer[r][m_sampleCount]
                            );
                        }
                    }
                }
            }

            sampleMic  = (int)((signed char) buffer[b++]) << 8;
            sampleMic += (int)((unsigned char)buffer[b++]);

            if (samplesAdded != 0)
            {
                m_sampleCount += samplesAdded;
                
                if (m_sampleCount >= 1024)
                {
                    std::vector<SampleVector::const_iterator> vbegin;

                    for (unsigned int channel = 0; channel < MetisMISOSettings::m_maxReceivers; channel++) {
                        vbegin.push_back(m_convertBuffer[channel].begin());
                    }

                    m_sampleFifo->writeSync(vbegin, 1024);
                    m_sampleCount = 0;
                }
            }
        }
    } 
    else 
    {
        qDebug()<<"MetisMISOUDPHandler::processIQBuffer: SYNC Error";
    }

    m_rxFrame++;
}