#include "Hd_3DCameraLMI_module.h"
#include <QDebug>
std::vector<cv::Mat> queuePic;
QMutex* m_mutex;
int countNm = 0;
//0106 断连提示
void _stdcall ExceptionCB(unsigned int nMesgType, void* pUser) {
    mv_Status = "disconnect";
    qDebug() << "[Error] " << " LMI camera disconnects!";
}
bool m_Flag = false;
#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;

    for (i = 0; i < GoDataSet_Count(dataset); ++i)
    {
        GoDataMsg dataObj = GoDataSet_At(dataset, i);

        switch (GoDataMsg_Type(dataObj))
        {
        case GO_DATA_MESSAGE_TYPE_UNIFORM_SURFACE:
        {
            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);

            if (Width > 100)//轮廓出图
            {
                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 = 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_Flag = true;
                countNm++;
                qDebug() << "cout:"<< countNm;
                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:
        {
            MyBmp temp;
            GoSurfaceIntensityMsg surfaceIntMsg = dataObj;
            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);
    }
    
    //luminanceMat2 = imread("c://temp.png");
    if (m_Flag == true)
    {
        m_Flag = false;
        if (!heightMat2.empty() && !luminanceMat2.empty())
        {
            picVec.push_back(luminanceMat2);
            picVec.push_back(heightMat2);
            //imwrite("D://CCD//3D//" + to_string(clock()) + ".tiff", heightMat2);
            //imwrite("D://CCD//3D//" + to_string(clock()) + ".bmp", luminanceMat2);
        }
        else
        {
            heightMat2 = imread("c://temp.png", 0);
            luminanceMat2 = imread("c://temp.png", 0);
            picVec.push_back(luminanceMat2);
            picVec.push_back(heightMat2);
            qDebug() << "[INFO] " "image pic:IMG is empty --CameraName:" << id;
        }
        m_mutex->lock();
        queuePic.clear();
        queuePic.swap(picVec);
        //if (picVec.size() > 0)
        //    queuePic.append(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)
{

    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::readData(std::vector<cv::Mat>& vecPic, QByteArray& data, nlohmann::json& json)
{

   
    int cnt = 0;
    while (queuePic.size() == 0)
    {
        //qDebug() << " [Error] " << " srcImage is null";
        cnt++;
        QThread::msleep(100);
        if (cnt > 30)
        {
            //m_mutex->unlock();
            return false;
        }
    } 
    m_mutex->lock();
        //vector<cv::Mat> dstvec = queuePic.dequeue();
        qDebug() << queuePic.size();
    vecPic.assign(queuePic.begin(), queuePic.end());
    queuePic.clear();
    m_mutex->unlock();
    return true;
    
}

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

    id = atoi(reciveid.c_str());

    return true;
}

bool Hd_3DCameraLMI_module::initParas(QByteArray &, nlohmann::json& json)
{
    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
    {
        //m_mutex->lock();
        int timeOut = 0;
        while (queuePic.size() == 0&&timeOut<100)
        {
            timeOut++;
            _sleep(100);
        }
        int sai = queuePic.size();
        if(timeOut<100)
            result = true;
        //if(queuePic.size() > 0)
        //    result = true;
        //m_mutex->unlock();
    }
    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!";
}