#include "uprobeprotocol.h"

UProbeProtocol::UProbeProtocol(QObject *parent) :
    QThread(parent),UComPort()
{
    moveToThread((QThread*)this);
}

UProbeProtocol::~UProbeProtocol()
{
    this->requestInterruption();
    while(isRunning());
}

void UProbeProtocol::run()
{
    p_buffer = (char*)malloc(bufferSize);
    if(p_buffer==NULL)return;
    while(!this->isInterruptionRequested())
    {
        getMoreData();
        while(p_index<p_datacount)
        {
            if(p_buffer[p_index]==(char)0xA5)
            {
                //got to Sync state
                while (p_datacount<(p_index+6))
                {
                    getMoreData();
                }
                if(FSM_SyncState()==true)
                {
                    p_index+=currentPacket->packetSize + 6;
                    //forward DATA

                }
            }
            p_index++;
        }
    }
}

void UProbeProtocol::setPortName(const QString &name)
{
    UComPort::setPortName(name);
}

void UProbeProtocol::setSpeed(int speed)
{
    UComPort::setSpeed(speed);
}

bool UProbeProtocol::open()
{
    return UComPort::open();
}

bool UProbeProtocol::open(const QString &name, int speed)
{
    return UComPort::open(name,speed);
}

bool UProbeProtocol::close()
{
    return UComPort::close();
}

void UProbeProtocol::getMoreData()
{
    int avail;
    int maxCount=(bufferSize-32)-p_datacount;
    if(p_index>(bufferSize/2))
    {
        memcpy(p_buffer,p_buffer+p_index,p_datacount-p_index);
        p_datacount = p_datacount-p_index;
        p_index = 0;
    }
    if(opened())
    {
        avail=availableBytes();
        if(avail)
        {
            if(maxCount>=avail)
            {
                p_datacount += readBytes(p_buffer+p_datacount,avail);
            }
            else
            {
                p_datacount += readBytes(p_buffer+p_datacount,maxCount);
            }
        }
    }
}

bool UProbeProtocol::FSM_SyncState()
{
    if(p_buffer[p_index+1]==(char)0x0F0)
    {
        return FSM_ExtractHeaderState();
    }
    else
    {
        return false;
    }
}

bool UProbeProtocol::FSM_ExtractHeaderState()
{

    currentPacket = new UProbeProtocolPacket();
    currentPacket->packetNumber = p_buffer[p_index+2];
    currentPacket->packetSize =  ((unsigned int)p_buffer[p_index+3] + 1)*2;
    currentPacket->channelCount = 0xF & (int)(p_buffer[p_index+4]>>4);
    currentPacket->coding = (UProbeProtocol::UProbeProtocolPacket::PacketCoding)(0x3 & (int)(p_buffer[p_index+4]>>2));
    currentPacket->resolution = (UProbeProtocol::UProbeProtocolPacket::PacketResolution)(0x3 & (int)(p_buffer[p_index+4]));
    currentPacket->ID = (unsigned int)p_buffer[p_index+5];
    while(p_datacount<(p_index+2+4+currentPacket->packetSize+1))
    {
        getMoreData();
    }
    return FSM_ExtractPayloadState();
}


bool UProbeProtocol::FSM_ExtractPayloadState()
{
    if(!currentPacket || currentPacket->packetSize < 2 || (currentPacket->packetSize>UProbeProtocolMaxPacketSize))
        return false;
    currentPacket->data = (char*)malloc(currentPacket->packetSize);
    if(currentPacket->data != NULL)
    {
        memcpy(currentPacket->data,p_buffer+p_index+6,currentPacket->packetSize);
        return FSM_CheckCRCState();
    }
    return false;
}

bool UProbeProtocol::FSM_CheckCRCState()
{
    currentPacket->receivedCRC = p_buffer[p_index+6+currentPacket->packetSize];
    currentPacket->localCRC = spw_CRC(p_buffer+p_index,6+currentPacket->packetSize);
    if(currentPacket->localCRC == currentPacket->receivedCRC)
    {
        currentPacket->correct = true;
        return true;
    }
    currentPacket->correct = false;
    return false;
}