#include "Header_Files/udpserver.h"

//using namespace sbgtc;

UdpServer::UdpServer()
{
    m_udpSocket = new QUdpSocket(this);
    m_udpSocket->bind(45454, QUdpSocket::ShareAddress);
    connect(m_udpSocket, SIGNAL(readyRead()),this, SLOT(processPendingDatagrams()));

    m_sbgMagCibWorkThread = new sbgMagCibWorkThread();

    m_RecordSbgThread=new QThread();
    m_sbgRecorder=new sbgtc::SbgRecorder();
    m_sbgRecorder->moveToThread(m_RecordSbgThread);
    m_RecordSbgThread->start();

    m_RecordThread=new QThread();
    m_imager=new XimeaImager();
    m_imager->moveToThread(m_RecordThread);
    m_RecordThread->start();

    m_CopyFileThread=new QThread();
    m_copyFile=new FileOperation();
    m_copyFile->moveToThread(m_CopyFileThread);
    m_CopyFileThread->start();

    //系统采集步骤1:打开sbg串口并采集数据，打开光谱仪
//    connect(this, SIGNAL(systemStart()),m_sbgRecorder, SLOT(startRecordSbg()));
    connect(this, SIGNAL(systemStart()),m_imager, SLOT(openImger()));

    connect(this, SIGNAL(systemStop()),m_sbgRecorder, SLOT(closeSerialPort()));//
    connect(this, SIGNAL(systemStop()),m_imager, SLOT(closeImger()));


    //系统采集步骤2:开始采集高光谱影像
    connect(this, SIGNAL(startRecordHyperspectralSignal()),m_sbgRecorder, SLOT(startRecordHyperspectral()));
    connect(m_sbgRecorder, SIGNAL(sbgReady(double,QString)),m_imager, SLOT(startRecord(double,QString)));
    connect(this, SIGNAL(recordXimeaOnlySignal(double,QString)),m_imager, SLOT(startRecord(double,QString)));

    //系统采集步骤3:停止采集
    connect(m_imager, SIGNAL(recordFinished()),this, SLOT(onRecordFinished()));

    //系统采集步骤4:拷贝文件
    connect(this, SIGNAL(startCopyFileSignal()),m_copyFile, SLOT(copyFile()));
    connect(this, SIGNAL(startDeleteFileSignal()),m_copyFile, SLOT(deleteFile()));

    //系统采集步骤5:进程间通讯
    connect(m_sbgRecorder, SIGNAL(serialPortStatus(int)),this, SLOT(sendSerialPortStatus(int)));

    connect(m_sbgRecorder, SIGNAL(sbgSolutionModeSignal(int)),this, SLOT(sendSbgSolutionModeState(int)));
    connect(m_sbgRecorder, SIGNAL(sbgAccuracySignal(int)),this, SLOT(sendSbgAccuracyState(int)));

    connect(m_imager, SIGNAL(ximeaImageStatus(int)),this, SLOT(sendXimeaImageStatus(int)));
    connect(m_sbgMagCibWorkThread, SIGNAL(magCalibStateSignal(int)),this, SLOT(sendSbgMagCalibState(int)));
    connect(m_copyFile, SIGNAL(copyFileStatus(int)),this, SLOT(sendCopyFileStatus(int)));



    //当软件不正常关闭并且重启后，通知其他psdk程序
    m_clientIpAddress=QHostAddress(QHostAddress::LocalHost);
    sendSerialPortStatus(0);
    sendXimeaImageStatus(0);
    sendSbgMagCalibState(0);
    sendCopyFileStatus(0);




    std::cout<<"UdpServer::UdpServer--------:System ready!"<<std::endl;
}

void UdpServer::processPendingDatagrams()
{
    using namespace std;
    while (m_udpSocket->hasPendingDatagrams())
    {
        QByteArray datagram;

        datagram.resize(m_udpSocket->pendingDatagramSize());
        m_udpSocket->readDatagram(datagram.data(), datagram.size(),&m_clientIpAddress, &m_clientPort);


        printf("接收数据字节数: %d.\n",datagram.size());

        QList<QByteArray> datagramList=datagram.split(',');
        printf("有多少个list: %d.\n",datagramList.size());

        QString instruction=datagramList[0].data();// QByteArray转QString方法1
        switch (instruction.toInt())
        {
        case 1://启动系统: 打开sbg串口并采集数据，打开光谱仪
        {
            std::cout<<"1代表启动系统!"<<std::endl;
            emit systemStart();

            break;
        }
        case 2://关闭系统:关闭相机和sbg串口,关闭软件
        {
            std::cout<<"2代表关闭系统!"<<std::endl;


            if(m_sbgRecorder->getSbgState()>=1)
            {
                m_sbgRecorder->stopRecordSbg();
            }


            if(m_imager->getImagerState()>=1)
            {
                m_imager->stopRecord();
            }

            emit systemStop();

            //QCoreApplication::quit();
            break;
        }
        case 3://系统开始采集高光谱影像
        {
            //emit startRecordHyperspectralSignal();//真实的影像开始采集通过惯导中的信号(sbgReady)触发
            m_sbgRecorder->startRecordHyperspectral();

//            if(m_sbgRecorder->getSbgState()==2)//开始采集前还需要判断相机的状态??????????????????????????????????????????
//            {

//            }
//            else if(m_sbgRecorder->getSbgState()==3)
//            {
//                std::cout<<"系统已经开始采集!"<<std::endl;
//            }

            break;
        }
        case 4://系统停止采集高光谱影像
        {
            std::cout<<"4代表系统停止采集高光谱影像!"<<std::endl;

            if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=4)
            {
                m_imager->stopRecord();
            }

            break;
        }
        case 5://
        {
            if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=3)
            {
                std::cout<<"5代表设置帧率!"<<std::endl;
                m_imager->setFramerate(datagramList[1].toFloat());
            }

            break;
        }
        case 6://
        {
            if(m_imager->getImagerState()>=1 && m_imager->getImagerState()<=3)
            {
                std::cout<<"6代表自动曝光!"<<std::endl;
                m_imager->autoExposure();
            }

            break;
        }
        case 7:
        {
            if(datagramList[1].toInt()==1)
            {
                std::cout<<"拷贝数据!"<<std::endl;

                emit startCopyFileSignal();
            }
            else if(datagramList[1].toInt()==0)
            {
                std::cout<<"删除数据!"<<std::endl;

                emit startDeleteFileSignal();
            }

            break;
        }
        case 8:
        {
            std::cout<<"8代表磁场矫正!"<<std::endl;

            if(datagramList[1].toInt()==1)
            {
                //magCalib();
                std::cout<<"8-1: 开始磁场矫正!"<<std::endl;

                m_sbgMagCibWorkThread->start();
            }
            else if(datagramList[1].toInt()==0)
            {
                std::cout<<"8-0: 停止磁场矫正!"<<std::endl;

                m_sbgMagCibWorkThread->m_iMagCalibStopControl=0;
            }

            break;
        }
        case 9:
        {
            std::cout<<"9代表仅采集影像!"<<std::endl;

            if(datagramList[1].toInt()==1)
            {
                QString xx = getFileNameBaseOnTime();
                xx = xx + "testImage";
                emit recordXimeaOnlySignal(1000.0,xx);

//                emit recordXimeaOnlySignal(1000.0,"testImage");
            }
            else if(datagramList[1].toInt()==0)
            {
                m_imager->stopRecord();
            }

            break;
        }
        default:
            std::cout<<">=9没有意义!"<<std::endl;

            break;
        }
    }
}

double UdpServer::getTimeDifferenceBetweenSystemAndSbg(double secondSbg)
{
//    time_t timer;//time_t就是long int 类型
//    struct tm *tblock;
//    timer = time(NULL);//返回秒数(精度为秒)，从1970-1-1,00:00:00 可以当成整型输出或用于其它函数
//    tblock = localtime(&timer);
//    printf("Local time is: %s\n", asctime(tblock));



    //https://blog.csdn.net/FUN6367/article/details/89787566
    struct timespec systemTime;
    clock_gettime(CLOCK_REALTIME,&systemTime);
    tm systemTime_rili;
    localtime_r(&systemTime.tv_sec, &systemTime_rili);

//    std::cout<<"systemTime_rili--年: "<<systemTime_rili.tm_year+1900<<std::endl;
//    std::cout<<"systemTime_rili--月: "<<systemTime_rili.tm_mon+1<<std::endl;
//    std::cout<<"systemTime_rili--日: "<<systemTime_rili.tm_mday<<std::endl;
//    std::cout<<"systemTime_rili--时: "<<systemTime_rili.tm_hour<<std::endl;
//    std::cout<<"systemTime_rili--分: "<<systemTime_rili.tm_min<<std::endl;
//    std::cout<<"systemTime_rili--秒: "<<systemTime_rili.tm_sec<<std::endl;

//    printf("Local time is: %s\n", asctime(&systemTime_rili));



    double secondSystem=(systemTime_rili.tm_mday-1)*24*60*60+systemTime_rili.tm_hour*60*60+systemTime_rili.tm_min*60+systemTime_rili.tm_sec;
    double nanosecondSystem=secondSystem+static_cast<double>(systemTime.tv_nsec)/1000000000;

    printf("UdpServer::getTimeDifferenceBetweenSystemAndSbg------%f\n", nanosecondSystem-secondSbg);

    return nanosecondSystem-secondSbg;
}

void UdpServer::sender(int status)
{
    QByteArray datagram2send;

    datagram2send.operator =(QString::number(status).toStdString().c_str());
    m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, m_clientPort+1);
}

void UdpServer::sendSerialPortStatus(int serialPortStatus)
{
    std::cout<<"UdpServer::sendSerialPortStatus---------------------:"<< serialPortStatus <<std::endl;

    std::cout<<"UdpServer::sendSerialPortStatus---------------------:"<< m_clientIpAddress.AnyIPv4 <<std::endl;

    QByteArray datagram2send;

    QString status = "sbg," + QString::number(serialPortStatus);

    datagram2send.operator =(status.toStdString().c_str());
    m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}

void UdpServer::sendSbgMagCalibState(int SbgMagCalibState)
{
    std::cout<<"UdpServer::sendSbgMagCalibState---------------------:"<< SbgMagCalibState <<std::endl;

    QByteArray datagram2send;

    QString status = "mag," + QString::number(SbgMagCalibState);

    datagram2send.operator =(status.toStdString().c_str());
    m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}

void UdpServer::sendSbgSolutionModeState(int SolutionMode)
{
    std::cout<<"UdpServer::sendSbgSolutionModeState---------------------:"<< SolutionMode <<std::endl;

    QByteArray datagram2send;

    QString status = "SolutionMode," + QString::number(SolutionMode);

    datagram2send.operator =(status.toStdString().c_str());
    m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}

void UdpServer::sendSbgAccuracyState(int Accuracy)
{
//    std::cout<<"UdpServer::sendSbgAccuracyState---------------------:"<< Accuracy <<std::endl;

    QByteArray datagram2send;

    QString status = "Accuracy," + QString::number(Accuracy);

    datagram2send.operator =(status.toStdString().c_str());
    m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}

void UdpServer::sendXimeaImageStatus(int ximeaImageStatus)
{
    std::cout<<"UdpServer::sendXimeaImageStatus---------------------:"<< ximeaImageStatus <<std::endl;

    QByteArray datagram2send;

    QString status = "ximea," + QString::number(ximeaImageStatus);

    datagram2send.operator =(status.toStdString().c_str());
    m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}

void UdpServer::sendCopyFileStatus(int fileStatus)
{
    std::cout<<"UdpServer::sendCopyFileStatus---------------------:"<< fileStatus <<std::endl;

    QByteArray datagram2send;

    QString status = "file," + QString::number(fileStatus);

    datagram2send.operator =(status.toStdString().c_str());
    m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455);
}

void UdpServer::onRecordFinished()
{
    std::cout<<"UdpServer::onRecordFinished----------------:影像停止采集"<<std::endl;
}

sbgMagCibWorkThread::sbgMagCibWorkThread()
{

}

void sbgMagCibWorkThread::displayMagCalibResults(SbgEComMagCalibMode mode, const SbgEComMagCalibResults *pMagCalibResults)
{
    //
    // Display the magnetic calibration results
    //
    printf("\n\n======== Magnetic calibration report ========\n");

    //
    // Convert the quality indicator to human readable output
    //
    switch (pMagCalibResults->quality)
    {
    case SBG_ECOM_MAG_CALIB_QUAL_OPTIMAL:
    {
        printf("Quality:\t\toptimal\n");
        signalWrap(7);
        break;
    }
    case SBG_ECOM_MAG_CALIB_QUAL_GOOD:
    {
        printf("Quality:\t\tgood\n");
        signalWrap(6);
        break;
    }
    case SBG_ECOM_MAG_CALIB_QUAL_POOR:
    {
        printf("Quality:\t\tpoor\n");
        signalWrap(5);
        break;
    }
    default:
        printf("Quality:\t\tundefined\n");
    }

    //
    // Convert the confidence indicator to human readable output
    //
    switch (pMagCalibResults->confidence)
    {
    case SBG_ECOM_MAG_CALIB_TRUST_HIGH:
        printf("Confidence:\t\thigh\n");
        break;
    case SBG_ECOM_MAG_CALIB_TRUST_MEDIUM:
        printf("Confidence:\t\tmedium\n");
        break;
    case SBG_ECOM_MAG_CALIB_TRUST_LOW:
        printf("Confidence:\t\tlow\n");
        break;
    default:
        printf("Confidence:\t\tundefined\n");
    }

    //
    // Print advanced status
    //
    printf("Advanced Status:\n");
    if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS)
    {
        printf("\t- Not enough valid points. Maybe you are moving too fast.\n");
    }
    if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS)
    {
        printf("\t- Unable to find a calibration solution. Maybe there are too much non static distortions.\n");
    }
    if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE)
    {
        printf("\t- The magnetic calibration has troubles to correct the magnetometers and inertial frame alignment.\n");
    }

    //
    // Test if we have a 2D or 3D calibration mode
    //
    if (mode == SBG_ECOM_MAG_CALIB_MODE_2D)
    {
        //
        // In 2D mode, a X or Y motion issue means we have too much motion
        //
        if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE)
        {
            printf("\t- Too much roll motion for a 2D magnetic calibration.\n");
        }
        if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE)
        {
            printf("\t- Too much pitch motion for a 2D magnetic calibration.\n");
        }
    }
    else
    {
        //
        // In 3D mode, a X or Y motion issue means we have not enough motion
        //
        if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE)
        {
            printf("\t- Not enough roll motion for a 3D magnetic calibration.\n");
        }
        if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE)
        {
            printf("\t- Not enough pitch motion for a 3D magnetic calibration.\n");
        }
    }

    //
    // Test if we had enough yaw motion to compute a calibration
    //
    if (pMagCalibResults->advancedStatus & SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE)
    {
        //
        // Test if we are in
        printf("\t- Not enough yaw motion to compute a valid magnetic calibration.\n");
    }

    //
    // Display the number of points used to compute the magnetic calibration
    //
    printf("\n\n");
    printf("Used Points:\t%u\n", pMagCalibResults->numPoints);
    printf("Max Points:\t%u\n", pMagCalibResults->maxNumPoints);

    //
    // Display magnetic field deviation errors
    //
    printf("\n\n-------------------------------------\n");
    printf("Magnetic field deviation report\n");
    printf("-------------------------------------\n");
    printf("\t\tMean\tStd\tMax\n");
    printf("Before\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->beforeMeanError, pMagCalibResults->beforeStdError, pMagCalibResults->beforeMaxError);
    printf("After\t\t%0.2f\t%0.2f\t%0.2f\n", pMagCalibResults->afterMeanError, pMagCalibResults->afterStdError, pMagCalibResults->afterMaxError);
    printf("Accuracy (deg)\t%0.2f\t%0.2f\t%0.2f\n", sbgRadToDegF(pMagCalibResults->meanAccuracy), sbgRadToDegF(pMagCalibResults->stdAccuracy), sbgRadToDegF(pMagCalibResults->maxAccuracy));
    printf("\n\n\n");
}

void sbgMagCibWorkThread::run()
{
    m_iMagCalibStopControl = 1;


    SbgEComHandle			comHandle;
    SbgErrorCode			errorCode;
    SbgInterface			sbgInterface;
    int32					retValue = 0;
    SbgEComDeviceInfo		deviceInfo;
    SbgEComMagCalibResults	magCalibResults;
    SbgEComMagCalibMode		mode;

    //
    // Create an interface:
    // We can choose either a serial for real time operation, or file for previously logged data parsing
    // Note interface closing is also differentiated !
    //
    errorCode = sbgInterfaceSerialCreate(&sbgInterface, "/dev/sbg_serial_port", 460800);		// Example for Unix using a FTDI Usb2Uart converter
    //errorCode = sbgInterfaceSerialCreate(&sbgInterface, "COM23", 115200);								// Example for Windows serial communication

    //
    // Test that the interface has been created
    //
    if (errorCode == SBG_NO_ERROR)
    {
        //
        // Create the sbgECom library and associate it with the created interfaces
        //
        errorCode = sbgEComInit(&comHandle, &sbgInterface);

        //
        // Test that the sbgECom has been initialized
        //
        if (errorCode == SBG_NO_ERROR)
        {
            printf("sbgECom properly Initialized.\n");
            printf("sbgECom version %s\n\n", SBG_E_COM_VERSION_STR);

            //
            // Get device information
            //
            errorCode = sbgEComCmdGetInfo(&comHandle, &deviceInfo);

            //
            // Display device information if no error
            //
            if (errorCode == SBG_NO_ERROR)
            {
                printf("Device : %0.9u found\n", deviceInfo.serialNumber);
            }
            else
            {
                fprintf(stderr, "ellipseOnboardMagCalib: Unable to get device information.\n");
            }

            //
            // Define the calibration mode to perform
            //
            mode = SBG_ECOM_MAG_CALIB_MODE_3D;

            //
            // Start / reset the acquisition of magnetic field data
            // Each time this command is called, the device is prepared to acquire a new set of magnetic field data
            // You have to specify here if the magnetic field data acquisition will be used to compute a 2D or 3D calibration
            //
            errorCode = sbgEComCmdMagStartCalib(&comHandle, mode, SBG_ECOM_MAG_CALIB_HIGH_BW);

            //
            // Make sure that the magnetic calibration has started
            //
            if (errorCode == SBG_NO_ERROR)
            {
                //
                // The device is now acquiring some magnetic field data.
                // Wait for a user input before computing the magnetic calibration
                //
                printf("The device is acquiring magnetic field data.\n\nPress enter to stop the magnetic field acquisition.\n");
                //fgetc(stdin);//-------------------------------------------------------------------------------------------------------------------------------------------
                while (m_iMagCalibStopControl)
                {
                    printf("h");

                }

                printf("kkkkkkkkkkkkkkkkkkkkkkkkkkkk\n");

                //
                // Try to compute a magnetic calibration and get the results
                //
                errorCode = sbgEComCmdMagComputeCalib(&comHandle, &magCalibResults);

                //
                // Make sure that we were able to get magnetic calibration results
                //
                if (errorCode == SBG_NO_ERROR)
                {
                    //
                    // Test if the device has computed a valid magnetic calibration
                    //
                    if (magCalibResults.quality != SBG_ECOM_MAG_CALIB_QUAL_INVALID)
                    {
                        //
                        // Send the new magnetic calibration data
                        //
                        errorCode = sbgEComCmdMagSetCalibData(&comHandle, magCalibResults.offset, magCalibResults.matrix);

                        //
                        // Make sure that the new magnetic calibration data has been updated
                        //
                        if (errorCode == SBG_NO_ERROR)
                        {
                            printf("The new magnetic calibration has been applied.\n");

                            //
                            // Display the magnetic calibration status
                            //
                            displayMagCalibResults(mode, &magCalibResults);
                        }
                        else
                        {
                            fprintf(stderr, "ellipseOnboardMagCalib: Unable to upload new magnetic calibration data.\n");

                            signalWrap(4);
                        }
                    }
                    else
                    {
                        //
                        // Unable to compute a valid magnetic calibration
                        //
                        fprintf(stderr, "ellipseOnboardMagCalib: Unable to compute a valid magnetic calibration.\n");

                        signalWrap(3);
                    }
                }
                else
                {
                    fprintf(stderr, "ellipseOnboardMagCalib: Unable to get onboard magnetic calibration results.\n");

                    signalWrap(2);
                }
            }
            else
            {
                fprintf(stderr, "ellipseOnboardMagCalib: Unable to start the onboard magnetic calibration.\n");

                signalWrap(1);
            }

            //
            // Close the sbgEcom library
            //
            sbgEComClose(&comHandle);
        }
        else
        {
            //
            // Unable to initialize the sbgECom
            //
            fprintf(stderr, "ellipseOnboardMagCalib: Unable to initialize the sbgECom library.\n");
            retValue = -1;

            signalWrap(0);
        }

        //
        // Close the interface
        //
        sbgInterfaceSerialDestroy(&sbgInterface);
    }
    else
    {
        //
        // Unable to create the interface
        //
        fprintf(stderr, "ellipseOnboardMagCalib: Unable to create the interface.\n");
        retValue = -1;

        signalWrap(0);
    }

    printf("magCalib process complete!\n");
    //fgetc(stdin);
}

void sbgMagCibWorkThread::signalWrap(int state)
{
    std::cout<<"sbgMagCibWorkThread::signalWrap---------------------:"<< state <<std::endl;


    m_iMagCalibState=state;
    emit magCalibStateSignal(m_iMagCalibState);
}