#include "Hd_CameraModule_3DKeyence.h"
#include <exception>
#include <qthread.h>
#include <QTextCodec>
#include <qqueue.h>
#include <qmutex.h>

int G_xImageSize = 0;			 // Number of X points.
int G_yImageSize = 0;			 // Number of Y lines.
//const int MAX_LJXA_XDATANUM = 3200;
//const unsigned int BUFFER_FULL_COUNT = 30000;
//
//int _imageAvailable[MAX_LJXA_DEVICENUM] = { 0 };
//unsigned short* _luminanceBuf[MAX_LJXA_DEVICENUM] = { 0 };
//unsigned short* _heightBuf[MAX_LJXA_DEVICENUM] = { 0 };
//int _lastImageSizeHeight[MAX_LJXA_DEVICENUM];
int imgTakeNum = 0, imgTarget = 1;
int imgIndex = 0;
int getImageTimeOut = 3000;
QString lastError;
QMutex* m_mutex = nullptr;
std::mutex bufferMutex;
QQueue< cv::Mat> heightMatS;//高度图
QQueue<cv::Mat> luminanceMatS;//灰度图
//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;
}

bool JumpFlag = false;
int yCounts;
void myCallbackFunc(LJX8IF_PROFILE_HEADER* pProfileHeaderArray, WORD* pHeightProfileArray, WORD* pLuminanceProfileArray, DWORD dwLuminanceEnable, DWORD dwProfileDataCount, DWORD dwCount, DWORD dwNotify, DWORD dwUser)
{
    try
    {
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "  --> Enter callback function";
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "dwProfileDataCount = " << dwProfileDataCount;
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "dwCount = " << dwCount;
        std::cout << "dwCount->" << dwCount <<" dwProfileDataCount"<< dwProfileDataCount << "dwNotify = " << dwNotify<< std::endl;
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "dwNotify = " << dwNotify;
        //if (dwCount < 1000)return;
  /*      if ((dwNotify != 0) || (dwNotify & 0x10000) != 0)
        {
            return;
        }*/
        std::lock_guard<std::mutex> lock(bufferMutex);
        if (dwCount == 0)   
        {
            return;
        }
        if (_imageAvailable[dwUser] == 1) 
        {
            return;
        }

        

        // _heightBuf 代表的含义:程序里的高度缓存区，pHeightProfileArray 回调函数里的数据 
        // 此处崩溃的可能性：xImageSize 配置文件中设置的值和3D相机调试软件设置的值不一致，会崩溃。
        if (JumpFlag)
        {
            qDebug() << __FUNCTION__ << " line:" << __LINE__ << "JumpFlag";
            return;
        }
        if (_heightBuf[dwUser] == NULL)
        {
            qDebug() << __FUNCTION__ << " line:" << __LINE__ << "_heightBuf[dwUser] == NULL";
            _heightBuf[dwUser] = (unsigned short*)malloc(G_yImageSize * MAX_LJXA_XDATANUM * 2);
            //return;
        }
        if (pHeightProfileArray == NULL)
        {
            qDebug() << __FUNCTION__ << " line:" << __LINE__ << "pHeightProfileArray== NULL";
            return;
        }

        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "_heightBuf[dwUser]: " << _heightBuf[dwUser];
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "pHeightProfileArray: " << pHeightProfileArray;

        //判断内存是否重叠
        if (_heightBuf[dwUser] <= pHeightProfileArray || (unsigned short*)_heightBuf[dwUser] >= (WORD*)pHeightProfileArray + dwProfileDataCount * dwCount * 2)
        {
            memcpy(&_heightBuf[dwUser][0], pHeightProfileArray, dwProfileDataCount * dwCount * 2);
        }
        else
        {
            qDebug() << __FUNCTION__ << " line:" << __LINE__ << "first memcpy _heightBuf error";
            if (_heightBuf[dwUser] != NULL)
            {
                free(_heightBuf[dwUser]);
                _heightBuf[dwUser] = NULL;
            }
            qDebug() << __FUNCTION__ << " line:" << __LINE__ << "  memcpy _heightBuf again ";
            _heightBuf[dwUser] = (unsigned short*)malloc(yCounts * MAX_LJXA_XDATANUM * 2);
            memcpy(&_heightBuf[dwUser][0], pHeightProfileArray, dwProfileDataCount * dwCount * 2);
            qDebug() << __FUNCTION__ << " line:" << __LINE__ << " Second memcpy _heightBuf success ";
            
        }

        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "_luminanceBuf[dwUser]: " << _luminanceBuf[dwUser];
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "pLuminanceProfileArray: " << pLuminanceProfileArray;
        if (dwLuminanceEnable == 1)
        {
            if (_luminanceBuf[dwUser] == NULL)
            {
                qDebug() << __FUNCTION__ << " line:" << __LINE__ << "_luminanceBuf[dwUser]== NULL";
                //return;
                _luminanceBuf[dwUser] = (unsigned short*)malloc(G_yImageSize * MAX_LJXA_XDATANUM * 2);
            }
            if (_luminanceBuf[dwUser] <= pLuminanceProfileArray || (unsigned short*)_luminanceBuf[dwUser] >= (WORD*)pLuminanceProfileArray + dwProfileDataCount * dwCount * 2)
                memcpy(&_luminanceBuf[dwUser][0], pLuminanceProfileArray, dwProfileDataCount * dwCount * 2);
            else
                qDebug() << __FUNCTION__ << " line:" << __LINE__ << "memcpy _luminanceBuf error";
        }
        _imageAvailable[dwUser] = 1;                 // 图像获取成功  dwUser  和 deviceId   一致
        _lastImageSizeHeight[dwUser] = dwCount;      // 扫描的行数，  dwCount 和 yImageSize 一致
    }
    catch (std::exception e)
    {
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "  --> execute callback function error";
    }
}

int Hd_CameraModule_3DKeyence::LJXA_ACQ_OpenDevice(int lDeviceId, LJX8IF_ETHERNET_CONFIG* EthernetConfig, int HighSpeedPortNo)
{
    int errCode = LJX8IF_EthernetOpen(lDeviceId, EthernetConfig);

    _ethernetConfig[lDeviceId] = *EthernetConfig;
    _highSpeedPortNo[lDeviceId] = HighSpeedPortNo;

    qDebug() << __FUNCTION__ << " line:" << __LINE__ << "  Open device ! errCode:" << errCode;
    return errCode;
}

void Hd_CameraModule_3DKeyence::LJXA_ACQ_CloseDevice(int lDeviceId)
{
    LJX8IF_FinalizeHighSpeedDataCommunication(lDeviceId);
    LJX8IF_CommunicationClose(lDeviceId);
    qDebug() << __FUNCTION__ << " line:" << __LINE__ << "  Close device!";
}


bool Hd_CameraModule_3DKeyence::decodeInitData(QByteArray byteArray,QVector<QByteArray> &vector_data)
{
    while (!byteArray.isEmpty())
    {
        int type = 0;
        QByteArray m_type = byteArray.mid(0,4);
        memcpy((void*)&type,(void*)m_type.data(), 4);
        byteArray.remove(0,4);

        int len = 0;
        QByteArray m_len = byteArray.mid(0,4);
        memcpy((void*)&len,(void*)m_len.data(), 4);
        byteArray.remove(0,4);

        QByteArray mm_data = byteArray.mid(0,len);
        vector_data.append(mm_data);
        byteArray.remove(0,len);
    }
    return true;
}

Hd_CameraModule_3DKeyence* create()
{
    qDebug() << __FUNCTION__  << "line:" << __LINE__  << "  Hd_CameraModule_3DKeyence create success!" ;
    return new Hd_CameraModule_3DKeyence;
}

void destory(Hd_CameraModule_3DKeyence * ptr)
{
    if (ptr)
    {
        delete  ptr;
        ptr = nullptr;
    }
    qDebug()  << __FUNCTION__  << " line:" << __LINE__ << "  Hd_CameraModule_3DKeyence destory success!";
}

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


Hd_CameraModule_3DKeyence::~Hd_CameraModule_3DKeyence()
{
    if (isopen)
    {
        isopen = false;

        // Stop HighSpeed
        errCode = LJX8IF_StopHighSpeedDataCommunication(deviceId);
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "  Stop HighSpeed errCode:" << errCode;

        LJXA_ACQ_CloseDevice(deviceId);
        qDebug()  << __FUNCTION__ << "line:" << __LINE__ << " CloseDevice success!";
        if (heightImage)
        {
            free(heightImage);

        }
        if (luminanceImage)
        {
            free(luminanceImage);
        }
    }

    // Free memory
    if (_heightBuf[deviceId] != NULL)
    {
        free(_heightBuf[deviceId]);
    }

    if (_luminanceBuf[deviceId] != NULL)
    {
        free(_luminanceBuf[deviceId]);
    }

    if (startReq_ptr)
    {
        delete startReq_ptr;
        startReq_ptr = nullptr;
    }

    if (profileInfo_ptr)
    {
        delete profileInfo_ptr;
        profileInfo_ptr = nullptr;
    }

    if (setParam_Ptr)
    {
        delete setParam_Ptr;
        setParam_Ptr = nullptr;
    }

    if (getParam_Ptr)
    {
        delete getParam_Ptr;
        getParam_Ptr = nullptr;
    }
    if (m_mutex)
    {
        delete m_mutex;
        m_mutex = nullptr;
    }
    qDebug()  << __FUNCTION__ << "line:" << __LINE__ << " delete success!";
}

bool Hd_CameraModule_3DKeyence::readData(std::vector<cv::Mat> &mats,QByteArray &data)
{
    Q_UNUSED(data);

    QMutexLocker locker(m_mutex);
    if (luminanceMatS.empty() || heightMatS.empty())
    {
        qDebug() << " [Error] " << " srcImage is null";
        return false;
    }
    for (int i = 0; i < imgTarget; i++) {
        if (heightMatS.size() >= 1 && luminanceMatS.size() >= 1) {
            mats.push_back(luminanceMatS.dequeue());
            mats.push_back(heightMatS.dequeue());
        }
        else {
            mats.push_back(cv::Mat(G_xImageSize, G_yImageSize, CV_8UC1, cv::Scalar(0, 0, 0)));
            mats.push_back(cv::Mat(G_xImageSize, G_yImageSize, CV_8UC1, cv::Scalar(0, 0, 0)));
        }
    }
    imgTakeNum = 0;
    qCritical() << " [info] " << __FUNCTION__ << " line:" << __LINE__ << " checkImg readData,deviceId" << deviceId << " imgIndex" << imgIndex;
    return true;
}

bool Hd_CameraModule_3DKeyence::writeData(std::vector<cv::Mat> &mat,QByteArray &data)
{
    if (data == "clearCahce") {
        QMutexLocker locker(m_mutex);
        heightMatS.clear();
        luminanceMatS.clear();
    }
    //2024.02.18更新，相机对应的读SN信号时，表示新产品进入，设置初始值
    //if (data == "rest_newProductIn")
    //{
    //    //根据实际项目，设置初始值：曝光、增益，ROI区域等
    //    /*{
    //        if (expourseTimeList.size() > 0)
    //            SetExposureTime(expourseTimeList[0]);
    //        if (gainList.size() > 0)
    //            SetGain(gainList[0]);
    //    }
    //    getImageNum = 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 " << camera_name.toStdString() << std::endl;
    //        qWarning() << __FUNCTION__ << "  line: " << __LINE__ << "rest_newProductIn , queuePic.size() > 0 ,signal maybe too fast " << camera_name;
    //    }
    //    if (ifRunning == true)
    //    {
    //        ifBreak = true;
    //        Sleep(10);
    //    }
    //    imgIndex = 0;
    //    //getImageNum = 0;
    //    return true;
    //}
    

    return true;
}

//传入initParas函数，格式为：相机key+参数名1#参数值1#参数值2+参数名2#参数值1...
bool Hd_CameraModule_3DKeyence::initParas(QByteArray& initData)
{
    QByteArrayList valueList = initData.split('#');
    if (valueList.size() < 6) {
        qDebug() << "[Error] " << " Hd_CameraModule_HIK initParas fail--params error!参数数量不满足";
        lastError = "参数数量不满足";
        return false;
    }
    EthernetConfig.wPortNo = 24691;

    camera_name = valueList[0];
    auto valueL1 = valueList[0].split('.');
    if (valueL1.size() == 4)
    {
        EthernetConfig.abyIpAddress[0] = valueL1.at(0).toInt();
        EthernetConfig.abyIpAddress[1] = valueL1.at(1).toInt();
        EthernetConfig.abyIpAddress[2] = valueL1.at(2).toInt();
        EthernetConfig.abyIpAddress[3] = valueL1.at(3).toInt();
    }

    deviceId = valueList[1].toInt();

    xImageSize = valueList[2].toInt();
    G_xImageSize = valueList[2].toInt();

    yImageSize = valueList[3].toInt();
    G_yImageSize = valueList[3].toInt();

    imgTarget = valueList[4].toInt();

	getImageTimeOut = valueList[5].toInt();
    yCounts = yImageSize;
    qDebug() << __FUNCTION__ << " line:" << __LINE__ << " camera_name:" << camera_name << " paras init is success!";

    // 初始化申请空间 20230214
    startReq_ptr    = new LJX8IF_HIGH_SPEED_PRE_START_REQ;
    profileInfo_ptr = new LJX8IF_PROFILE_INFO;
    startReq_ptr->bySendPosition = 2;

    setParam_Ptr = new LJXA_ACQ_SETPARAM;
    getParam_Ptr = new LJXA_ACQ_GETPARAM;

    setParam_Ptr->y_linenum = yImageSize;
    setParam_Ptr->y_pitch_um = y_pitch_um;
    setParam_Ptr->timeout_ms = timeout_ms;
    setParam_Ptr->use_external_batchStart = use_external_batchStart;

    // Allocate user memory
    heightImage = (unsigned short*)malloc(sizeof(unsigned short) * xImageSize * yImageSize);
    luminanceImage = (unsigned short*)malloc(sizeof(unsigned short) * xImageSize * yImageSize);

    errCode = LJXA_ACQ_OpenDevice(deviceId, &EthernetConfig, HighSpeedPortNo);
    if ( errCode != LJX8IF_RC_OK )
    {
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << " Failed to open device ";
        //Free user memory
        if ( heightImage   )
        {
            free(heightImage);
        }

        if ( luminanceImage )
        {
            free(luminanceImage);
        }
        return false;
    }

    isopen = true;

    // Allocate memory
    _heightBuf[deviceId] = (unsigned short*)malloc(yImageSize * MAX_LJXA_XDATANUM * 2);
    if (_heightBuf[deviceId] == NULL)
    {
        return LJX8IF_RC_ERR_NOMEMORY;
    }

    _luminanceBuf[deviceId] = (unsigned short*)malloc(yImageSize * MAX_LJXA_XDATANUM * 2);
    if (_luminanceBuf[deviceId] == NULL)
    {
        return LJX8IF_RC_ERR_NOMEMORY;
    }

    // Initialize
    if (!InitHighSpeed())
    {
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "  InitHighSpeed error！ " ;
    }
    return true;
}
bool Hd_CameraModule_3DKeyence::InitHighSpeed()
{
    try
    {
        if ( startReq_ptr == nullptr )
        {
            startReq_ptr = new LJX8IF_HIGH_SPEED_PRE_START_REQ;
            startReq_ptr->bySendPosition = 2;
            qWarning() << __FUNCTION__ << "  line: " << __LINE__ << " startReq_ptr is null! ";
        }
        if ( profileInfo_ptr == nullptr )
        {
            profileInfo_ptr = new LJX8IF_PROFILE_INFO;
            qWarning() << __FUNCTION__ << "  line: " << __LINE__ << " profileInfo_ptr is null! ";
        }
        errCode = LJX8IF_InitializeHighSpeedDataCommunicationSimpleArray(deviceId, &_ethernetConfig[deviceId], _highSpeedPortNo[deviceId], &myCallbackFunc, yImageSize, deviceId); // 初始化高速通讯
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "   errCode:" << errCode;
        errCode = LJX8IF_PreStartHighSpeedDataCommunication(deviceId, startReq_ptr, profileInfo_ptr);              // 预开始启动高速通信，作用 做预连接
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "   errCode:" << errCode;

        // zUnit
        errCode = LJX8IF_GetZUnitSimpleArray(deviceId, &zUnit);
        if (errCode != 0 || zUnit == 0)
        {
            qCritical() << __FUNCTION__ << "  line:" << __LINE__ << " Failed to acquire zunit! ";
            return false;
        }
        errCode = LJX8IF_StartHighSpeedDataCommunication(deviceId);           // 正式开始高速通讯
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << " errCode: " << errCode;

        //Start HighSpeed
        _imageAvailable[deviceId] = 0;
        _lastImageSizeHeight[deviceId] = 0;
    }
    catch (const QString ev)
    {
        qDebug() << __FUNCTION__ << " line:" << __LINE__ << "  ev: " <<  ev;
        return false;
    }
    return true;
   
}

bool Hd_CameraModule_3DKeyence::run()
{
    
    ifRunning = true;
    ifBreak = false;
    if ( !isopen )
    {
        qCritical()  << __FUNCTION__ << " line:" << __LINE__ << " device is not opened! ";
        return false;
    }

    try
    {
        while (imgTakeNum != imgTarget)
        {
            int sleepMs = 10;
            int cnt = 0;
            int timeoutCnt = getImageTimeOut / sleepMs;
            while (_imageAvailable[deviceId] != 1 )
            {
                QThread::msleep(10);
                if (cnt++ >= timeoutCnt)
                {
                    heightMatS.push_back(cv::Mat(G_yImageSize, G_xImageSize, CV_8UC1, cv::Scalar(0, 0, 0)));
                    luminanceMatS.push_back(cv::Mat(G_yImageSize, G_xImageSize, CV_8UC1, cv::Scalar(0, 0, 0)));
                    imgTakeNum++;
                    qDebug() << "imgGetERROR insert blank img cameraID:" << camera_name;
                    continue;
                }
            }


            std::lock_guard<std::mutex> lock(bufferMutex);
            _getParam[deviceId].luminance_enabled = profileInfo_ptr->byLuminanceOutput;
            _getParam[deviceId].x_pointnum = profileInfo_ptr->wProfileDataCount;
            _getParam[deviceId].y_linenum_acquired = _lastImageSizeHeight[deviceId];
            _getParam[deviceId].x_pitch_um = profileInfo_ptr->lXPitch / 100.0f;
            _getParam[deviceId].y_pitch_um = y_pitch_um;
            _getParam[deviceId].z_pitch_um = zUnit / 100.0f;

            *getParam_Ptr = _getParam[deviceId];
            qDebug() << __FUNCTION__ << " line:" << __LINE__ << " getPara Finished: ";
            int xDataNum = _getParam[deviceId].x_pointnum;

            unsigned short* dwHeightBuf = (unsigned short*)&_heightBuf[deviceId][0];
            qDebug() << __FUNCTION__ << " line:" << __LINE__ << " dwHeightBuf Init Finished: ";
            memcpy(heightImage, dwHeightBuf, xDataNum * yImageSize * 2);

            qDebug() << __FUNCTION__ << " line:" << __LINE__ << " Height Img memcpy Finished: ";
            std::vector<unsigned short>poits(xDataNum * yImageSize * 2, 0);
            if (_getParam[deviceId].luminance_enabled > 0)
            {
                unsigned short* dwLuminanceBuf = (unsigned short*)&_luminanceBuf[deviceId][0];
                memcpy(luminanceImage, dwLuminanceBuf, xDataNum * yImageSize * 2);
            }
            // Free memory
            if (_heightBuf[deviceId] != NULL)
            {
                free(_heightBuf[deviceId]);
                _heightBuf[deviceId] = NULL;
            }

            if (_luminanceBuf[deviceId] != NULL)
            {
                free(_luminanceBuf[deviceId]);
                _luminanceBuf[deviceId] = NULL;
            }
            qDebug() << __FUNCTION__ << " line:" << __LINE__ << " Free memory Finished: ";

            m_mutex->lock();
            cv::Mat luminanceMat;
            if(_lastImageSizeHeight[deviceId] != 0)
                luminanceMat = cv::Mat(_lastImageSizeHeight[deviceId], xImageSize, CV_16UC1, luminanceImage);
            else
                luminanceMat = cv::Mat(yImageSize, xImageSize, CV_16UC1, luminanceImage);
            cv::normalize(luminanceMat, luminanceMat, 0, 255, cv::NORM_MINMAX);
            cv::convertScaleAbs(luminanceMat, luminanceMat);

            luminanceMatS.enqueue(luminanceMat);

            cv::Mat heightImg;
            if (_lastImageSizeHeight[deviceId] != 0)
                cv::Mat(_lastImageSizeHeight[deviceId], xImageSize, CV_16UC1, heightImage).copyTo(heightImg);
            else
                cv::Mat(yImageSize, xImageSize, CV_16UC1, heightImage).copyTo(heightImg);
            heightMatS.enqueue(heightImg);
            m_mutex->unlock();
            qDebug() << __FUNCTION__ << " line:" << __LINE__ << " success to acquire 3d image! camera_name: " << camera_name;
            //Start HighSpeed
            _imageAvailable[deviceId] = 0;
            _lastImageSizeHeight[deviceId] = 0;
            imgTakeNum++;
        }
        qDebug() << camera_name <<"Finish";
        return true;
    }
    catch (QString ev)
    {
        qCritical() << __FUNCTION__ << "  line: " << __LINE__ << "  ev: " << ev;
        return false;
    }
}