#include "Hd_3DCameraLMI_module.h"
#include <QDebug>
QQueue<std::vector<cv::Mat>> queuePic;
QMutex* m_mutex;
bool recivingImage = false,frameStart = false;
int lastFrameId = -1 ;
long long frameStampCountStart = 0, frameStampCountEnd = 0;
std::vector<long long>frameVec;
//0106 断连提示
void _stdcall ExceptionCB(unsigned int nMesgType, void* pUser) {
    mv_Status = "disconnect";
    qDebug() << "[Error] " << " LMI camera disconnects!";
}

#pragma region LMI

// Data callback function
// This function is called from a separate thread spawned by the GoSDK library.
// Processing within this function should be minimal.
kStatus kCall onData(void* ctx, void* sys, void* dataset)
{
    double time_Start = (double)clock();
    unsigned int i, j;
    Mat heightMat1, heightMat2;//高度图
    Mat luminanceMat1, luminanceMat2;//灰度图
    //DataContext* context = (DataContext*)ctx;

    std::vector<cv::Mat> picVec;
    //qDebug() << "GoDataSet_Count(dataset):" << GoDataSet_Count(dataset);
    for (i = 0; i < GoDataSet_Count(dataset); ++i)
    {
        GoDataMsg dataObj = GoDataSet_At(dataset, i);
        auto type = GoDataMsg_Type(dataObj);
        switch (type)
        {
        case GO_DATA_MESSAGE_TYPE_STAMP:
        {
            GoStampMsg stampMsg = (GoStampMsg)dataObj;
            // 遍历每个时间戳

                for (kSize j = 0; j < GoStampMsg_Count(stampMsg); ++j)
                {
                    GoStamp* stamp = GoStampMsg_At(stampMsg, j);
                    // 打印触发时间戳
                    if(stamp->frameIndex != lastFrameId)//进行到下一帧
                    {
                        
                        qDebug() << "lastFrameId:" << lastFrameId
                            << "frameStampCountStart:" << frameStampCountStart
                            << "ms frameStampCountEnd:" << frameStampCountEnd << "ms";
                        QString frameList;
                        for (auto value : frameVec)
                            frameList.append(QString::number(value)+",");
                        frameList.append(QString::number(frameVec.size()));
                        qDebug() << "frameVec:" << frameList;
                        frameStampCountStart = stamp->timestamp / 1024;

                        qDebug() << "Start reciving Frame ID: " << stamp->frameIndex
                            << ", Timestamp: " << stamp->timestamp / 1024 << " ms"
                            << ", Encoder: " << stamp->encoder;
                            //<< ",encoderAtZ:" << stamp->encoderAtZ
                            //<< ", status: " << stamp->status
                            //<< ",id:" << stamp->id
                            //<< ", reserved32u: " << stamp->reserved32u
                            //<< ",reserved64u:" << stamp->reserved64u
                            //<< ", status: " << stamp->status
                            //<< ",ptpTime:" << stamp->ptpTime
                            //<< std::endl;
                        lastFrameId = stamp->frameIndex;
                        frameStampCountStart = stamp->timestamp / 1024;
                        frameVec.push_back(stamp->timestamp / 1024);
                    }
                    else
                    {
                        frameStampCountEnd = stamp->timestamp / 1024;
                        frameVec.push_back(stamp->timestamp / 1024);
                    }
                }
                //frameStart = true;
            
            break;
        }
        case GO_DATA_MESSAGE_TYPE_UNIFORM_SURFACE:
        {
            recivingImage = true;
            MyTiff temp;
            GoUniformSurfaceMsg surfaceMsg = dataObj;
            double XResolution = NM_TO_MM(GoSurfaceMsg_XResolution(surfaceMsg));
            double YResolution = NM_TO_MM(GoSurfaceMsg_YResolution(surfaceMsg));
            double ZResolution = NM_TO_MM(GoSurfaceMsg_ZResolution(surfaceMsg));
            double XOffset = UM_TO_MM(GoSurfaceMsg_XOffset(surfaceMsg));
            double YOffset = UM_TO_MM(GoSurfaceMsg_YOffset(surfaceMsg));
            double ZOffset = UM_TO_MM(GoSurfaceMsg_ZOffset(surfaceMsg));

            int Width = GoSurfaceMsg_Width(surfaceMsg);
            int Height = GoSurfaceMsg_Length(surfaceMsg);

            temp.Tiff = new unsigned short[Width * Height * sizeof(unsigned short)];
            temp.w = Width;
            temp.h = Height;
            for (int rowIdx = 0; rowIdx < Height; rowIdx++)
            {
                k16s* data = GoSurfaceMsg_RowAt(surfaceMsg, rowIdx);
                for (int colIdx = 0; colIdx < Width; colIdx++)
                {   
                    unsigned short val = data[colIdx] - 0x8000;
                    //unsigned short val = data[colIdx];
                    //unsigned short val = ZOffset + ZResolution * data[colIdx];
                    temp.Tiff[rowIdx * Width + colIdx] = val;
                }
            }

            heightMat1 = cv::Mat(temp.h, temp.w, CV_16U, temp.Tiff);
            heightMat1.copyTo(heightMat2);
            
            m_lmiheightFlag = true;
            delete[]temp.Tiff;
            temp.Tiff = nullptr;
            /*cv::Mat dst;
            SYSTEMTIME t;
            std::vector<int> params{ (int)cv::IMWRITE_TIFF_RESUNIT,2, (int)cv::IMWRITE_TIFF_COMPRESSION,1,(int)cv::IMWRITE_TIFF_XDPI,300,(int)cv::IMWRITE_TIFF_YDPI,300 };
            GetLocalTime(&t);
            std::string filename = std::to_string(t.wHour) + "-" + std::to_string(t.wMinute) + "-" + std::to_string(t.wSecond) + "-" + std::to_string(t.wMilliseconds) + ".tiff";
            cv::imwrite(filename, img, params);*/
        }
            break;
        case GO_DATA_MESSAGE_TYPE_SURFACE_INTENSITY:
        {
            recivingImage = true;
            MyBmp temp;
            GoSurfaceIntensityMsg surfaceIntMsg = dataObj;
            //double ZResolution = NM_TO_MM(GoSectionMsg_ZResolution(surfaceIntMsg));
            double XResolution = NM_TO_MM(GoSurfaceIntensityMsg_XResolution(surfaceIntMsg));
            double YResolution = NM_TO_MM(GoSurfaceIntensityMsg_YResolution(surfaceIntMsg));
            double XOffset = UM_TO_MM(GoSurfaceIntensityMsg_XOffset(surfaceIntMsg));
            double YOffset = UM_TO_MM(GoSurfaceIntensityMsg_YOffset(surfaceIntMsg));

            int Width = GoSurfaceIntensityMsg_Width(surfaceIntMsg);
            int Height = GoSurfaceIntensityMsg_Length(surfaceIntMsg);

            temp.Bmp = new unsigned char[Width * Height * sizeof(unsigned char)];
            temp.w = Width;
            temp.h = Height;

            for (int rowIdx = 0; rowIdx < Height; rowIdx++)
            {
                k8u* data = GoSurfaceIntensityMsg_RowAt(surfaceIntMsg, rowIdx);
                for (int colIdx = 0; colIdx < Width; colIdx++)
                {
                    temp.Bmp[rowIdx * Width + colIdx] = data[colIdx];
                }
            }

            luminanceMat1 = cv::Mat(temp.h, temp.w, CV_8UC1, temp.Bmp);
            luminanceMat1.copyTo(luminanceMat2);
            m_lmiluminanceFlag = true;
            delete[]temp.Bmp;
            temp.Bmp = nullptr;
            //SYSTEMTIME t;
            //GetLocalTime(&t);
            //std::string filename = std::to_string(t.wHour) + "-" + std::to_string(t.wMinute) + "-" + std::to_string(t.wSecond) + "-" + std::to_string(t.wMilliseconds) + ".bmp";
            //cv::imwrite(filename, img);
        }
            break;
        }
        //DataSetCollection.push_back(dataObj);
    }
    if (recivingImage == false)
        return true;
    recivingImage = false;
    frameStart = false;
    if (!luminanceMat2.empty())
    {
        picVec.push_back(luminanceMat2);
    }
    else
    {
        luminanceMat2 = imread("C://empty.png");
        picVec.push_back(luminanceMat2);
    }
    if( !heightMat2.empty())
    {
        picVec.push_back(heightMat2);
    }
    else
    {
        heightMat2 = imread("C://empty2.png");
        picVec.push_back(heightMat2);
    }
    m_mutex->lock();
    if(picVec.size() > 0)
        queuePic.push_front(picVec);
    m_mutex->unlock();
    heightMat1.release();
    heightMat2.release();
    luminanceMat1.release();
    luminanceMat2.release();
    picVec.clear();
    GoDestroy(dataset);
    double time_End = (double)clock();
    qDebug() << "[INFO] " "image pic:" << picIndex++ << " srcImage Exist,Time:" << (time_End - time_Start) << "ms" << "--CameraName:" << id;

    return kOK;
}


bool InitLmi(k32u ID)
{
    
    //return true;//edit
    kStatus status;
    if ((status = GoSdk_Construct(&api)) != kOK)
    {
        //printf("Error: GoSdk_Construct:%d\n", status);
        qDebug() << "[Error] "  << "Error: GoSdk_Construct:%d\n", status;
        return false;
    }

    if ((status = GoSystem_Construct(&system1, kNULL)) != kOK)
    {
        //printf("Error: GoSystem_Construct:%d\n", status);
        qDebug() << "[Error] "  << "Error: GoSystem_Construct:%d\n", status;
        return false;
    }

    if ((status = GoSystem_FindSensorById(system1, ID, &sensor)) != kOK)
    {
        //printf("Error: GoSystem_FindSensor:%d\n", status);
        qDebug() << "[Error] "  << "Error: GoSystem_FindSensor:%d\n", status;
        return false;
    }

    /*GoSystem_Disconnect(system1);*/
    // create connection to GoSystem object
    if ((status = GoSensor_Connect(sensor)) != kOK)
    {
        //ExceptionCB(status, NULL);
        qDebug() << "[Error] " << "Error: GoSensor_Connect:%d\n", status;
        return false;
    }

    // enable sensor data channel
    if ((status = GoSystem_EnableData(system1, kTRUE)) != kOK)
    {
        //printf("Error: GoSensor_EnableData:%d\n", status);
        qDebug() << "[Error] "<< "Error: GoSensor_EnableData:%d\n", status;
        return false;
    }

    // set data handler to receive data asynchronously
    if ((status = GoSystem_SetDataHandler(system1, onData, contextPointer)) != kOK)
    {
        //printf("Error: GoSystem_SetDataHandler:%d\n", status);
        qDebug() << "[Error] "  << "Error: GoSystem_SetDataHandler:%d\n", status;
        return false;
    }
    
    if ((status = GoSystem_SetDataCapacity(system1, kSIZE_MAX)) != kOK)
    {
        qDebug() << "[Error] " << "Error: GoSystem_SetDataCapacity:%d\n", status;
    }
    return true;
}

bool StartLmi()
{

    kStatus status;
    if ((status = GoSystem_Start(system1)) != kOK)
    {
        //printf("Error: GoSystem_Stop:%d\n", status);
        qDebug() << "[Error] "  << "Failed to start LMI ";
        return false;
    }
    return true;
}

bool StopLmi()
{
    kStatus status;
    if ((status = GoSystem_Stop(system1)) != kOK)
    {
        qDebug() << "[Error] "  << "Failed to stop LMI ";
        return false;
    }
    return true;
}

void CloseLmi()
{
    // destroy handles
    GoDestroy(system1);
    GoDestroy(api);
}

#pragma endregion


Hd_3DCameraLMI_module::Hd_3DCameraLMI_module()
{
    m_mutex = new QMutex();
}

Hd_3DCameraLMI_module::~Hd_3DCameraLMI_module()
{
    ExceptionCB(-1, NULL);
}

bool Hd_3DCameraLMI_module::sendData(std::vector<cv::Mat>& vecPic, QByteArray &data )
{
    
    if (data == "connect")
    {
        data = mv_Status;
        return true;
    }
  

    m_mutex->lock();
    if (queuePic.empty())
    {
        //qDebug() << " [Error] " << " srcImage is null";
        cv::Mat blank_image1(100, 100, CV_8UC1, cv::Scalar(0, 0, 0));
        cv::Mat blank_image2(100, 100, CV_8UC1, cv::Scalar(0, 0, 0));
        vector<cv::Mat> dstvec;
        dstvec.push_back(blank_image1);
        dstvec.push_back(blank_image2);
        vecPic.assign(dstvec.begin(), dstvec.end());
        m_mutex->unlock();
        return false;
    } 
    vector<cv::Mat> dstvec = queuePic.dequeue();
    queuePic.clear();
    m_mutex->unlock();
    vecPic.assign(dstvec.begin(), dstvec.end());
    return true;
    
}

bool Hd_3DCameraLMI_module::reciveData(std::vector<cv::Mat> &, QByteArray &qbyte)
{
    string reciveid = qbyte.toStdString();

    id = atoi(reciveid.c_str());

    return true;
}

bool Hd_3DCameraLMI_module::initParas(QByteArray &)
{
    //return true;
    bool flag = InitLmi(id); //3D初始化
    m_status = flag ? OK : UnInit;
    if (!flag)
    {
        qDebug() << "[Error] "  << "Failed to Init LMI,id:"<< id;
        return false;
    }

    flag = StartLmi();
    m_status == flag ? Grab : OK;

    if (!flag)
    {
        qDebug() << "[Error] "  << " Failed to start LMI";
        return false;
    }

    qDebug() << "[INFO] " << " success to start LMI,id:" << id;
        
    return true;
}

bool Hd_3DCameraLMI_module::run()
{

    bool result = false;
    //qDebug() << "[info]" << __FUNCTION__ << " line:" <<  __LINE__ << " run start" << id;
    try
    {
        int timeOut = 0;
        queuePic.clear();
        while (timeOut < 50)
        {
            m_mutex->lock();
            if (queuePic.size() > 0)
            {
                result = true;
                m_mutex->unlock();
                return true;
            }
            m_mutex->unlock();
            timeOut += 10;
            QThread::msleep(10);
        }
    }
    catch (QString ev)
    {
        qDebug() << "[Error] " << " ev:" << ev ;
    }
    //qDebug() << "[info]"<< __FUNCTION   _ << " line:" <<  __LINE__ << " run end" ;
    return result;
}

Hd_3DCameraLMI_module *create()
{
    qDebug() << "[INFO] "  << "Hd_MvCamera_module create success!" ;
    return new Hd_3DCameraLMI_module;
}

void destory(Hd_3DCameraLMI_module *ptr)
{
    if (ptr)
    {
        delete  ptr;
        ptr = nullptr;
    }
    qDebug() << "[INFO] "  << "Hd_MvCamera_module  destory success!";
}