#include "device_tof3d.h"
Error_manager DeviceTof3D::Init(const google::protobuf::RepeatedPtrField<tof3dVzenseEtc> &tof_devices) {
    LOG(INFO) << "---------- Init DeviceTof3D ----------";
    if (tof_devices.size() < DeviceAzimuth::DEVICE_AZIMUTH_MAX) {
        return {TOF3D_VZENSE_DEVICE_INIT_FAILED, MAJOR_ERROR, "device numbers is not enough: %d < %d",
                tof_devices.size(), DeviceAzimuth::DEVICE_AZIMUTH_MAX};
    }

    // 使能tof3d相机SDK
    auto t_vz_status = VZ_Initialize();
    if (t_vz_status != VzReturnStatus::VzRetOK && t_vz_status != VzReturnStatus::VzRetReInitialized) {
        return {TOF3D_VZENSE_DEVICE_INIT_FAILED, MAJOR_ERROR, "VzInitialize failed status: %d", t_vz_status};
    }

    // 注册相机列表
    m_devices_list.resize(DeviceAzimuth::DEVICE_AZIMUTH_MAX);
    for (auto &device: tof_devices) {
        // 校验方位
        if (device.azimuth() >= DeviceAzimuth::DEVICE_AZIMUTH_MAX) {
            return {ERROR, NORMAL, "azimuth should smaller than %d.", DeviceAzimuth::DEVICE_AZIMUTH_MAX};
        }
        // 校验方位配置是否出现重复
        if (m_devices_list[device.azimuth()].etc == nullptr) {
            m_devices_list[device.azimuth()].etc = &device;
        } else {
            return {ERROR, NORMAL, "device %s id %d is already have the same id device: %s.",
                    device.ip().c_str(), device.azimuth(), m_devices_list[device.azimuth()].etc->ip().c_str()};
        }
    }

    // 检索相机
    if (SearchDevice() != SUCCESS) {
        return {ERROR, NORMAL, "search devices failed, please recheck the internet."};
    }

    // 配置相机列表配置
    for (auto &device: m_devices_list) {
        if (device.etc->enable_device() && device.handle == nullptr) {
            ConnectDevice(device.etc->azimuth());
            m_devices_status[device.etc->azimuth()] = 0;
            setTof3dParams(device.handle, device.etc->bip());
            device.handle_mutex = &all_device_mutex;
            device.condit = new Thread_condition();
            device.t = new std::thread(&DeviceTof3D::run, this, device.etc->azimuth());
        }
    }

    // 配置自动重连功能
    VZ_SetHotPlugStatusCallback(DeviceTof3D::HotPlugStateCallback, this);

    // 使能相机列表设备
    for (auto &device: m_devices_list) {
        if (device.etc->enable_device()) {
            device.condit->notify_all(true);
        }
    }

    return {SUCCESS, NORMAL, "VzInitialize success."};
}

Error_manager DeviceTof3D::updateTrans(const DeviceAzimuth &azimuth, const CoordinateTransformation3D &trans) {
    rotation_mutex.lock();
    rotation_matrix3[azimuth] =
            Eigen::AngleAxisf(trans.yaw() * M_PI / 180.f, Eigen::Vector3f::UnitZ()) *
            Eigen::AngleAxisf(trans.pitch() * M_PI / 180.f, Eigen::Vector3f::UnitY()) *
            Eigen::AngleAxisf(trans.roll() * M_PI / 180.f, Eigen::Vector3f::UnitX());
    move[azimuth] << trans.x(), trans.y(), trans.z();
    rotation_mutex.unlock();
    return {};
}

cv::Mat DeviceTof3D::getDeviceIrMat() {
    cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
    for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
        if (!m_device_mat[device_index].timeout()) {
            auto t_device_mat = m_device_mat[device_index]();
            // 拼接图像
            cv::Mat merge_mat_lf = merge_mat(cv::Rect(0, 0, 640, 480));
            t_device_mat.irMat.copyTo(
                    merge_mat(cv::Rect(
                            (device_index & 0x1) * t_device_mat.irMat.cols,
                            ((device_index & 0x2) >> 1) * t_device_mat.irMat.rows,
                            t_device_mat.irMat.cols,
                            t_device_mat.irMat.rows)));
        } else {
            DLOG(WARNING) << device_index << " not get data.";
        }
    }
    return merge_mat;
}

cv::Mat DeviceTof3D::getDevicePointsMat(const DeviceAzimuth &azimuth) {
    if (azimuth < DeviceAzimuth::DEVICE_AZIMUTH_MAX && !m_device_mat[azimuth].timeout()) {
        return m_device_mat[azimuth]().pointMat;
    }
    return {};
}

DeviceTof3D::DeviceTof3DSaveInfo DeviceTof3D::getDeviceSaveInfo(const DeviceAzimuth &azimuth) {
    return azimuth < DeviceAzimuth::DEVICE_AZIMUTH_MAX ? m_device_mat[azimuth]() : DeviceTof3DSaveInfo{};
}

Error_manager DeviceTof3D::getDeviceStatus() {
    for (auto &statu: m_devices_status) {
        if (statu() != 0) {
            return {FAILED, NORMAL};
        }
    }
    return {};
}

Error_manager DeviceTof3D::SearchDevice(const double &time) {
    uint32_t deviceCount = 0;

    auto t = std::chrono::steady_clock::now();
    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t;
    while (cost.count() < time) {
        auto t_vz_status = VZ_GetDeviceCount(&deviceCount);
        cost = std::chrono::steady_clock::now() - t;
        if (t_vz_status != VzReturnStatus::VzRetOK) {
            LOG(INFO) << "VzGetDeviceCount failed! make sure the DCAM is connected";
            std::this_thread::sleep_for(std::chrono::seconds(1));
            continue;
        }
        LOG(INFO) << "Found device count: " << deviceCount;
        std::this_thread::sleep_for(std::chrono::seconds(1));

        // 校验是否含有全部设备
        if (isAllDevicesFound(deviceCount)) {
            return {SUCCESS, NORMAL};
        }
    }

    return {ERROR, NORMAL, "Can't find all devices in config file."};
}

bool DeviceTof3D::isAllDevicesFound(uint32_t deviceCount) {
//    if (deviceCount < DeviceAzimuth::DEVICE_AZIMUTH_MAX) {
//        return false;
//    }

    auto *pDeviceListInfo = new VzDeviceInfo[16];
    VZ_GetDeviceInfoList(deviceCount, pDeviceListInfo);

    for (auto &device: m_devices_list) {
        for (int search_index = 0; search_index < deviceCount; search_index++) {
            device.status.found = device.etc->ip() == pDeviceListInfo[search_index].ip;
            if (device.status.found) {
                break;
            }
        }
    }

    for (auto &device: m_devices_list) {
        if (device.etc->enable_device() && !device.status.found) {
            return false;
        }
    }
    return true;
}

Error_manager DeviceTof3D::setTof3dParams(VzDeviceHandle handle, const Tof3dVzenseBuiltInParams &params) {
    if (handle == nullptr) {
        LOG(WARNING) << "handle is nullptr!!!";
        return {ERROR, CRITICAL_ERROR, "%s handle is nullptr!!!"}; //TODO
    }

    auto t_vz_status = VZ_SetWorkMode(handle, (VzWorkMode) params.work_mode());
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_SetWorkMode failed status:" << t_vz_status;
        return {};  //TODO
    }

    t_vz_status = VZ_SetIRGMMGain(handle, params.irgmmgain());
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_SetIRGMMGain failed status:" << t_vz_status;
        return {};  //TODO
    }

    t_vz_status = VZ_SetFrameRate(handle, params.frame_rate());
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_SetFrameRate failed status:" << t_vz_status;
        return {};  //TODO
    }

    VzExposureTimeParams exposure_param;
    exposure_param.mode = (VzExposureControlMode) params.enable_manual_exposure_time();
    exposure_param.exposureTime = params.exposure_time();
    t_vz_status = VZ_SetExposureTime(handle, VzToFSensor, exposure_param);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_SetExposureTime failed status:" << t_vz_status;
        return {};  //TODO
    }

    t_vz_status = VZ_SetFillHoleFilterEnabled(handle, params.enable_filter_fill_hole());
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_SetFillHoleFilterEnabled failed status:" << t_vz_status;
        return {};  //TODO
    }

    t_vz_status = VZ_SetSpatialFilterEnabled(handle, params.enable_filter_spatial());
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_SetSpatialFilterEnabled failed status:" << t_vz_status;
        return {};  //TODO
    }

    VzFlyingPixelFilterParams fly;
    fly.enable = params.enable_flying_pixel_filter();
    fly.threshold = params.flying_pixel_filter_value();
    t_vz_status = VZ_SetFlyingPixelFilterParams(handle, fly);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_SetFlyingPixelFilterParams failed status:" << t_vz_status;
        return {};  //TODO
    }

    VzConfidenceFilterParams confidence;
    confidence.enable = params.enable_confidence_filter();
    confidence.threshold = params.confidence_filter_value();
    t_vz_status = VZ_SetConfidenceFilterParams(handle, confidence);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_SetConfidenceFilterParams failed status:" << t_vz_status;
        return {};  //TODO
    }

    VzTimeFilterParams time_filter;
    time_filter.enable = params.enable_time_filter();
    time_filter.threshold = params.time_filter_value();
    t_vz_status = VZ_SetTimeFilterParams(handle, time_filter);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_SetTimeFilterParams failed status:" << t_vz_status;
        return {};  //TODO
    }

    return Error_manager();
}

Error_manager DeviceTof3D::getTof3dParams(VzDeviceHandle handle, Tof3dVzenseBuiltInParams &params) {
    if (handle == nullptr) {
        LOG(WARNING) << "handle is nullptr!!!";
        return {ERROR, CRITICAL_ERROR, "%s handle is nullptr!!!"}; //TODO
    }

    // 获取参数
    VzWorkMode mode;
    auto t_vz_status = VZ_GetWorkMode(handle, &mode);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_GetWorkMode failed status:" << t_vz_status;
        return {};  //TODO
    }
    params.set_work_mode(mode);

    uint8_t irgmmgain;
    t_vz_status = VZ_GetIRGMMGain(handle, &irgmmgain);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_GetIRGMMGain failed status:" << t_vz_status;
        return {};  //TODO
    }
    params.set_irgmmgain(irgmmgain);

    int frame_rate;
    t_vz_status = VZ_GetFrameRate(handle, &frame_rate);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_GetFrameRate failed status:" << t_vz_status;
        return {};  //TODO
    }
    params.set_frame_rate(frame_rate);

    VzExposureTimeParams exposure_param = {VzExposureControlMode_Manual, 0};
    t_vz_status = VZ_GetProperty(handle, "Py_ToFExposureTimeMax", &exposure_param,
                                 sizeof(exposure_param));
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_GetExposureTime failed status:" << t_vz_status;
        return {};  //TODO
    }
    params.set_enable_manual_exposure_time(exposure_param.mode);
    params.set_exposure_time(exposure_param.exposureTime);

    bool boolret;
    t_vz_status = VZ_GetFillHoleFilterEnabled(handle, &boolret);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_GetFillHoleFilterEnabled failed status:" << t_vz_status;
        return {};  //TODO
    }
    params.set_enable_filter_fill_hole(boolret);

    t_vz_status = VZ_GetSpatialFilterEnabled(handle, &boolret);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_GetSpatialFilterEnabled failed status:" << t_vz_status;
        return {};  //TODO
    }
    params.set_enable_filter_spatial(boolret);

    VzFlyingPixelFilterParams fly;
    t_vz_status = VZ_GetFlyingPixelFilterParams(handle, &fly);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_GetFlyingPixelFilterParams failed status:" << t_vz_status;
        return {};  //TODO
    }
    params.set_enable_flying_pixel_filter(fly.enable);
    params.set_flying_pixel_filter_value(fly.threshold);

    VzConfidenceFilterParams confidence;
    t_vz_status = VZ_GetConfidenceFilterParams(handle, &confidence);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_GetConfidenceFilterParams failed status:" << t_vz_status;
        return {};  //TODO
    }
    params.set_enable_confidence_filter(confidence.enable);
    params.set_confidence_filter_value(confidence.threshold);

    VzTimeFilterParams time_filter;
    t_vz_status = VZ_GetTimeFilterParams(handle, &time_filter);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << " VZ_GetTimeFilterParams failed status:" << t_vz_status;
        return {};  //TODO
    }
    params.set_enable_time_filter(time_filter.enable);
    params.set_time_filter_value(time_filter.threshold);

    return Error_manager();
}

Error_manager DeviceTof3D::ConnectDevice(const DeviceAzimuth &azimuth) {
    auto &device = m_devices_list[azimuth];
    auto t_vz_status = VZ_OpenDeviceByIP(device.etc->ip().c_str(), &device.handle);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        device.status.connected = false;
        LOG(WARNING) << "OpenDevice " << device.etc->ip() << " failed status:" << t_vz_status;
        return {ERROR, NORMAL};
    } else {
        device.status.connected = true;
        LOG(INFO) << "OpenDevice " << device.etc->ip() << " success status:" << t_vz_status;
    }

    t_vz_status = VZ_StartStream(device.handle);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << "VZ_StartStream " << device.etc->ip() << " failed status:" << t_vz_status;
        return {ERROR, NORMAL};
    } else {
        LOG(INFO) << "VZ_StartStream " << device.etc->ip() << " success status:" << t_vz_status;
    }
    return {};
}

Error_manager DeviceTof3D::StartStreamDevice(VzDeviceHandle handle) {
    if (handle == nullptr) {
        return {ERROR, CRITICAL_ERROR};
    }

    auto t_vz_status = VZ_StartStream(handle);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << "VZ_StartStream failed status:" << t_vz_status;
    } else {
        LOG(INFO) << "VZ_StartStream success status:" << t_vz_status;
    }

    return {};
}

Error_manager DeviceTof3D::DisConnectDevice(VzDeviceHandle handle) {
    if (handle == nullptr) {
        return {ERROR, CRITICAL_ERROR};
    }

    auto t_vz_status = VZ_StopStream(handle);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << "VZ_StopStream failed status:" << t_vz_status;
    } else {
        LOG(INFO) << "VZ_StopStream success status:" << t_vz_status;
    }

    t_vz_status = VZ_CloseDevice(&handle);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << "VZ_CloseDevice failed status:" << t_vz_status;
    } else {
        LOG(INFO) << "VZ_CloseDevice success status:" << t_vz_status;
    }

    return {};
}

Error_manager DeviceTof3D::CloseStreamDevice(VzDeviceHandle handle) {
    if (handle == nullptr) {
        return {ERROR, CRITICAL_ERROR};
    }

    auto t_vz_status = VZ_StopStream(handle);
    if (t_vz_status != VzReturnStatus::VzRetOK) {
        LOG(WARNING) << "VZ_StopStream failed status:" << t_vz_status;
    } else {
        LOG(INFO) << "VZ_StopStream success status:" << t_vz_status;
    }

    return {};
}

Error_manager DeviceTof3D::getDepthAndIrPicture(VzDeviceHandle handle, VzFrame &depthFrame, VzFrame &irFrame) {
    if (handle == nullptr) {
        return {ERROR, CRITICAL_ERROR};
    }

    Error_manager ret;
    VzFrameReady frameReady = {0};
    auto t_vz_status = VZ_GetFrameReady(handle, 200, &frameReady);

    if (t_vz_status != VzRetOK) {
        DLOG(WARNING) << "Device VZ_GetFrameReady failed status:" << t_vz_status;
        return {FAILED, MINOR_ERROR, "Device VZ_GetFrameReady failed status: %d.", t_vz_status};    //TODO
    }

    if (1 == frameReady.depth) {
        t_vz_status = VZ_GetFrame(handle, VzDepthFrame, &depthFrame);

        if (t_vz_status == VzRetOK && depthFrame.pFrameData != nullptr) {
            // LOG(INFO) << ip << " frameIndex :" << depthFrame.frameIndex;
        } else {
            LOG(INFO) << "Device VZ_GetFrame VzIrFrame failed status: " << t_vz_status;
            return {FAILED, MINOR_ERROR, "Device VZ_GetFrame VzIrFrame failed status: %d.", t_vz_status};
        }
    } else {
        return {FAILED, MINOR_ERROR, "Device VZ_GetFrameReady depth not ready: %d.", frameReady.depth};
    }

    if (1 == frameReady.ir) {
        t_vz_status = VZ_GetFrame(handle, VzIRFrame, &irFrame);
        if (t_vz_status == VzRetOK && irFrame.pFrameData != nullptr) {
            // LOG(INFO) << ip << " frameIndex :" << irFrame.frameIndex;
        } else {
            LOG(INFO) << "VZ_GetFrame VzIrFrame status:" << t_vz_status;
            return {FAILED, MINOR_ERROR, "Device VZ_GetFrame VzIrFrame failed status: %d.", t_vz_status};
        }
    } else {
        return {FAILED, NORMAL, "Device VZ_GetFrameReady ir not ready."};
    }

    return ret;
}

Error_manager DeviceTof3D::DepthFrame2Mat(VzFrame &frame, cv::Mat &mat) {
    if (frame.frameType != VzDepthFrame) {
        return {};
    }

    if (mat.type() != CV_16UC1) {
        LOG(INFO) << "Mat type " << mat.type() << " not is " << CV_16UC1;
        return {};
    }

    if (mat.rows != 480 || mat.cols != 640) {
        LOG(INFO) << "Mat size (" << mat.rows << ", " << mat.cols << ") not is (480, 640)";
        return {};
    }

    memcpy(mat.data, frame.pFrameData, frame.dataLen);
    mat.convertTo(mat, CV_8U, 255.0 / 7495);
    applyColorMap(mat, mat, cv::COLORMAP_RAINBOW);

    return {};
}

Error_manager DeviceTof3D::IrFrame2Mat(VzFrame &frame, cv::Mat &mat) {
    if (frame.frameType != VzIRFrame) {
        return {};
    }

    if (mat.type() != CV_8UC1) {
        LOG(INFO) << "Mat type " << mat.type() << " not is " << CV_8UC1;
        return {};
    }

    if (mat.rows != 480 || mat.cols != 640) {
        LOG(INFO) << "Mat size (" << mat.rows << ", " << mat.cols << ") not is (480, 640)";
        return {};
    }

    memcpy(mat.data, frame.pFrameData, frame.dataLen);

    return {};
}

Error_manager DeviceTof3D::Frame2Mat(VzFrame &frame, cv::Mat &mat) {
    switch (frame.frameType) {
        case VzDepthFrame:
            return DepthFrame2Mat(frame, mat);
        case VzIRFrame:
            return IrFrame2Mat(frame, mat);
        default:
            break;
    }
    return {};
}

void DeviceTof3D::HotPlugStateCallback(const VzDeviceInfo *pInfo, int status, void *contex) {
    LOG(WARNING) << "ip " << pInfo->ip << "    " << (status == 0 ? "add" : "remove");

    if (contex == nullptr) {
        LOG(WARNING) << contex << " contex is nullptr.";
        return;
    }

    DeviceTof3D *pDeviceTof3D = (DeviceTof3D *) contex;

    for (int device_index = 0; device_index < pDeviceTof3D->m_devices_list.size(); device_index++) {
        // LOG(INFO) << device_index << " " << &pDeviceTof3D->m_devices_list[device_index] << " " << pDeviceTof3D->m_devices_list[device_index].etc->ip();
        pDeviceTof3D->m_devices_list[device_index].handle_mutex->lock();
        if (pDeviceTof3D->m_devices_list[device_index].etc->ip() == pInfo->ip) {
            if (status == 0) {
                LOG(WARNING) << pInfo->ip << " VZ_OpenDevice "
                             << VZ_OpenDeviceByUri(pInfo->uri, &pDeviceTof3D->m_devices_list[device_index].handle);
                LOG(WARNING) << pInfo->ip << " VZ_StartStream "
                             << VZ_StartStream(pDeviceTof3D->m_devices_list[device_index].handle);
            } else {
                LOG(WARNING) << pInfo->ip << " VZ_StopStream "
                             << VZ_StopStream(pDeviceTof3D->m_devices_list[device_index].handle);
                LOG(WARNING) << pInfo->ip << " VZ_CloseDevice "
                             << VZ_CloseDevice(&pDeviceTof3D->m_devices_list[device_index].handle);
            }
            pDeviceTof3D->m_devices_status[device_index] = status;
        }
        pDeviceTof3D->m_devices_list[device_index].handle_mutex->unlock();
    }
}

void DeviceTof3D::run(const DeviceAzimuth &azimuth) {
    auto &device = m_devices_list[azimuth];

    cv::Mat t_ir_img;
    cv::Mat t_point_img;
    VzFrame depthFrame = {0};
    VzFrame irFrame = {0};
    VzVector3f worldV[480 * 640];// = new VzVector3f;

    auto t_start_time = std::chrono::steady_clock::now();
    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
    TofTransformationManager::iter()->set(azimuth, device.etc->trans());

    LOG(INFO) << device.etc->ip() << ": " << azimuth << " in thread " << std::this_thread::get_id();
    CoordinateTransformation3D device_trans;
    while (device.condit->is_alive()) {
        device.condit->wait();
        cost = std::chrono::steady_clock::now() - t_start_time;
        std::this_thread::sleep_for(std::chrono::milliseconds(100 - std::min<int>(99, cost.count() * 1000)));
        t_start_time = std::chrono::steady_clock::now();

        if (device.condit->is_alive()) {
            Error_manager ret;
            DeviceTof3DSaveInfo depthInfo;
            Error_manager vz_ret = {FAILED, NORMAL};
            // 更新旋转坐标
            if (TofTransformationManager::iter()->get(azimuth, device_trans)) {
                updateTrans(azimuth, device_trans);
            }
            // 相机采集数据
            device.handle_mutex->lock();
            if (device.handle) {
                vz_ret = getDepthAndIrPicture(device.handle, depthFrame, irFrame);
                if (vz_ret == SUCCESS) {
                    VZ_ConvertDepthFrameToPointCloudVector(device.handle, &depthFrame, worldV);
                } else {
                    ret.compare_and_merge_up(
                            {FAILED, MINOR_ERROR, "Device %s VZ_GetFrame failed.", device.etc->ip().c_str()});
                }
            }
            device.handle_mutex->unlock();

            if (vz_ret == SUCCESS) {
                // 灰度图转换
                Frame2Mat(irFrame, depthInfo.irMat);
                // 点云转换
                for (int cols = 0; cols < depthInfo.pointMat.cols - 0; cols++) {
                    for (int rows = 0; rows < depthInfo.pointMat.rows - 0; rows++) {
                        int i = rows * depthInfo.pointMat.cols + cols;
                        if (sqrt(worldV[i].x * worldV[i].x + worldV[i].y * worldV[i].y + worldV[i].z * worldV[i].z) *
                            0.001f > 5) {
                            continue;
                        }
                        Eigen::Vector3f trans;
                        trans << worldV[i].x * 0.001f, worldV[i].y * 0.001f, worldV[i].z * 0.001f;
                        trans = rotation_matrix3[azimuth] * trans + move[azimuth];
                        if (trans(2) < 0.03 || fabs(trans(0)) > 1.5 || fabs(trans(1)) > 2.7 || trans(2) > 1) {
                            // continue;
                        }
                        depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0] = trans(0);
                        depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[1] = trans(1);
                        depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[2] = trans(2);
                    }
                }
                t_ir_img = depthInfo.irMat.clone();
                t_point_img = depthInfo.pointMat.clone();
                
                // 将图片和点云存储起来
                TransitData::get_instance_pointer()->setImage(t_ir_img, azimuth);
                TransitData::get_instance_pointer()->setImageCL(t_point_img, azimuth);
                m_device_mat[azimuth] = depthInfo;
                
            }
        } else {
            LOG_EVERY_N(WARNING, 1000) << device.etc->ip() << " thread was killed.";
        }
    }

    LOG(INFO) << device.etc->ip() << " thread end " << std::this_thread::get_id();
}

void DeviceTof3D::stop() {
    for (auto &device: m_devices_list) {
        DisConnectDevice(device.handle);
        device.condit->kill_all();
        device.t->join();
        delete device.t;
        delete device.condit;
        device.t = nullptr;
        device.t = nullptr;
    }
}

Error_manager DeviceTof3D::Stop() {
    for (auto &device: m_devices_list) {
        if (device.condit->is_alive()) {
            device.condit->kill_all();
            device.t->join();
            delete device.t;
            device.t = nullptr;
            CloseStreamDevice(device.handle);
        }
    }
    return {};
}

Error_manager DeviceTof3D::Continue() {
    for (auto &device: m_devices_list) {
        if (device.condit->is_alive()) {
            continue;
        }
        StartStreamDevice(device.handle);
        device.condit->reset();
        device.t = new std::thread(&DeviceTof3D::run, this, device.etc->azimuth());
        device.condit->notify_all(true);
    }

    return {};
}