#include "PICUploader.h"
#include "GlobalDefine.h"
#include <QHostAddress>
#include <QtEndian>


CPICUploader::CPICUploader(CPICUploaderGui *Gui)
{
    mPICUploaderGui = Gui;
    mPICUploaderGui->mProgramHandle = this;

    connect(&mBootloaderSocket,SIGNAL(connected()),this,SLOT(BootloaderSocketConnected()));
    connect(&mBootloaderSocket,SIGNAL(disconnected()),this,SLOT(BootloaderSocketDisconnected()));
    connect(&mBootloaderSocket,SIGNAL(readyRead()),this,SLOT(BootloaderDataAvailable()));
    connect(&mBootloaderSocket,SIGNAL(error(QAbstractSocket::SocketError)),this,SLOT(BootloaderSocketError(QAbstractSocket::SocketError)));

    mBootloaderProtocol.mProgramHandle = this;
}


int CPICUploader::Start()
{
    mBootloaderProtocol.BootloaderProtocolInit();
    ResetBooloaderInterfaceStateMachine();
    return RET_OK;
}


int CPICUploader::OpenHexFileRequest(QString FilePath)
{
    if(mHexFile.HexFileLoaded() == true)
    {
        mHexFile.CloseOpenedHexFile();
    }

    int ret = mHexFile.OpenDataFile(FilePath);

    if(ret == 0)
        return 0;

    QString HexFileStats = QString("HexFileSize: %1\nFirmware Size: %4\nNb analyzed Records: %2\nNb Big Records: %3\nData CRC32: 0x%5").arg(mHexFile.GetFileSize()).arg(mHexFile.GetTotalParsedRecords()).arg(mHexFile.GetNBRecords()).arg(mHexFile.GetFirmwareSize()).arg(mHexFile.GetDataCRC32(),0,16);

    mPICUploaderGui->SetHexFileStats(HexFileStats);


    //HexFileStats << "HexFileSize: " << mHexFile.GetFileSize() << "\n";
   // HexFileStats <<
}

int CPICUploader::ShowHexFileInfoRequest()
{
    mPICUploaderGui->AddTextToLogScreen(mHexFile.GetHexFileInfoString());
}

int CPICUploader::ConnectToBootloader(QString IP)
{

    mBootloaderSocket.connectToHost(IP,99);
    return RET_OK;
}

int CPICUploader::DisconnectFromBootloader()
{
    mBootloaderSocket.disconnectFromHost();
    return true;
}

void CPICUploader::BootloaderSocketConnected()
{
    mPICUploaderGui->UploaderSocketConnected();
    mPICUploaderGui->AddTextToLogScreen("Socket Connected");
    BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_BOOTLOADER_CONNECTED_CMD);
}

void CPICUploader::BootloaderSocketDisconnected()
{
    mPICUploaderGui->UploaderSocketDisconnected();
    mPICUploaderGui->AddTextToLogScreen("Socket Disconnected");
    BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_BOOTLOADER_DISCONNECTED_CMD);
}

void CPICUploader::BootloaderDataAvailable()
{

    QByteArray Data = mBootloaderSocket.readAll();
    mPICUploaderGui->AddTextToLogScreen(QString("Recevied %1 bytes from bootloader").arg(Data.size()));
    mBootloaderProtocol.BooloaderProtocolRxFrame(Data);

}

void CPICUploader::BootloaderSocketError(QAbstractSocket::SocketError Error)
{
    mPICUploaderGui->AddTextToLogScreen(QString("Socket error %1 in PICUploader").arg(Error));
}

int CPICUploader::SendSingleCmdRequest(int CmdID)
{
    QByteArray Frame = mBootloaderProtocol.BootloaderProtocolGetFrame(CmdID,QByteArray());
    mBootloaderSocket.write(Frame);
    return RET_OK;
}

int CPICUploader::SendCmdRequest(int CmdID, QByteArray Data)
{
    QByteArray Frame = mBootloaderProtocol.BootloaderProtocolGetFrame(CmdID,Data);
    mBootloaderSocket.write(Frame);
    return RET_OK;
}

int CPICUploader::GuiEvent(int EventID, void *Data)
{
    switch(EventID)
    {
        case CPICUploaderGui::UPLOADER_GUI_HEARTBEAT_BTN_CLICK_EVENT:
        {
            SendSingleCmdRequest(BOOTLOADER_HEARTBEAT_REQUEST);
            break;
        }
        case CPICUploaderGui::UPLOADER_GUI_FLASH_ERASE_BTN_CLICK_EVENT:
        {
            BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_ERASE_FLASH_GUI_CMD);
            break;
        }
        case CPICUploaderGui::UPLOADER_GUI_INIT_UPLOAD_BTN_CLICK_EVENT:
        {
            BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_START_UPLOAD_GUI_CMD);
            break;
        }
        case CPICUploaderGui::UPLOADER_GUI_SEND_DATA_CHUNK_BTN_CLICK_EVENT:
        {
            SendSingleCmdRequest(BOOTLOADER_SEND_DATA_CHUNK_REQUEST);
            break;
        }
        case CPICUploaderGui::UPLOADER_GUI_UPLOAD_FINISHED_BTN_CLICK_EVENT:
        {
            SendSingleCmdRequest(BOOTLOADER_UPLOAD_FINISHED_REQUEST);
            break;
        }
        case CPICUploaderGui::UPLOADER_GUI_EXECUTE_UPGRADE_BTN_CLICK_EVENT:
        {
            SendSingleCmdRequest(BOOTLOADER_EXECUTE_UPGRAGE_REQUEST);
            break;
        }
        case CPICUploaderGui::UPLOADER_GUI_ABORT_BTN_CLICK_EVENT:
        {
            BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_ABORT_GUI_CMD);
            break;
        }
        case CPICUploaderGui::UPLOADER_GUI_AUTOMATIC_UPDATE_START_BTN_CLICK_EVENT:
        {
            break;
        }
        case CPICUploaderGui::UPLOADER_GUI_GET_STATE_BTN_CLICK_EVENT:
        {
            SendSingleCmdRequest(BOOTLOADER_GET_STATE_REQUEST);
            break;
        }
        case CPICUploaderGui::UPLOADER_GUI_CHECK_FLASH_CLICK_EVENT:
        {
            SendSingleCmdRequest(BOOTLOADER_CHECK_FLASH_FIRMW_INTEGRITY_REQUEST);
            break;
        }
        case CPICUploaderGui::UPLOADER_GUI_GET_STORED_FIRMWARE_INFO_CLICK_EVENT:
        {
            SendSingleCmdRequest(BOOTLOADER_GET_STORED_FIRMWARE_INFO_REQUEST);
            break;
        }
    }
    return RET_OK;
}

int CPICUploader::BootloaderRxCmd(int CmdID, QByteArray Data)
{
    switch(CmdID)
    {
        case BOOTLOADER_HEARTBEAT_RESPONSE:
        {
            mPICUploaderGui->AddTextToLogScreen(QString("Heartbeat device response: %1").arg((int)Data.at(0)));
            break;
        }
        case BOOTLOADER_ERASE_BOOTLOADER_FLASH_RESPONSE:
        {
            mPICUploaderGui->AddTextToLogScreen(QString("Erase flash device response: %1").arg((int)Data.at(0)));
            BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_ERASE_BOOTLOADER_FLASH_ACK_HOST_CMD,Data.at(0));
            break;
        }
        case BOOTLOADER_ERASE_BOOTLOADER_FLASH_RESULT_RESPONSE:
        {
            mPICUploaderGui->AddTextToLogScreen(QString("Erase flash result device response: %1").arg((int)Data.at(0)));
            BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_ERASE_BOOTLOADER_FLASH_RESULT_HOST_CMD,Data.at(0));
            break;
        }
        case BOOTLOADER_INIT_UPLOAD_RESPONSE:
        {
            int ChunkSize;
            quint32 temp = *(int*)Data.mid(1,4).data();
            qToBigEndian(temp,&ChunkSize);
            mPICUploaderGui->AddTextToLogScreen(QString("Init Upload device response: %1, Chunk Size: %2").arg((int)Data.at(0)).arg(ChunkSize));
            BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_INIT_UPLOAD_HOST_CMD,(int)Data.at(0),(void*)&ChunkSize);

            break;
        }
        case BOOTLOADER_GET_STATE_RESPONSE:
        {
            mPICUploaderGui->AddTextToLogScreen(QString("Get State device Response: %1").arg((int)Data.at(0)));
            break;
        }
        case BOOTLOADER_READY_FOR_DATA_RESPONSE:
        {
            mPICUploaderGui->AddTextToLogScreen(QString("Ready for data device response: %1").arg((int)Data.at(0)));
            BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_READY_FOR_DATA_HOST_CMD,(int)Data.at(0));
            break;
        }
        case BOOTLOADER_SEND_DATA_CHUNK_RESPONSE:
        {
            quint32 temp = *(int*)Data.mid(1,4).data();
            int Index;
            qToBigEndian(temp,&Index);
            mPICUploaderGui->AddTextToLogScreen(QString("Send Chunk device response: %1, Index: %2").arg((int)Data.at(0)).arg(Index));
            BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_SEND_DATA_CHUNK_HOST_CMD,(int)Data.at(0),(void*)&Index);
            break;
        }
        case BOOTLOADER_UPLOAD_FINISHED_RESPONSE:
        {
            mPICUploaderGui->AddTextToLogScreen(QString("Upload finished device response: %1").arg((int)Data.at(0)));
            BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_UPLOAD_FINISHED_HOST_CMD,(int)Data.at(0));
            break;
        }
        case BOOTLOADER_EXECUTE_UPGRADE_RESPONSE:
        {
            mPICUploaderGui->AddTextToLogScreen(QString("Execute upgrade device response: %1").arg((int)Data.at(0)));
            BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_EXECUTE_UPGRADE_HOST_CMD,(int)Data.at(0));
            break;
        }
        case BOOTLOADER_ABORT_OPERATION_RESPONSE:
        {
            mPICUploaderGui->AddTextToLogScreen(QString("Abort Operation device response: %1").arg((int)Data.at(0)));
            BootloaderInterfaceStateMachine(BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT,BOOTLOADER_UPLOAD_ABORT_OPERATION_HOST_CMD,(int)Data.at(0));
            break;
        }
        case BOOTLOADER_CHECK_FLASH_FIRMW_INTEGRITY_RESPONSE:
        {
            mPICUploaderGui->AddTextToLogScreen(QString("Flash integrity check response: %1").arg((int)Data.at(0)));
            break;
        }
        case BOOTLOADER_GET_STORED_FIRMWARE_INFO_RESPONSE:
        {
            QString FirmwareInfo;
            FirmwareInfo.clear();

            if(Data.at(0) == 0)
            {
                FirmwareInfo = "--------------------------------\nNo firmware stored in SPI flash\n--------------------------------";
            }
            else
            {
                quint32 FirmwareFlags, NbRecords, FirmwareSize, VersionCode, DataCRC32;

                quint32 temp = *(int*)Data.mid(1,4).data();
                qToBigEndian(temp,&FirmwareFlags);

                temp = *(int*)Data.mid(5,4).data();
                qToBigEndian(temp,&NbRecords);

                temp = *(int*)Data.mid(9,4).data();
                qToBigEndian(temp,&FirmwareSize);

                temp = *(int*)Data.mid(13,4).data();
                qToBigEndian(temp,&VersionCode);

                temp = *(int*)Data.mid(17,4).data();
                qToBigEndian(temp,&DataCRC32);

                FirmwareInfo = QString("\n----------------------------------\nStored firmware info:\nFirmware flags: 0x%1\nNb Records: %2\nFirmware Size: %3\nVersion: %4\nData CRC32: 0x%5\n----------------------------------\n")\
                        .arg(FirmwareFlags,0,16).arg(NbRecords).arg(FirmwareSize).arg(VersionCode).arg(DataCRC32,0,16);

            }

            mPICUploaderGui->AddTextToLogScreen(FirmwareInfo);

            break;
        }
    }
}

int CPICUploader::BootloaderInterfaceStateMachine(int Event, int Cmd, int CmdParam, void* Data)
{
    switch(Event)
    {
        case BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT:
        {
            switch(Cmd)
            {
                case BOOTLOADER_UPLOAD_BOOTLOADER_DISCONNECTED_CMD:
                {
                    ResetBooloaderInterfaceStateMachine();
                    return RET_OK;
                    break;
                }
            }
            break;
        }
    }

    // State machine implementation starts here...

    switch (mBootloaderInterfaceSMState)
    {
        case BOOTLOADER_INTERFACE_SM_STANDBY_STATE:
        {
            switch(Event)
            {
                case BOOTLOADER_UPLOAD_SM_TICK_EVENT:
                {
                    break;
                }
                case BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT:
                {
                    switch(Cmd)
                    {
                        case BOOTLOADER_UPLOAD_BOOTLOADER_CONNECTED_CMD:
                        {
                            mBootloaderInterfaceSMState = BOOTLOADER_INTERFACE_SM_BOOTLDR_CONNECTED_STATE;
                            mPICUploaderGui->AddTextToLogScreen("[BootloaderSM] Going into CONNECTED state\n");
                            break;
                        }
                    }
                    break;
                }
            }

            break;
        }
        case BOOTLOADER_INTERFACE_SM_BOOTLDR_CONNECTED_STATE:
        {
            switch(Event)
            {
                case BOOTLOADER_UPLOAD_SM_TICK_EVENT:
                {
                    break;
                }
                case BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT:
                {
                    switch(Cmd)
                    {
                        case BOOTLOADER_UPLOAD_ERASE_FLASH_GUI_CMD:
                        {
                            SendSingleCmdRequest(BOOTLOADER_ERASE_BOOTLOADER_FLASH_REQUEST);
                            mBootloaderInterfaceSMState = BOOTLOADER_INTERFACE_SM_ERASE_FLASH_STATE;
                            mPICUploaderGui->AddTextToLogScreen("[BootloaderSM] Going into ERASE_FLASH state\n");
                            break;
                        }
                        case BOOTLOADER_UPLOAD_START_FULL_UPDATE_GUI_CMD:
                        {
                            break;
                        }
                        case BOOTLOADER_UPLOAD_START_UPLOAD_GUI_CMD:
                        {
                            SendSingleCmdRequest(BOOTLOADER_INIT_UPLOAD_REQUEST);
                            mBootloaderInterfaceSMState = BOOTLOADER_INTERFACE_SM_INIT_UPLOAD_STATE;
                            mPICUploaderGui->AddTextToLogScreen("[BootloaderSM] Going into INIT_UPLOAD state\n");
                            break;
                        }
                    }
                    break;
                }
            }
            break;
        }
        case BOOTLOADER_INTERFACE_SM_ERASE_FLASH_STATE:
        {
            switch(Event)
            {
                case BOOTLOADER_UPLOAD_SM_TICK_EVENT:
                {
                    break;
                }
                case BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT:
                {
                    switch(Cmd)
                    {
                        case BOOTLOADER_UPLOAD_ERASE_BOOTLOADER_FLASH_ACK_HOST_CMD:
                        {
                            if(CmdParam != BOOTLOADER_FLASH_ERASE_OK)
                            {
                                mPICUploaderGui->AddTextToLogScreen("Flash erase refused from host.  Going back to CONNECTED state\n");
                            }
                            else
                            {
                                mPICUploaderGui->AddTextToLogScreen("Device confirmed erasing flash in progress... \n");
                            }

                            break;
                        }
                        case BOOTLOADER_UPLOAD_ERASE_BOOTLOADER_FLASH_RESULT_HOST_CMD:
                        {
                            if(CmdParam != BOOTLOADER_FLASH_ERASE_OK)
                            {
                                mPICUploaderGui->AddTextToLogScreen("Flash erase failed.  Going back to CONNECTED state\n");
                                mBootloaderInterfaceSMState = BOOTLOADER_INTERFACE_SM_BOOTLDR_CONNECTED_STATE;
                            }
                            else
                            {
                                mPICUploaderGui->AddTextToLogScreen("Flash erase success. \n");
                                mBootloaderInterfaceSMState = BOOTLOADER_INTERFACE_SM_BOOTLDR_CONNECTED_STATE;
                                mPICUploaderGui->AddTextToLogScreen("[BootloaderSM] Going into CONNECTED state\n");
                            }

                            break;
                        }
                        default:
                        {

                        }
                    }
                    break;
                }
            }
            break;
        }
        case BOOTLOADER_INTERFACE_SM_INIT_UPLOAD_STATE:
        {
            switch(Event)
            {
                case BOOTLOADER_UPLOAD_SM_TICK_EVENT:
                {
                    break;
                }
                case BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT:
                {
                    switch(Cmd)
                    {
                        case BOOTLOADER_UPLOAD_INIT_UPLOAD_HOST_CMD:
                        {
                            if(CmdParam == BOOTLOADER_INIT_UPLOAD_SUCCESS)
                            {
                                //Host is in update mode.  Load the buffer size and wait for "Ready for data" CMD (stay in the same state)
                                mFirmwareUploadBufferSize = *(int*)Data;
                                mPICUploaderGui->AddTextToLogScreen(QString("Init upload accepted by host.  Frame size: %1\n").arg(mFirmwareUploadBufferSize));
                                mBootloaderInterfaceSMState = BOOTLOADER_INTERFACE_SM_WAIT_READY_FOR_DATA_FROM_HOST_STATE;
                                mPICUploaderGui->AddTextToLogScreen("[BootloaderSM] Going into READY_FOR_DATA state\n");
                            }
                            else
                            {
                                //Something's wrong with the host.  Abort upload...
                                //TODO: Make the GUI aware of this.
                                mPICUploaderGui->AddTextToLogScreen(QString("Init upload refused.  Error code: %1.  Going back to STANDBY state\n").arg(CmdParam));
                                ResetBooloaderInterfaceStateMachine();
                            }
                            break;
                        }
                    }
                    break;
                }
            }
            break;
        }
        case BOOTLOADER_INTERFACE_SM_WAIT_READY_FOR_DATA_FROM_HOST_STATE:
        {
            switch(Event)
            {
                case BOOTLOADER_UPLOAD_SM_TICK_EVENT:
                {
                    break;
                }
                case BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT:
                {
                    switch(Cmd)
                    {
                        case BOOTLOADER_UPLOAD_READY_FOR_DATA_HOST_CMD:
                        {
                            //Host is ready for data.  Start the upload state machine
                            mPICUploaderGui->AddTextToLogScreen("Host ready for data.  Starting upload\n");
                            BootloaderFirmwareUploadStateMachine(BOOTLOADER_FIRM_UPLD_CMD_EVENT,BOOTLOADER_FIRM_UPLD_INIT_UPLOAD_CMD);
                            mBootloaderInterfaceSMState = BOOTLOADER_INTERFACE_SM_SEND_DATA_STATE;
                            break;
                        }
                        default:
                        {
                            mPICUploaderGui->AddTextToLogScreen(QString("Invalid response from host received int <WAIT READY FOR DATA> state.  Response Cmd received: %1.  Going back to STANDBY state\n").arg(Cmd));
                            ResetBooloaderInterfaceStateMachine();
                            break;
                        }

                    }
                    break;
                }
            }

            break;
        }
        case BOOTLOADER_INTERFACE_SM_SEND_DATA_STATE:
        {
            switch(Event)
            {
                case BOOTLOADER_UPLOAD_SM_TICK_EVENT:
                {
                    break;
                }
                case BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT:
                {
                    switch(Cmd)
                    {
                        case BOOTLOADER_UPLOAD_SEND_DATA_CHUNK_HOST_CMD:
                        {
                            int Res = BootloaderFirmwareUploadStateMachine(BOOTLOADER_FIRM_UPLD_CMD_EVENT,BOOTLOADER_FIRM_UPLD_CHUNK_RX_RESPONSE_CMD,CmdParam,*(int*)Data);
                            switch(Res)
                            {
                                case BOOTLOADER_FIRM_UPLD_SENDING_RES:
                                {
                                    //All is going well.
                                    break;
                                }
                                case BOOTLOADER_FIRM_UPLD_FINISHED_RES:
                                {
                                    mBootloaderInterfaceSMState = BOOTLOADER_INTERFACE_SM_DATA_FINISHED_STATE;
                                    mPICUploaderGui->AddTextToLogScreen("Bootloader upload state machine uploaded all firmware\n");
                                    break;
                                }
                                case BOOTLOADER_FIRM_UPLD_ERROR_RES:
                                {
                                    break;
                                }
                                case BOOTLOADER_FIRM_UPLD_TIMEOUT_RES:
                                {
                                    break;
                                }
                                case BOOTLOADER_FIRM_UPLD_ABORT_RES:
                                {
                                    break;
                                }
                                case BOOTLOADER_FIRM_UPLD_OK_RES:
                                {
                                    break;
                                }
                            }

                            break;
                        }
                    }
                    break;
                }
            }
            break;
        }
        case BOOTLOADER_INTERFACE_SM_DATA_FINISHED_STATE:
        {
            switch(Event)
            {
                case BOOTLOADER_UPLOAD_SM_TICK_EVENT:
                {
                    break;
                }
                case BOOTLOADER_UPLOAD_SM_NEW_CMD_EVENT:
                {
                    switch(Cmd)
                    {
                        case BOOTLOADER_UPLOAD_BOOTLOADER_CONNECTED_CMD:
                        {
                            break;
                        }
                        case BOOTLOADER_UPLOAD_UPLOAD_FINISHED_HOST_CMD:
                        {
                            mBootloaderInterfaceSMState = BOOTLOADER_INTERFACE_SM_BOOTLDR_CONNECTED_STATE;
                            break;
                        }
                    }
                    break;
                }
            }
            break;
        }

    }


    return RET_OK;
}

//State machine managing firmware packets upload and errors
int CPICUploader::BootloaderFirmwareUploadStateMachine(int Event, int Cmd, int CmdResult, int CmdParam)
{
    switch(mBootloaderFirmwareUpldState)
    {
        case BOOTLOADER_FIRM_UPLD_STANDBY_STATE:
        {
            switch(Event)
            {
                case BOOTLOADER_FIRM_UPLD_TICK_EVENT:
                {
                    break;
                }
                case BOOTLOADER_FIRM_UPLD_CMD_EVENT:
                {
                    switch(Cmd)
                    {
                        case BOOTLOADER_FIRM_UPLD_INIT_UPLOAD_CMD:
                        {
                            //TODO: Send First chunk
                            bool Finished;
                            mFirmwareData = mHexFile.GetRawData();
                            SendCmdRequest(BOOTLOADER_SEND_DATA_CHUNK_REQUEST, GetCurDataChunk(Finished));
                            mPICUploaderGui->AddTextToLogScreen(QString("Upload SM: Sending first frame (%1)\n").arg(mFirmwareUploadCurFrame));
                            int NbChunks = mFirmwareData.size() / mFirmwareUploadBufferSize;
                            mPICUploaderGui->SetUploadProgressSettings(NbChunks);
                            mPICUploaderGui->ResetProgressBar();
                            if(Finished)
                            {
                                mPICUploaderGui->AddTextToLogScreen(QString("That was the last chunk.  Going into UPLOAD_FINISHED state\n"));
                                mBootloaderFirmwareUpldState = BOOTLOADER_FIRM_UPLD_FINISHED_STATE;
                            }
                            else
                            {
                                mBootloaderFirmwareUpldState = BOOTLOADER_FIRM_UPLD_SEND_CHUNK_STATE;
                            }
                            return BOOTLOADER_FIRM_UPLD_SENDING_RES;
                            break;
                        }
                        case BOOTLOADER_FIRM_UPLD_ABORT_UPLOAD_CMD:
                        {
                            ResetFirmwareUploadStateMachine();
                            return BOOTLOADER_FIRM_UPLD_ABORT_RES;
                            break;
                        }
                        default:
                        {
                            //TODO: Find out what to do with that...
                            return BOOTLOADER_FIRM_UPLD_ERROR_RES;
                            break;
                        }
                    }
                    break;
                }
            }

            break;
        }
//        case BOOTLOADER_FIRM_UPLD_WAIT_FOR_HOST_READY_STATE:
//        {
//            switch(Event)
//            {
//                case BOOTLOADER_FIRM_UPLD_TICK_EVENT:
//                {
//                    return BOOTLOADER_FIRM_UPLD_WAITING_FOR_HOST_RES;
//                    break;
//                }
//                case BOOTLOADER_FIRM_UPLD_CMD_EVENT:
//                {
//                    switch(Cmd)
//                    {
//                        case BOOTLOADER_FIRM_UPLD_INIT_UPLOAD_ACK_CMD:
//                        {
//                            if(CmdResult == BOOTLOADER_INIT_UPLOAD_SUCCESS)
//                            {
//                                //Host is in update mode.  Load the buffer size and wait for "Ready for data" CMD (stay in the same state)
//                                mFirmwareUploadBufferSize = CmdParam;
//                                return BOOTLOADER_FIRM_UPLD_WAITING_FOR_HOST_RES;
//                            }
//                            else
//                            {
//                                ResetFirmwareUploadStateMachine();
//                                return BOOTLOADER_FIRM_UPLD_ERROR_RES;
//                            }
//                            break;
//                        }
//                        case BOOTLOADER_FIRM_UPLD_HOST_READY_FOR_DATA_CMD:
//                        {
//                            //Host is ready for data.  Send the first chunk and go into next state (send chunk)
//                            //TODO: Send first data chunk
//                            mBootloaderFirmwareUpldState = BOOTLOADER_FIRM_UPLD_SEND_CHUNK_STATE;
//                            return BOOTLOADER_FIRM_UPLD_SENDING_RES;
//                            break;
//                        }
//                        default:
//                        {
//                            return BOOTLOADER_FIRM_UPLD_ERROR_RES;
//                            break;
//                        }
//                    }

//                    break;
//                }

//            }
//            break;
//        }
        case BOOTLOADER_FIRM_UPLD_SEND_CHUNK_STATE:
        {
            switch(Event)
            {
                case BOOTLOADER_FIRM_UPLD_TICK_EVENT:
                {
                    break;
                }
                case BOOTLOADER_FIRM_UPLD_CMD_EVENT:
                {
                    switch(Cmd)
                    {
                        case BOOTLOADER_FIRM_UPLD_CHUNK_RX_RESPONSE_CMD:
                        {
                            switch(CmdResult)
                            {
                                case BOOTLOADER_CHUNK_TRANSFER_SUCCESS:
                                {
                                    if(mFirmwareUploadCurFrame == 700 || mFirmwareUploadCurFrame == 800 || mFirmwareUploadCurFrame == 900)
                                    {
                                        int toto = 5;
                                    }
                                    bool Finished = false;
                                    mPICUploaderGui->AddTextToLogScreen(QString("Upload SM: Sending frame Nb: %1\n").arg(mFirmwareUploadCurFrame));
                                    mFirmwareUploadCurFrame++;
                                    SendCmdRequest(BOOTLOADER_SEND_DATA_CHUNK_REQUEST, GetCurDataChunk(Finished));
                                    mPICUploaderGui->TickProgressBar();
                                    if(Finished)
                                    {
                                        mPICUploaderGui->AddTextToLogScreen(QString("That was the last chunk.  Going into UPLOAD_FINISHED state\n"));
                                        mBootloaderFirmwareUpldState = BOOTLOADER_FIRM_UPLD_FINISHED_STATE;
                                    }

                                    break;
                                }
                                case BOOTLOADER_CHUNK_TRANSFER_ERROR_RESEND:
                                {
                                    //TODO: Resend current frame
                                    if(mFirmwareUploadCurFrame != CmdParam)
                                    {
                                        mPICUploaderGui->AddTextToLogScreen(QString("Upload of frame %1 failed. and frame index desynchronized (Curframe = %2).  Aborting and going to STANDBY mode\n").arg(CmdParam).arg(mFirmwareUploadCurFrame));
                                        ResetFirmwareUploadStateMachine();
                                        return BOOTLOADER_FIRM_UPLD_ERROR_RES;
                                    }
                                    bool Finished;
                                    mPICUploaderGui->AddTextToLogScreen(QString("Upload of frame %1 failed.  Trying to resend\n").arg(CmdParam));
                                    SendCmdRequest(BOOTLOADER_SEND_DATA_CHUNK_REQUEST, GetCurDataChunk(Finished));
                                    //TODO: Manage the number of failed attempts.

                                    break;
                                }
                                case BOOTLOADER_CHUNK_TRANSFER_ERROR_FLASH_FAILURE:
                                {
                                    ResetFirmwareUploadStateMachine();
                                    return BOOTLOADER_FIRM_UPLD_ERROR_RES;
                                    break;
                                }
                                case BOOTLOADER_CHUNK_TRANSFER_ERROR_INVALID_CHUNK_INDEX:
                                {
                                    ResetFirmwareUploadStateMachine();
                                    return BOOTLOADER_FIRM_UPLD_ERROR_RES;
                                    break;
                                }
                                case BOOTLOADER_CHUNK_TRANSFER_ERROR_FLASH_ERROR:
                                {
                                    ResetFirmwareUploadStateMachine();
                                    return BOOTLOADER_FIRM_UPLD_ERROR_RES;
                                    break;
                                }
                            }

                            return BOOTLOADER_FIRM_UPLD_SENDING_RES;
                            break;
                        }
                        case BOOTLOADER_FIRM_UPLD_ABORT_UPLOAD_CMD:
                        {
                            ResetFirmwareUploadStateMachine();
                            return BOOTLOADER_FIRM_UPLD_ABORT_RES;
                            break;
                        }
                        default:
                        {
                            ResetFirmwareUploadStateMachine();
                            break;
                        }
                    }
                    break;
                }
            }

            break;
        }
        case BOOTLOADER_FIRM_UPLD_FINISHED_STATE:
        {
            switch(Event)
            {
                case BOOTLOADER_FIRM_UPLD_TICK_EVENT:
                {
                    break;
                }
                case BOOTLOADER_FIRM_UPLD_CMD_EVENT:
                {
                    switch(Cmd)
                    {
                        case BOOTLOADER_FIRM_UPLD_CHUNK_RX_RESPONSE_CMD:
                        {switch(CmdResult)
                            {
                                case BOOTLOADER_CHUNK_TRANSFER_SUCCESS:
                                {
                                    //TODO: Send upload finished CMD to host


                                    mPICUploaderGui->AddTextToLogScreen(QString("Upload of last frame success.  Frame Nb %1\n").arg(CmdParam));
                                    mPICUploaderGui->TickProgressBar();

                                    return BOOTLOADER_FIRM_UPLD_FINISHED_RES;
                                    break;
                                }
                                case BOOTLOADER_CHUNK_TRANSFER_ERROR_RESEND:
                                {
                                    //TODO: Resend current frame
                                    if(mFirmwareUploadCurFrame != CmdParam)
                                    {
                                        //TODO: Manage this error... (Abort)
                                        ResetFirmwareUploadStateMachine();
                                        return BOOTLOADER_FIRM_UPLD_ERROR_RES;
                                    }
                                    bool Finished;
                                    mPICUploaderGui->AddTextToLogScreen(QString("Upload of frame %1 failed.  Trying to resend\n").arg(CmdParam));
                                    SendCmdRequest(BOOTLOADER_SEND_DATA_CHUNK_REQUEST, GetCurDataChunk(Finished));

                                    break;
                                }
                                case BOOTLOADER_CHUNK_TRANSFER_ERROR_FLASH_FAILURE:
                                {
                                    ResetFirmwareUploadStateMachine();
                                    return BOOTLOADER_FIRM_UPLD_ERROR_RES;
                                    break;
                                }
                                case BOOTLOADER_CHUNK_TRANSFER_ERROR_INVALID_CHUNK_INDEX:
                                {
                                    ResetFirmwareUploadStateMachine();
                                    return BOOTLOADER_FIRM_UPLD_ERROR_RES;
                                    break;
                                }
                                case BOOTLOADER_CHUNK_TRANSFER_ERROR_FLASH_ERROR:
                                {
                                    ResetFirmwareUploadStateMachine();
                                    return BOOTLOADER_FIRM_UPLD_ERROR_RES;
                                    break;
                                }
                            }
                            ResetFirmwareUploadStateMachine();

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

            break;
        }
    }

    return BOOTLOADER_FIRM_UPLD_OK_RES;

}

int CPICUploader::ResetBooloaderInterfaceStateMachine()
{
    mBootloaderInterfaceSMState = BOOTLOADER_INTERFACE_SM_STANDBY_STATE;
    mFirmwareUploadBufferSize = 0;
    mFirmwareUploadCurFrame = 0;
    ResetFirmwareUploadStateMachine();
    return RET_OK;
}

int CPICUploader::ResetFirmwareUploadStateMachine()
{
    mBootloaderFirmwareUpldState = BOOTLOADER_FIRM_UPLD_STANDBY_STATE;
    mFirmwareData.clear();
    return RET_OK;
}

//Sends current data chunk based on mFirmwareUploadCurFrame and mFirmwareUploadBufferSize.
QByteArray CPICUploader::GetCurDataChunk(bool &IsLastChunk)
{
    QByteArray Chunk;
    QByteArray Payload;
    Chunk.clear();

    unsigned int index = mFirmwareUploadCurFrame * mFirmwareUploadBufferSize;
    if((index + mFirmwareUploadBufferSize) >= mFirmwareData.size())
    {
        IsLastChunk = true;
    }

    Payload = mFirmwareData.mid(index,mFirmwareUploadBufferSize);

    Chunk.append(IntToByteArray(mFirmwareUploadCurFrame));
    Chunk.append(IntToByteArray(Payload.size()));
    Chunk.append(Payload);

    return Chunk;
}


QByteArray CPICUploader::IntToByteArray(int data)
{
    QByteArray Array;
    unsigned char nibble = (char)((data >> 24) &0x000000FF);
    Array.append(nibble);

    nibble = (char)((data >> 16) &0x000000FF);
    Array.append(nibble);

    nibble = (char)((data >> 8) &0x000000FF);
    Array.append(nibble);

    nibble = (char)(data &0x000000FF);
    Array.append(nibble);

    return Array;
}
