#include <triggerManager.h>
#include <backEndManager.h>
#include <threadPoolManager.h>
#include <backStruct.h>
#include <cameraManager.h>
#include <jsonParsing.h>
#define debugTesting 0
static correctionStruct* correctionData;
void triggerManager::initialization(backEndManager* input)
{
    _backEndManager = input;
    dManager = _backEndManager->_detDataManager;
    _cameraManager = _backEndManager->_cameraManager;
    _threadManager = _backEndManager->_threadManager;
    _jsonConfiguration = _backEndManager->_jsonConfiguration;
    startPos = _jsonConfiguration->tcp.startPos;
    triggerFileName = _jsonConfiguration->tcp.moduleName;
    IpAndPort = _jsonConfiguration->tcp.finstcp_init;
    //triggerInitStr = QByteArray("sleeptime_%1_readnum_%2_writetype_ushort");
    QLibrary* m_lib = new QLibrary(triggerFileName);
    bool flag = m_lib->load();
    if (!m_lib->isLoaded())
    {
        //emit _backEndManager->sendErrorToMainwindow(dllNotFound);
        emit _backEndManager->sendInitString("找不到" + triggerFileName);
        return;
    }

    if (flag)
    {
        CreateFunction createFun = (CreateFunction)m_lib->resolve("create");
        ReleaseFunction releaseFun = (ReleaseFunction)m_lib->resolve("destory");
        bool initFailure = false;
        if (createFun && releaseFun)
        {
            trigger = createFun();
            destory_finstcp = releaseFun;
            QByteArray initByte = QByteArray::fromStdString("sleeptime_20_readnum_300_writetype_ushort");
            trigger->reciveData(vec_mat, QByteArray(IpAndPort.toUtf8()));
            emit _backEndManager->sendInitString(triggerFileName + "连接到" + IpAndPort);
            initFailure = trigger->initParas(initByte);
        }

        if (initFailure == false)
        {
            emit _backEndManager->sendErrorToMainwindow(triggerInitFailure);
            delete m_lib;
            m_lib = nullptr;
            emit _backEndManager->sendInitString(triggerFileName + "初始化失败");
            return;
        }
        else
        {
            initFlag = true;
            emit _backEndManager->sendInitString(triggerFileName + "成功");
        }
    }
}
void triggerManager::reciveCommond()
{
    //auto start = std::chrono::high_resolution_clock::now();
    //QByteArray byte = m_readAllCmd.toUtf8();      // 设置 从起始位开始读取的指令
    QByteArray byte = QByteArray::fromStdString(_jsonConfiguration->tcp.finstcp_readAllCmd.toStdString());
    if (!sendFinsMsg(byte))
        qCritical() << "[TCP] " << " finstcp_ptr reciveData error!";
    QByteArray readByte;
    if (!reciveFinsMsg(readByte))
        qCritical() << "[TCP] " << " finstcp_ptr sendData error!";
    tLocker.reFreshLocker(readByte);//经过检查后再触发
    //auto end = std::chrono::high_resolution_clock::now();
    //auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
    //std::cout << "cost: " << duration.count() << " mm" << std::endl;
    decodeCameraCMD(readByte);
    //workThread->snDatas.checkSnFromWaittingVec(readByte);
}
void triggerManager::decodeCameraCMD(QByteArray byte)
{
#if debugTesting
    //{
        correctionData = new correctionStruct();

        //correctionData->in_X_X0Y = { 536.37,657.3,351.32 };
        //correctionData->in_Y_X0Y = { -1383.29,-1234.18,-1242.13 };
        //correctionData->in_Z_X0Y = { -131.51,-131.51,-131.51 };
        //correctionData->in_Z_X0Y_Value = { -78.6552,-78.6552,-78.6552 };

        //correctionData->in_X_X0Z = { 615.24,191.31,191.5 };
        //correctionData->in_Y_X0Z = { -372.66,-372.66,-372.66 };
        //correctionData->in_Z_X0Z = { -925.76,-928.33,-1001.64 };
        //correctionData->in_Y_X0Z_Value = { -79.4859,-79.4859,-79.4859 };

        //correctionData->in_X_Y0Z = { -213.89,-213.89,-213.89 };
        //correctionData->in_Y_Y0Z = { -1006.12,-1083.8,-1005.4 };
        //correctionData->in_Z_Y0Z = { -602.89,-620.53,-740.52 };
        //correctionData->in_X_Y0Z_Value = { 80.9952,80.9952,80.9952 };

        correctionData->in_X_X0Y = { 536.37,657.3,351.32  };
        correctionData->in_Y_X0Y = {-1383.29,-1234.18,-1242.13 };
        correctionData->in_Z_X0Y = { -131.51,-130.61,-130.66 };
        correctionData->in_Z_X0Y_Value = { -78.6552,-82.3836,-83.4522 };

        correctionData->in_X_X0Z = { 615.24,191.31,191.5};
        correctionData->in_Y_X0Z = { -372.66,-370.9,-370.18};
        correctionData->in_Z_X0Z = { -925.76,-928.33,-1001.64 };
        correctionData->in_Y_X0Z_Value = { -79.4859,-79.4859,-79.8369};

        correctionData->in_X_Y0Z = { -213.89,-209.83,-212.4 };
        correctionData->in_Y_Y0Z = { -1006.12,-1083.8,-1005.4 };
        correctionData->in_Z_Y0Z = { -602.89,-620.53,-740.52 };
        correctionData->in_X_Y0Z_Value = { 80.9952,84.006,81.1005 };

    //    correctionData = new correctionStruct();
        //correctionData->in_X_X0Y = { -258.71,-447.8,-447.2 };
        //correctionData->in_Y_X0Y = { -1099.67,-1133.42,-938.83 };
        //correctionData->in_Z_X0Y = { 711.77,711.62,711.730000 };
        //correctionData->in_Z_X0Y_Value = { 63.9795,64.5333,65.859300 };

        //correctionData->in_X_X0Z = { -402.3,-455.91,-236.75 };
        //correctionData->in_Y_X0Z = { -539.46,-536.21,-536.2 };
        //correctionData->in_Z_X0Z = { -418.88,425.12,425.13 };
        //correctionData->in_Y_X0Z_Value = { 118.15,-228.19,-256.5 };

        //correctionData->in_X_Y0Z = { 395.250000,395.23,328.34 };
        //correctionData->in_Y_Y0Z = { -1147.29,-1147.28,-940.82 };
        //correctionData->in_Z_Y0Z = { 118.15,-228.19,-256.5 };
        //correctionData->in_X_Y0Z_Value = { -36.652200,-36.7302,41.0436 };
        std::shared_ptr <detData> data = dManager->insertDetData("181964", "TEST", 0, 0);
        data->imgAmountMin = 0;
        data->correctionData = correctionData;
        _threadManager->addTask(data);//添加任务
        correctionData = nullptr;

    //}
    //if (checkTrigger(1, 2, byte) == true)
    {
        //std::shared_ptr <detData> data = dManager->insertDetData("Camera1" , "TEST", 0 , 0);
        //data->correctionData = correctionData;
        //_threadManager->addTask(data);//添加任务
        //auto camera = _cameraManager->getCamera("123");
        //if(camera != nullptr)
        //    camera->captureImage(data);
    }
    //if (checkTrigger(2, 2, byte) == true)
    {
        //std::shared_ptr <detData> data = dManager->insertDetData("Camera1", "TEST", 1, 1);
        //data->correctionData = correctionData;
        //data->oriImgs.push_back(cv::imread("E://Cisco2-2024//TestImg//HFFGG6534_1_1_Battlestar20240326_180201.bmp",-1));
        //_threadManager->addTask(data);//添加任务
        //auto camera = _cameraManager->getCamera("123");
        //if (camera != nullptr)
        //    camera->captureImage(data);
    }
    //auto camera = _cameraManager->getCamera("181964");
    //if (camera != nullptr)
    //    camera->trigger();
    //double rangeData = _cameraManager->cameras.find("181964")->second->getCameraValue();
    QThread::msleep(10000000);
    return;
#endif
#if debugTesting == 0
    if (byte.isEmpty())
        return;
    double cameraData = 0;
    //算法测试
#if 0
    if (checkTrigger(1, 1, byte) == true)
    {


        correctionData = new correctionStruct();
        correctionData->in_X_X0Y = { -258.71,-447.8,-447.2 };
        correctionData->in_Y_X0Y = { -1099.67,-1133.42,-938.83 };
        correctionData->in_Z_X0Y = { 711.77,711.62,711.730000 };
        correctionData->in_Z_X0Y_Value = { 63.9795,64.5333,65.859300 };

        correctionData->in_X_X0Z = { -402.3,-455.91,-236.75 };
        correctionData->in_Y_X0Z = { -539.46,-536.21,-536.2 };
        correctionData->in_Z_X0Z = { -418.88,425.12,425.13 };
        correctionData->in_Y_X0Z_Value = { 118.15,-228.19,-256.5 };

        correctionData->in_X_Y0Z = { 395.250000,395.23,328.34 };
        correctionData->in_Y_Y0Z = { -1147.29,-1147.28,-940.82 };
        correctionData->in_Z_Y0Z = { 118.15,-228.19,-256.5 };
        correctionData->in_X_Y0Z_Value = { -36.652200,-36.7302,41.0436 };
        std::shared_ptr <detData> data = dManager->insertDetData("Camera1", "correctionDataThread", "TEST", 0);
        data->correctionData = correctionData;
        _threadManager->addTask(data);//添加任务
        correctionData = nullptr;

    }
#endif 
    //相机测试
#if 0
    if (checkTrigger(1, 2, byte) == true)
    {
        std::shared_ptr <detData> data = dManager->insertDetData("Camera1", "TEST",0, 0);
        data->correctionData = correctionData;
        _threadManager->addTask(data);//添加任务
        auto camera = _cameraManager->getCamera("123");
       if (camera != nullptr)
           camera->captureImage(data);
    }
    if (checkTrigger(2, 2, byte) == true)
    {
        std::shared_ptr <detData> data = dManager->insertDetData("Camera1", "TEST", 0, 0);
        data->correctionData = correctionData;
        _threadManager->addTask(data);//添加任务
        auto camera = _cameraManager->getCamera("456");
       if (camera != nullptr)
           camera->captureImage(data);
    }
#endif
    //总流程开始，机器人发送1至寄存器259

    //上位机确认OK之后，清零寄存器259，并发送1至寄存器260
    //如产品型号为Product_1且为正面，则上位机发送1至寄存器256，发送0至寄存器257
    //之后机器人确认之后需发送测量点位总数具体值至寄存器258
    if (checkTrigger(259, 1, byte) == true)
    {
        sendFinsMsg("write_short_259_0");//触发信号清零
        sendFinsMsg("write_short_260_1");//开始检测信号
    }

    //机器人激光当前点位	264	1...9
    //机器人到达激光点位1时，寄存器261至1，264至1，并将机器人在激光点位1坐标XYZ按照定义规则，保存进寄存器265~270
    std::string aaa = byte.toStdString();
    //上位机确认机器人到达点位1(264)时，确认OK后，寄存器262至1，261至0，取点完成后，发送寄存器263至1
    if (checkTrigger(261, 1, byte) == true)//读取请求地址
    {
        int armPos = getValue(264, byte);

        int xValue1 = getValue(265, byte);
        int xValue2 = getValue(266, byte);
        int xValue3 = getValue(267, byte);

        int yValue1 = getValue(268, byte);
        int yValue2 = getValue(269, byte);
        int yValue3 = getValue(270, byte);

        int zValue1 = getValue(271, byte);
        int zValue2 = getValue(272, byte);
        int zValue3 = getValue(273, byte);

        double xValue = (xValue1 == 1 ? 1 : -1) * ((double)xValue2 + ((double)xValue3) / (std::pow(10, std::to_string(xValue3).length())));
        double yValue = (yValue1 == 1 ? 1 : -1) * ((double)yValue2 + ((double)yValue3) / (std::pow(10, std::to_string(yValue3).length())));
        double zValue = (zValue1 == 1 ? 1 : -1) * ((double)zValue2 + ((double)zValue3) / (std::pow(10, std::to_string(zValue3).length())));

        auto camera = _cameraManager->getCamera("181964");
        if (camera != nullptr)
            camera->trigger();
        double rangeData = _cameraManager->cameras.find("181964")->second->getCameraValue();

        qDebug() << "jiaozhun xValue:" << xValue << " yValue:" << yValue << " zValue:" << zValue <<" rangeData:"<< rangeData;
        std::cout << "jiaozhun xValue:" << xValue << " yValue:" << yValue << " zValue:" << zValue << " rangeData:" << rangeData<<std::endl;
        if (rangeData == 0)
        {
            std::cout << "激光取图错误" << std::endl;
            return;
        }

        if (armPos == 1)//第一次
            correctionData = new correctionStruct();
        if (correctionData == nullptr)
            return;

        if (armPos <= 3)
        {
            correctionData->in_X_X0Y.push_back(xValue);
            correctionData->in_Y_X0Y.push_back(yValue);
            correctionData->in_Z_X0Y.push_back(zValue);
            correctionData->in_Z_X0Y_Value.push_back(rangeData);
        }
        else if (armPos <= 6)
        {
            correctionData->in_X_X0Z.push_back(xValue);
            correctionData->in_Y_X0Z.push_back(yValue);
            correctionData->in_Z_X0Z.push_back(zValue);
            correctionData->in_Y_X0Z_Value.push_back(rangeData);
        }
        else if (armPos <= 9)
        {
            correctionData->in_X_Y0Z.push_back(xValue);
            correctionData->in_Y_Y0Z.push_back(yValue);
            correctionData->in_Z_Y0Z.push_back(zValue);
            correctionData->in_X_Y0Z_Value.push_back(rangeData);
        }
        
        sendFinsMsg("write_short_261_0");//清零
        sendFinsMsg("write_short_262_1");
        sendFinsMsg("write_short_263_1");

        //上位机校正完成，将寄存器274置为1，并将结果按照定义XYZ规则放入310~336中
        if (armPos == 9)
        {
            std::shared_ptr <detData> data = dManager->insertDetData("181964",  "TEST", 0 , 0);
            data->imgAmountMin = 0;
            data->correctionData = correctionData;
            _threadManager->addTask(data);//添加任务
            correctionData = nullptr;

        }
    }

    //机器人到达激光点位9时，寄存器261至1，264至9，并将机器人在激光点位9坐标XYZ按照定义规则，保存进寄存器265~270

    if (checkTrigger(300, 1, byte) == true)
    {
        QThread::msleep(1000);//延时一秒等待机械臂停稳
        sendFinsMsg("write_short_300_0");//触发信号清零
        sendFinsMsg("write_short_301_1");//开始检测信号
        int Pos = getValue(303, byte);
        std::cout << "arm @Pos:" << Pos;

        std::shared_ptr <detData> data = dManager->insertDetData("Camera1", "TEST", 1, 1);
        data->oriShowIndex = 0;
        data->resShowIndex = 0;
        data->armPos = Pos;
        auto camera = _cameraManager->getCamera("Camera1");//查找相机
        if (camera != nullptr)
            camera->captureImage(data);//拍照
        data->imgSaveFileParameter.push_back("Pos" + std::to_string(Pos));//设置部分文件名
        data->correctionData = correctionData;
        _threadManager->addTask(data);//添加任务

        QThread::msleep(1000);//延时一秒完成拍照，后续改成拍完直接发送
        sendFinsMsg("write_short_302_1");//开始检测信号
    }


#endif
}
int triggerManager::getValue(int dmPos, QByteArray byte)
{
    int triggerPos = (dmPos - _backEndManager->_jsonConfiguration->tcp.startPos) * 2;
    if (byte.size()  < triggerPos)
        return false;
    int val1 = byte.at(triggerPos) * 256;
    int val2 = static_cast<unsigned char>(byte.at(triggerPos + 1));
    return val1 + val2;
}