#include "BootloaderProtocol.h"
#include "PICUploader.h"




CBootloaderProtocol::CBootloaderProtocol(QObject *parent) : QObject(parent)
{
    mCRCEngine.InitTable();
}

CBootloaderProtocol::~CBootloaderProtocol()
{

}

int CBootloaderProtocol::BootloaderProtocolInit()
{
    BootloaderProtocolResetStateMachine();
}

int CBootloaderProtocol::BootloaderProtocolResetStateMachine()
{
    mBootloaderDataSize = 0;
    mBootloaderHeader = 0;
    mBootloaderBufPtr = 0;
    mBootloaderDataBuffer.clear();
    mBootloaderCommand = 0;
    mBootloaderCRC = 0;
    mBootloaderComputedCRC = CRC_START_32;
    mBootloaderState = RxHeader1;
    mBootloaderDataCtr = 0;

}

int CBootloaderProtocol::BooloaderProtocolRxFrame(QByteArray Frame)
{
    for(int i = 0; i < Frame.size(); i++)
    {
        BootloaderProtocolStateMachine(Frame.at(i));
    }
}

void CBootloaderProtocol::BootloaderProtocolStateMachine(unsigned char Data)
{

    switch(mBootloaderState)
        {
            case Initialization:	//Reset all pointers and data...
            {
                mBootloaderDataSize = 0;
                mBootloaderBufPtr = 0;
                mBootloaderDataBuffer.clear();
                mBootloaderCommand = 0;
                mBootloaderCRC = 0;
                mBootloaderComputedCRC = CRC_START_32;
                mBootloaderState = RxHeader1;
                break;
            }
            case RxHeader1:	//Wait for data header...
            {
                mBootloaderHeader += Data;	//0xDE
                mBootloaderComputedCRC = mCRCEngine.UpdateCRC32(mBootloaderComputedCRC,Data);

                if(Data == BOOTLOADER_FRAME_HEADER_1)
                {
                    mBootloaderState = RxHeader2;
                }
                else
                {
                    BootloaderProtocolResetStateMachine();
                }

                break;
            }
        case RxHeader2:	//Wait for data header...
        {
            mBootloaderHeader <<= 8;
            mBootloaderHeader += Data;	//0xAD
            mBootloaderComputedCRC = mCRCEngine.UpdateCRC32(mBootloaderComputedCRC,Data);

            if(Data == BOOTLOADER_FRAME_HEADER_2)
            {
                mBootloaderState = RxHeader3;
            }
            else
            {
                BootloaderProtocolResetStateMachine();
            }
            break;
        }
        case RxHeader3:	//Wait for data header...
        {
            mBootloaderHeader <<= 8;
            mBootloaderHeader += Data;	//0xBE
            mBootloaderComputedCRC = mCRCEngine.UpdateCRC32(mBootloaderComputedCRC,Data);

            if(Data == BOOTLOADER_FRAME_HEADER_3)
            {
                mBootloaderState = RxHeader4;
            }
            else
            {
                BootloaderProtocolResetStateMachine();
            }
            break;
        }
        case RxHeader4:	//Wait for data header...
        {
            mBootloaderHeader <<= 8;
            mBootloaderHeader += Data;	//0xEF
            mBootloaderComputedCRC = mCRCEngine.UpdateCRC32(mBootloaderComputedCRC,Data);

            if(mBootloaderHeader != BOOTLOADER_FRAME_HEADER)
            {
                //TODO, send NACK?
                BootloaderProtocolResetStateMachine();
                break;
            }
            else
            {
                mBootloaderState = RxCmd;
            }

            break;
        }
        case RxCmd:
        {
            mBootloaderCommand = Data;
            mBootloaderState = RxPayloadSize1;
            mBootloaderComputedCRC = mCRCEngine.UpdateCRC32(mBootloaderComputedCRC,Data);
            break;
        }
        case RxPayloadSize1:
        {
            mBootloaderDataSize = Data;
            mBootloaderState = RxPayloadSize2;
            mBootloaderComputedCRC = mCRCEngine.UpdateCRC32(mBootloaderComputedCRC,Data);
            break;
        }
        case RxPayloadSize2:
        {
            mBootloaderDataSize <<= 8;
            mBootloaderDataSize += Data;

            mBootloaderState = RxPayloadSize3;
            mBootloaderComputedCRC = mCRCEngine.UpdateCRC32(mBootloaderComputedCRC,Data);
            break;
        }
        case RxPayloadSize3:
        {
            mBootloaderDataSize <<= 8;
            mBootloaderDataSize += Data;
            mBootloaderComputedCRC = mCRCEngine.UpdateCRC32(mBootloaderComputedCRC,Data);

            mBootloaderState = RxPayloadSize4;
            break;
        }
        case RxPayloadSize4:
        {
            mBootloaderDataSize <<= 8;
            mBootloaderDataSize += Data;
            mBootloaderComputedCRC = mCRCEngine.UpdateCRC32(mBootloaderComputedCRC,Data);

            if(mBootloaderDataSize > MAX_BOOTLOADER_PAYLOAD_SIZE)
            {
                //TODO, send NACK?
                BootloaderProtocolResetStateMachine();
                break;
            }

            mBootloaderState = RxPayload;
            break;
        }
        case RxPayload:	//Data size
        {
            mBootloaderDataBuffer.append(Data);
            mBootloaderDataCtr++;
            mBootloaderComputedCRC = mCRCEngine.UpdateCRC32(mBootloaderComputedCRC,Data);

            if(mBootloaderDataCtr == mBootloaderDataSize)
            {
                mBootloaderState = RxCRC1;
                break;
            }

            break;
        }
        case RxCRC1:	//Data size
        {
            mBootloaderCRC = Data;
            mBootloaderState = RxCRC2;
            break;
        }
        case RxCRC2:	//Data size
        {
            mBootloaderCRC <<= 8;
            mBootloaderCRC += Data;
            mBootloaderState = RxCRC3;
            break;
        }
        case RxCRC3:	//Data size
        {
            mBootloaderCRC <<= 8;
            mBootloaderCRC += Data;
            mBootloaderState = RxCRC4;
            break;
        }
        case RxCRC4:	//Data size
        {
            mBootloaderCRC <<= 8;
            mBootloaderCRC += Data;

            mBootloaderComputedCRC ^= 0xffffffffL;
            //if(mBootloaderCRC == 0xBAADCAFE)
            if(mBootloaderCRC == mBootloaderComputedCRC)
            {
                mProgramHandle->BootloaderRxCmd(mBootloaderCommand,mBootloaderDataBuffer);
            }
            else
            {
                qDebug("Bootloader Protocol bad CRC...");
            }

            BootloaderProtocolResetStateMachine();
            break;
        }
        default:
        {
            BootloaderProtocolResetStateMachine();
            break;
        }
    }
}

QByteArray CBootloaderProtocol::BootloaderProtocolGetFrame(char Cmd, QByteArray Data)
{
    QByteArray Frame;
    Frame.clear();

    // Header
    Frame.append(BOOTLOADER_FRAME_HEADER_1);
    Frame.append(BOOTLOADER_FRAME_HEADER_2);
    Frame.append(BOOTLOADER_FRAME_HEADER_3);
    Frame.append(BOOTLOADER_FRAME_HEADER_4);

    //Cmd
    Frame.append(Cmd);

    //Size
    unsigned int Size = Data.size();

    char nibble = (char)((Size >> 24) &0x000000FF);
    Frame.append(nibble);

    nibble = (char)((Size >> 16) &0x000000FF);
    Frame.append(nibble);

    nibble = (char)((Size >> 8) &0x000000FF);
    Frame.append(nibble);

    nibble = (char)(Size &0x000000FF);
    Frame.append(nibble);

    //Payload
    Frame.append(Data);

    //CRC
    unsigned int CRC = mCRCEngine.ComputeCRC32(Frame);

    nibble = (char)((CRC >> 24) &0x000000FF);
    Frame.append(nibble);

    nibble = (char)((CRC >> 16) &0x000000FF);
    Frame.append(nibble);

    nibble = (char)((CRC >> 8) &0x000000FF);
    Frame.append(nibble);

    nibble = (char)(CRC &0x000000FF);
    Frame.append(nibble);

/*    Frame.append(0xBA);
    Frame.append(0xAD);
    Frame.append(0xCA);
    Frame.append(0xFE);*/

    //Send
    return Frame;


}