#include "Hd_CameraModule_3DLMI.h"
#include <QDebug>
#include <QQueue>
#include <QTextCodec>

QByteArray mv_Status = "connected";//0106 新增相机断联报警
long long imgIndex = 0;
std::vector<GoDataMsg>DataSetCollection;
std::mutex DataSetMutex;

k32u id = 117599; //3D 序列号
kAssembly api = kNULL;
GoSystem system1 = kNULL;
GoSensor sensor = kNULL;
GoDataSet dataset = kNULL;
DataContext* contextPointer = new DataContext();
LmiStatus m_status = UnInit;
bool m_lmiheightFlag = false;
bool m_lmiluminanceFlag = false;
bool moduleStatus = true;

//data == "rest_newProductIn"时要结束run；机台报警，取图超时，复位后重新拍的情况。
bool ifBreak = false;
//ifRunning减少ifBreak的判断；"rest_newProductIn"时，Logic_camera_thread框架会结束未处理完的相相机取图
bool ifRunning = false;
QString byteArrayToUnicode(const QByteArray array) {

    // state用于保存转换状态，它的成员invalidChars，可用来判断是否转换成功
    // 如果转换成功，则值为0，如果值大于0，则说明转换失败
    QTextCodec::ConverterState state;
    // 先尝试使用utf-8的方式把QByteArray转换成QString
    QString text = QTextCodec::codecForName("UTF-8")->toUnicode(array.constData(), array.size(), &state);
    // 如果转换时无效字符数量大于0，说明编码格式不对
    if (state.invalidChars > 0)
    {
        // 再尝试使用GBK的方式进行转换，一般就能转换正确(当然也可能是其它格式，但比较少见了)
        text = QTextCodec::codecForName("GBK")->toUnicode(array);
    }
    return text;
}
//QQueue<std::vector<cv::Mat>> queuePic;
QMutex* m_mutex;
//static Mat heightMat1, heightMat2;//高度图
//static Mat luminanceMat1, luminanceMat2;//灰度图

QQueue< cv::Mat> heightMatS;//高度图
QQueue<cv::Mat> luminanceMatS;//灰度图

//是否为标准通信流程：
//标准通信流程，run函数设置曝光/增益；非标准通信流程（简单通信流程），回调函数设置曝光/增益
bool ifStandard = false;
//相机取图超时时间
int getImageTimeOut = 1000;
//总取图数
int getImageNum = 0;
//曝光值列表
QQueue<float> expourseTimeList;
//增益列表
QQueue<float> gainList;
static bool m_flag = false;

//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;
    //DataContext* context = (DataContext*)ctx;
    GoStamp stamp;
    std::vector<cv::Mat> picVec;

    m_mutex->lock();
    m_flag = true;
    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];
                        //unsigned short val = ZOffset + ZResolution * data[colIdx];
                        temp.Tiff[rowIdx * Width + colIdx] = val;
                    }
                }

                cv::Mat heightMat1 = cv::Mat(temp.h, temp.w, CV_16U, temp.Tiff);
                //heightMat1.copyTo(heightMat2);
                heightMatS.push_back(heightMat1.clone());
                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];
                }
            }

            cv::Mat luminanceMat1 = cv::Mat(temp.h, temp.w, CV_8UC1, temp.Bmp);
            //luminanceMat1.copyTo(luminanceMat2);
            luminanceMatS.push_back(luminanceMat1.clone());
            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);
    }

    m_mutex->unlock();
    GoDestroy(dataset);
    double time_End = (double)clock();
    qDebug() << "[INFO] " << __FUNCTION__ << " line:" << __LINE__  << " checkImg getImg Time:" << (time_End - time_Start) << "ms" 
        << " --CameraName:" << id << " imgIndex:" << imgIndex++ << "--FrameID : " << stamp.frameIndex;

    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_CameraModule_3DLMI::Hd_CameraModule_3DLMI()
{
    m_mutex = new QMutex();
}

Hd_CameraModule_3DLMI::~Hd_CameraModule_3DLMI()
{
    ExceptionCB(-1, NULL);
}
//从类中读数据到实例对象
bool Hd_CameraModule_3DLMI::readData(std::vector<cv::Mat>& vecPic, QByteArray &data )
{
    
    /*if (data == "connect")
    {
        data = mv_Status;
        return true;
    }*/
  

    m_mutex->lock();
    if (luminanceMatS.empty() || heightMatS.empty())
    {
        qDebug() << " [Error] " << " srcImage is null";
        m_mutex->unlock();
        return false;
    } 
    cv::Mat mheightMat2 = cv::Mat();
    heightMatS.dequeue().copyTo(mheightMat2);
    cv::Mat mluminanceMat2 = cv::Mat();
    luminanceMatS.dequeue().copyTo(mluminanceMat2);
    vecPic.push_back(mheightMat2.clone());
    vecPic.push_back(mluminanceMat2.clone());

    m_mutex->unlock(); 
    qCritical() << " [info] " << __FUNCTION__ << " line:" << __LINE__ << "  checkImg readData id:" << id<<" imgIndex" << imgIndex;
    /*vector<cv::Mat> dstvec;
    dstvec.push_back(heightMat2.clone());
    dstvec.push_back(luminanceMat2.clone());
    m_mutex->unlock();
    vecPic.assign(dstvec.begin(), dstvec.end());*/
    return true;
    
}
//实例对象把数据写入到类
bool Hd_CameraModule_3DLMI::writeData(std::vector<cv::Mat> &, QByteArray &qbyte)
{
    /*string reciveid = qbyte.toStdString();

    id = atoi(reciveid.c_str());*/
    //2024.02.18更新，相机对应的读SN信号时，表示新产品进入，设置初始值
    if (qbyte == "rest_newProductIn")
    {
        //根据实际项目，设置初始值：曝光、增益，ROI区域等
        /*{
            if (expourseTimeList.size() > 0)
                SetExposureTime(expourseTimeList[0]);
            if (gainList.size() > 0)
                SetGain(gainList[0]);
        }*/
        if (luminanceMatS.size() > 0 || heightMatS.size() > 0)
        {
            m_mutex->lock();
            luminanceMatS.clear();
            heightMatS.clear();
            m_mutex->unlock();
            std::cout << "rest_newProductIn , queuePic.size() > 0 ,signal maybe too fast " << id << std::endl;
            qWarning() << __FUNCTION__ << "  line: " << __LINE__ << "rest_newProductIn , queuePic.size() > 0 ,signal maybe too fast " << id;
        }
        if (ifRunning == true)
        {
            ifBreak = true;
            Sleep(10);
        }
        imgIndex = 0;
        getImageNum = 0;
        return true;
    }
    //else
    //{
    //    m_flag = false;
    //    expourseTimeList.clear();
    //    gainList.clear();
    //}
    return true;
}

//传入initParas函数，格式为：相机key+参数名1#参数值1#参数值2+参数名2#参数值1...
bool Hd_CameraModule_3DLMI::initParas(QByteArray& initData)
{
    QByteArrayList valueList = initData.split('+');

    if (valueList.size() > 1)
    for (int i = 1; i < valueList.size(); i++)
    {
        QString valuei = byteArrayToUnicode(valueList[i]);
        if (valuei.contains("是否为标准通信流程"))
        {
            if (valuei.contains("true"))
                ifStandard = true;
        }
        if (valuei.contains("取图超时"))
        {
            QStringList gainL = valuei.split('#');
            if (gainL.size() > 1)
                getImageTimeOut = gainL[1].toInt();
            if (getImageTimeOut < 50)
                getImageTimeOut = 1000;
        }
    }

    string reciveid = valueList[0].toStdString();

    id = atoi(reciveid.c_str()); //id 相机序列号

    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_CameraModule_3DLMI::run()
{
    ifRunning = true;
    ifBreak = false;
    bool result = false;
    //qDebug() << "[info]" << __FUNCTION__ << " line:" <<  __LINE__ << " run start" << id;
    try
    {
        m_flag = false;
        int mytime = 0;
        while (true)
        {
            if (moduleStatus == false || mytime++ > getImageTimeOut || ifBreak == true)
            {
                result = false;
                break;
            }
            m_mutex->lock();
            if (luminanceMatS.size() > 0 && heightMatS.size() > 0)
            {
                result = true;
                m_mutex->unlock();
                break;
            }
            m_mutex->unlock();
            Sleep(1);
        }
    }
    catch (QString ev)
    {
        qDebug() << "[Error] " << " ev:" << ev ;
    }
    //qDebug() << "[info]"<< __FUNCTION__ << " line:" <<  __LINE__ << " run end" ;
    return result;
}

//相机连接状态：正常连接或断开
bool Hd_CameraModule_3DLMI::checkStatus()
{
    return moduleStatus;
}

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

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