#include "stardundeespw_usb.h"
#include <socexplorerengine.h>
#include <qhexedit.h>

stardundeeSPW_USB::stardundeeSPW_USB(socexplorerplugin *parent) :
    abstractSpwBridge(parent)
{
    Q_UNUSED(parent)
    this->manager = new stardundeeSPW_USB_Manager(parent);
    this->manager->start();
}

bool stardundeeSPW_USB::connectBridge()
{
    return this->manager->connectBridge();
}

bool stardundeeSPW_USB::disconnectBridge()
{
    return this->manager->disconnectBridge();
}

int stardundeeSPW_USB::pushRMAPPacket(char *packet, int size)
{
    return this->manager->sendPacket(packet,size);
}

unsigned int stardundeeSPW_USB::Write(unsigned int *Value, unsigned int count, unsigned int address)
{
    //Add transactionID!
    char writeBuffer[RMAP_WRITE_PACKET_MIN_SZ(RMAP_MAX_XFER_SIZE)+1];
    writeBuffer[0]=1;//Link number
    int transactionID = 0;
    int written=0;
    SocExplorerEngine::message(this->plugin,"Enter Write function");
    //Quite stupide loop, I guess that I always get the number of byte I asked for!
    for(;count>=RMAP_MAX_XFER_SIZE;count-=RMAP_MAX_XFER_SIZE)
    {
        for(int i=0;i<(RMAP_MAX_XFER_SIZE);i++)
        {
            writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+1] = (char) ((unsigned int) Value[written+i]>>24);
            writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+2] = (char) ((unsigned int) Value[written+i]>>16);
            writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+3] = (char) ((unsigned int) Value[written+i]>>8);
            writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+4] = (char) ((unsigned int) Value[written+i]);
        }
        RMAP_build_tx_request_header(254,2,1,transactionID,address+written,RMAP_MAX_XFER_SIZE*4,writeBuffer+1);
        manager->sendPacket(writeBuffer,RMAP_WRITE_PACKET_MIN_SZ(RMAP_MAX_XFER_SIZE*4)+1);
        written+=RMAP_MAX_XFER_SIZE;
    }
    if(count>0)
    {
        for(int i=0;i<((int)count);i++)
        {
            writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+1] = (char) ((unsigned int) Value[written+i]>>24);
            writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+2] = (char) ((unsigned int) Value[written+i]>>16);
            writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+3] = (char) ((unsigned int) Value[written+i]>>8);
            writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+4] = (char) ((unsigned int) Value[written+i]);
        }
        RMAP_build_tx_request_header(254,2,1,transactionID,address+written,count*4,writeBuffer+1);
        manager->sendPacket(writeBuffer,RMAP_WRITE_PACKET_MIN_SZ(count*4) +1);
        //        QHexEdit* viewer = new QHexEdit();
        //        viewer->setData(QByteArray(writeBuffer,RMAP_WRITE_PACKET_MIN_SZ(count*4)+1));
        //        viewer->show();
        written+=count;
    }
    return written;
}

unsigned int stardundeeSPW_USB::Read(unsigned int *Value, unsigned int count, unsigned int address)
{
    //Add transactionID!
    char requestBuffer[RMAP_READ_HEADER_MIN_SZ+1];
    char* RMAP_AnswerBuffer;
    requestBuffer[0]=1;//Link number
    int transactionID = 0;
    int read=0;
//    SocExplorerEngine::message(this->plugin,"Enter read function");
    SocExplorerEngine::message(this->plugin,QString("Enter read function, count=%1, RMAP_MAX_XFER_SIZE=%2").arg(count).arg(RMAP_MAX_XFER_SIZE));

    //Quite stupide loop, I guess that I always get the number of byte I asked for!
    while((int)count>=(int)RMAP_MAX_XFER_SIZE)
    {
        transactionID = manager->getRMAPtransactionID();
        SocExplorerEngine::message(this->plugin,QString("New transactionID:%1").arg(transactionID));
        RMAP_build_rx_request_header(254,2,1,transactionID,address+(read*4),RMAP_MAX_XFER_SIZE*4,requestBuffer+1);
        manager->sendPacket(requestBuffer,RMAP_READ_HEADER_MIN_SZ+1);
        int len=manager->getRMAPanswer(transactionID,&RMAP_AnswerBuffer);
        for(int i=0;i<(len/4);i++)
        {
            Value[read+i]=((unsigned int)(RMAP_AnswerBuffer[i])<<24) + ((unsigned int)(RMAP_AnswerBuffer[i+1])<<16) + ((unsigned int)(RMAP_AnswerBuffer[i+2])<<8) + ((unsigned int)(RMAP_AnswerBuffer[i+3]));
        }
        free(RMAP_AnswerBuffer);
        read+=RMAP_MAX_XFER_SIZE;
        count-=RMAP_MAX_XFER_SIZE;
    }
    if((int)count>0)
    {
        transactionID = manager->getRMAPtransactionID();
        SocExplorerEngine::message(this->plugin,QString("New transactionID: %1").arg(transactionID));
        SocExplorerEngine::message(this->plugin,QString("Building request with:"));
        SocExplorerEngine::message(this->plugin,QString("Address = %1").arg(address+(read*4),8,16));
        SocExplorerEngine::message(this->plugin,QString("Size = %1").arg(count*4));
        SocExplorerEngine::message(this->plugin,QString("Size + 13 = %1").arg((count*4)+13));
        RMAP_build_rx_request_header(254,2,1,transactionID,address+(read*4),count*4,requestBuffer+1);
        manager->sendPacket(requestBuffer,RMAP_READ_HEADER_MIN_SZ+1);
        int len=manager->getRMAPanswer(transactionID,&RMAP_AnswerBuffer);
        for(int i=0;i<(len/4);i++)
        {
            Value[read+i]=((unsigned int)(RMAP_AnswerBuffer[i])<<24) + ((unsigned int)(RMAP_AnswerBuffer[i+1])<<16) + ((unsigned int)(RMAP_AnswerBuffer[i+2])<<8) + ((unsigned int)(RMAP_AnswerBuffer[i+3]));
        }
        free(RMAP_AnswerBuffer);
        read+=count;
    }
    return read;
}

stardundeeSPW_USB_Manager::stardundeeSPW_USB_Manager(socexplorerplugin *parent)
    :QThread((QObject*)parent)
{
    this->handleMutex = new QMutex(QMutex::NonRecursive);
    this->RMAP_AnswersSem = new QSemaphore(0);
    this->RMAP_AnswersMtx=new QMutex(QMutex::Recursive);
    this->RMAP_pending_transaction_IDsMtx=new QMutex(QMutex::Recursive);
    this->plugin = parent;
    connected = false;
}

stardundeeSPW_USB_Manager::~stardundeeSPW_USB_Manager()
{
    this->terminate();
    while (!this->isFinished()) {
        this->usleep(1000);
    }
}


void stardundeeSPW_USB_Manager::run()
{
    USB_SPACEWIRE_PACKET_PROPERTIES properties;
    USB_SPACEWIRE_ID pIdentifier=NULL;
    USB_SPACEWIRE_STATUS stat;
    SocExplorerEngine::message(this->plugin,"Starting Startdundee USB pooling thread");
    char buffer[RMAP_MAX_XFER_SIZE*4];
    while (!this->isInterruptionRequested())
    {
        if(this->connected)
        {
            handleMutex->lock();
            //SocExplorerEngine::message(this->plugin,"Looking for new RMAP packets");
            if(USBSpaceWire_WaitOnReadPacketAvailable(hDevice,0.01))
            {
                SocExplorerEngine::message(this->plugin,"Got packet");
                stat = USBSpaceWire_ReadPackets(hDevice, buffer, RMAP_MAX_XFER_SIZE*4,1, 1, &properties, &pIdentifier);
                if (stat == TRANSFER_SUCCESS)
                {
                    if(USBSpaceWire_GetReadTrafficType(&properties, 0) ==SPACEWIRE_TRAFFIC_PACKET)
                    {
                        SocExplorerEngine::message(this->plugin,"It's a SPW packet");
                        if(USBSpaceWire_GetReadEOPStatus(&properties, 0)== SPACEWIRE_USB_EOP)
                        {
                            SocExplorerEngine::message(this->plugin,"Got end of packet");
                            if(buffer[1]==(char)SPW_PROTO_ID_RMAP) //RMAP packet
                            {
                                SocExplorerEngine::message(this->plugin,"Got RMAP packet");
                                SocExplorerEngine::message(this->plugin,QString("Rmap packet size %1").arg(properties.len));
                                char* packetbuffer = (char*)malloc(properties.len-13);
                                memcpy(packetbuffer,buffer+12,properties.len-13);
                                USBSpaceWire_FreeRead(hDevice, pIdentifier);
                                pIdentifier = NULL;
                                handleMutex->unlock();
                                RMAP_Answer* packet=new RMAP_Answer(RMAP_get_transactionID(buffer+1),packetbuffer,properties.len);
                                RMAP_AnswersMtx->lock();
                                RMAP_Answers.append(packet);
                                RMAP_AnswersMtx->unlock();
                                RMAP_AnswersSem->release();
                            }
                            else //any non-rmap packet will be pushed to the network
                            {
                                USBSpaceWire_FreeRead(hDevice, pIdentifier);
                                handleMutex->unlock();
                                SocExplorerEngine::message(this->plugin,"Got SPW packet");
                            }
                        }
                        else
                        {
                            SocExplorerEngine::message(this->plugin,"No EOP received");
                        }
                    }

                }
                else
                {
                    USBSpaceWire_FreeRead(hDevice, pIdentifier);
                    handleMutex->unlock();
                }
            }
            else
            {
                USBSpaceWire_FreeRead(hDevice, pIdentifier);
                handleMutex->unlock();
            }
        }
        else
        {
            sleep(1);
            SocExplorerEngine::message(this->plugin,"Bridge not connected");
        }
        usleep(1000);
        //        sleep(2);
    }
    SocExplorerEngine::message(this->plugin,"Exiting Startdundee USB pooling thread");
}

bool stardundeeSPW_USB_Manager::connectBridge()
{
    this->handleMutex->lock();
    int status;
    U32 statusControl;
    int brickNumber=0;
    int linkNumber=1;
    this->connected = false;
    if (!USBSpaceWire_Open(&hDevice, brickNumber)) // Open the USB device
    {
        SocExplorerEngine::message(this->plugin,"stardundee *** Open *** ERROR: USBSpaceWire_Open(&hDevice, 0))");
        this->handleMutex->unlock();
        return false;
    }
    SocExplorerEngine::message(this->plugin,"stardundee *** Open *** USBSpaceWire_Open successful");

    USBSpaceWire_EnableNetworkMode(hDevice, 0); // deactivate the network mode
    CFGSpaceWire_EnableRMAP(1);                 // Enable the use of RMAP for the StarDundee brick configuration
    CFGSpaceWire_SetRMAPDestinationKey(0x20);   // Set the destination key expected by STAR-Dundee devices

    // Set the path and return path to the device
    CFGSpaceWire_StackClear();
    CFGSpaceWire_AddrStackPush(0);
    CFGSpaceWire_AddrStackPush(254);
    CFGSpaceWire_RetAddrStackPush(254);
    // set the base transmit rate to 100 MHz
    status = CFGSpaceWire_SetBrickBaseTransmitRate( hDevice, CFG_BRK_CLK_100_MHZ, CFG_BRK_DVDR_1, 0xff);
    if (status != CFG_TRANSFER_SUCCESS)
    {
        SocExplorerEngine::message(this->plugin,"ERROR CFGSpaceWire_SetBrickBaseTransmitRate");
        return false;
    }
    else
    {
        SocExplorerEngine::message(this->plugin,"OK CFGSpaceWire_SetBrickBaseTransmitRate, base rate = 100 MHz");
    }

    // read the link status
    if (CFGSpaceWire_GetLinkStatusControl(hDevice, linkNumber, &statusControl) != CFG_TRANSFER_SUCCESS)
    {
        SocExplorerEngine::message(this->plugin,"Could not read link status control for link " + QString::number(linkNumber));
        return false;
    }
    else
    {
        SocExplorerEngine::message(this->plugin,"OK CFGSpaceWire_GetLinkStatusControl of link " + QString::number(linkNumber));

        // Set the link status control register properties
        CFGSpaceWire_LSEnableAutoStart(&statusControl, 1);
        CFGSpaceWire_LSEnableStart(&statusControl, 1);
        CFGSpaceWire_LSEnableDisabled(&statusControl, 0);
        CFGSpaceWire_LSEnableTristate(&statusControl, 0);
        CFGSpaceWire_LSSetOperatingSpeed(&statusControl, 9);    // sets the link speed to ( 100 MHz / (9+1) ) = 10 MHz

        // Set the link status control register
        if (CFGSpaceWire_SetLinkStatusControl(hDevice, linkNumber, statusControl) != CFG_TRANSFER_SUCCESS)
        {
            SocExplorerEngine::message(this->plugin,"Could not set the link status control for link " + QString::number(linkNumber));
            return false;
        }
        else
        {
            SocExplorerEngine::message(this->plugin,"Set the link status control for link " + QString::number(linkNumber));
        }
    }

    if (CFGSpaceWire_SetAsInterface(hDevice, 1, 0) != CFG_TRANSFER_SUCCESS)
    {
        SocExplorerEngine::message(this->plugin,"Could not set the device to be an interface");
        return false;
    }
    else
    {
        SocExplorerEngine::message(this->plugin,"Device set to be an interface");
    }

    USBSpaceWire_RegisterReceiveOnAllPorts(hDevice);    // Register to receive on all ports
    USBSpaceWire_ClearEndpoints(hDevice);               // clear the USB endpoints
    USBSpaceWire_SetTimeout(hDevice,1.0);
    SocExplorerEngine::message(this->plugin,"The driver's current send buffer size is " + QString::number(USBSpaceWire_GetDriverSendBufferSize(hDevice)) + " bytes");
    SocExplorerEngine::message(this->plugin,"The driver's current read buffer size is " + QString::number(USBSpaceWire_GetDriverReadBufferSize(hDevice)) + " bytes");
    SocExplorerEngine::message(this->plugin,"USBSpaceWire_IsReadThrottling is " + QString::number(USBSpaceWire_IsReadThrottling(hDevice)));
    this->connected = true;
    this->handleMutex->unlock();
    return true;
}

bool stardundeeSPW_USB_Manager::disconnectBridge()
{
    this->handleMutex->lock();
    USBSpaceWire_Close(hDevice); // Close the device
    SocExplorerEngine::message(this->plugin,"stardundee *** Close *** USBSpaceWire_Close, device: " + QString::number(0));
    USBSpaceWire_UnregisterReceiveOnAllPorts(hDevice);  // Stop receiving on all ports
    this->handleMutex->unlock();
    return true;
}

int stardundeeSPW_USB_Manager::getRMAPtransactionID()
{
    this->RMAP_pending_transaction_IDsMtx->lock();
    int  ID=0;
    bool found=true;
    while(ID<65536)
    {
        for(int i=0;i<RMAP_pending_transaction_IDs.count();i++)
        {
            if(RMAP_pending_transaction_IDs[i]==ID)found=false;
        }
        if(found==true)break;
        ID++;
        found = true;
    }
    if(found)
    {
        RMAP_pending_transaction_IDs.append(ID);
    }
    this->RMAP_pending_transaction_IDsMtx->unlock();
    return ID;
}

int stardundeeSPW_USB_Manager::getRMAPanswer(int transactionID, char **buffer)
{
    *buffer=NULL;
    int count=0;
    while (*buffer==NULL)
    {
        this->RMAP_AnswersMtx->lock();
        for(int i=0;i<RMAP_Answers.count();i++)
        {
            if(RMAP_Answers[i]->transactionID==transactionID)
            {
                this->RMAP_pending_transaction_IDsMtx->lock();
                for(int j=0;j<RMAP_pending_transaction_IDs.count();j++)
                {
                    if(RMAP_pending_transaction_IDs[j]==transactionID)
                    {
                        RMAP_pending_transaction_IDs.removeAt(j);
                    }
                }
                this->RMAP_pending_transaction_IDsMtx->unlock();
                *buffer = RMAP_Answers[i]->data;
                count = RMAP_Answers[i]->len;
                RMAP_Answer* tmp=RMAP_Answers[i];
                RMAP_Answers.removeAt(i);
                delete tmp;
            }
        }
        this->RMAP_AnswersMtx->unlock();
        //if no answer found in the stack wait until a new packet is pushed
        if(!buffer)
        {
            SocExplorerEngine::message(this->plugin,"waiting until a new packet is pushed");
            this->RMAP_AnswersSem->acquire();
        }
    }
    return count;
}

bool stardundeeSPW_USB_Manager::sendPacket(char *packet, int size)
{
    USB_SPACEWIRE_STATUS result;
    USB_SPACEWIRE_ID pIdentifier;
    SocExplorerEngine::message(this->plugin,"Sending SPW packet");
    this->handleMutex->lock();
    result = USBSpaceWire_SendPacket(hDevice,packet,size,1, &pIdentifier);
    if (result != TRANSFER_SUCCESS)
    {
        SocExplorerEngine::message(this->plugin,"ERR sending the READ command ");
        this->handleMutex->unlock();
        return false;
    }
    else
    {
        SocExplorerEngine::message(this->plugin,"Packet sent");
        USBSpaceWire_FreeSend(hDevice, pIdentifier);
    }
    this->handleMutex->unlock();
    return true;
}

void stardundeeSPW_USB_Manager::pushRmapPacket(char *packet, int len)
{
    char* packetbuffer = (char*)malloc(len);
    memcpy(packetbuffer,packet,len);
    RMAP_Answer* RMPAPpacket=new RMAP_Answer(RMAP_get_transactionID(packetbuffer+1),packetbuffer,len);
    RMAP_AnswersMtx->lock();
    RMAP_Answers.append(RMPAPpacket);
    RMAP_AnswersMtx->unlock();
}