//
// Created by zx on 2023/11/24.
//

#include "detect_manager.h"

Error_manager DetectManager::Init(const google::protobuf::RepeatedPtrField<tof3dVzenseEtc> &tof_devices) {
    for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
        TofTransformationManager::iter()->set((DeviceAzimuth) device_index, tof_devices[device_index].trans());
        updateTrans((DeviceAzimuth) device_index, tof_devices[device_index].trans());
    }

#ifdef ENABLE_TENSORRT_DETECT
    detector = new TensorrtWheelDetector(ETC_PATH PROJECT_NAME "/model.engine", ETC_PATH PROJECT_NAME "/class.txt");
#else
    detector = new TensorrtWheelDetector(ETC_PATH PROJECT_NAME "/model.onnx", ETC_PATH PROJECT_NAME "/class.txt");
#endif

    m_thread = new std::thread(&DetectManager::run, this);
    m_condit.notify_all(true);

    return {};
}

Error_manager DetectManager::Release() {
    stop();
    return {};
}

Error_manager DetectManager::updateTrans(const DeviceAzimuth &azimuth, const CoordinateTransformation3D &trans) {
    Eigen::Matrix4f rotaition = Eigen::Matrix4f::Identity();
    Eigen::Matrix3f rotation_matrix3 = Eigen::Matrix3f::Identity();
    rotation_matrix3 =
            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());
    Eigen::Vector3f move;
    move << trans.x(), trans.y(), trans.z();
    rotaition.block<3, 3>(0, 0) = rotation_matrix3;
    rotaition.col(3) << move;
    rotation_matrix4[azimuth] = rotaition;
    return {};
}

#include "tool/time.hpp"

bool boxIsValid(const JetStream::SegBox &box, int min, bool reverse = false) {
    if (box.confidence() < 0.8) {
        return false;
    }
    int left_side = box.x();
    if (reverse) {
        left_side = 640 - (box.x() + box.width());
    }

    return left_side > min;
}

int IsInImageValidRegion(const JetStream::LabelYolo &labels) {
    if (labels.boxes_size() != 4) {
        return -1; //没有识别框
    }

    for (int device_index = 0; device_index < 4; device_index++) {
        // LOG(INFO) << device_index << ": "<< labels.boxes(device_index).confidence() << " " << labels.boxes(device_index).x();
    }

    JetStream::SegBox box2 = labels.boxes(1);
    JetStream::SegBox box4 = labels.boxes(3);
    bool frontExsit = labels.boxes(0).confidence() > 0.8 || labels.boxes(1).confidence() > 0.8;
    bool backExist = labels.boxes(2).confidence() > 0.8 || labels.boxes(3).confidence() > 0.8;
    if (!frontExsit) {
        if (backExist) {  //后有前没有
            return 1;  //往前
        } else {  //前后都没有
            return -1;
        }
    } else {
        if (backExist) {
            bool front_OK = boxIsValid(labels.boxes(1), 20) || boxIsValid(labels.boxes(0), 20, true);
            bool back_OK = boxIsValid(labels.boxes(2), 20, true) || boxIsValid(labels.boxes(3), 20);
            if (front_OK && back_OK) {
                return 0;
            } else {
                return 1;
            }
        } else {  // 前有后没有
            bool front_OK = boxIsValid(labels.boxes(1), 320) || boxIsValid(labels.boxes(0), 320, true);
            if (!front_OK) {
                return 1;
            } else {
                return -1;
            }
        }
    }
}

void DetectManager::run() {
    LOG(INFO) << "detect thread running in " << std::this_thread::get_id();

    DetectResult m_detect_result_record;
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vct_wheel_cloud;
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vct_tires_cloud;
    for (int i = 0; i < DeviceAzimuth::DEVICE_AZIMUTH_MAX; i++) {
        vct_wheel_cloud.emplace_back(new pcl::PointCloud<pcl::PointXYZ>);
        vct_tires_cloud.emplace_back(new pcl::PointCloud<pcl::PointXYZ>);
    }

    auto t_start_time = std::chrono::steady_clock::now();
    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;

    while (m_condit.is_alive()) {
        m_condit.wait();
        // 参数初始化
        cost = std::chrono::steady_clock::now() - t_start_time;
        DLOG(INFO) << "last while cost time is " << cost.count() * 1000 << " ms";
        std::this_thread::sleep_for(std::chrono::milliseconds(100 - std::min<int>(49, cost.count() * 1000)));
        t_start_time = std::chrono::steady_clock::now();

        if (m_condit.is_alive()) {
            std::vector<int> have_cloud;
            DetectResult detect_result;
            for (int i = 0; i < DeviceAzimuth::DEVICE_AZIMUTH_MAX; i++) {
                vct_wheel_cloud[i]->clear();
                vct_tires_cloud[i]->clear();
            }

            cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
            DeviceTof3D::DeviceTof3DSaveInfo devices_info[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                devices_info[device_index] = DeviceTof3D::iter()->getDeviceSaveInfo((DeviceAzimuth) device_index);
                devices_info[device_index].irMat.copyTo(merge_mat(cv::Rect((device_index & 0x1) * 640,
                                                                           ((device_index & 0x2) >> 1) * 480, 640, 480)));
            }

            cost = std::chrono::steady_clock::now() - t_start_time;
            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
            JetStream::LabelYolo seg_results;
            YoloSegMergeData(merge_mat, seg_results);
            cost = std::chrono::steady_clock::now() - t_start_time;
            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";

            // 判断识别结果数量以及是否存图
            int seg_num = 0;
            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                if (seg_results.boxes(device_index).confidence() > 0.7) {
                    seg_num++;
                }
            }
            if (seg_num == 0) {
                detect_statu = MeasureStatu::Measure_Empty_Clouds;
                m_detect_result_record.reset();
                continue;
            }

            // 取高置信度识别结果
            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                if (seg_results.boxes(device_index).confidence() < 0.9) {
                    continue;
                }
                cv::Mat pointMat = devices_info[device_index].pointMat;
                have_cloud.push_back(device_index);
                std::vector<cv::Point> seg_points;
                for (int i = 0; i < seg_results.boxes(device_index).wheels_size(); i++) {
                    int index = seg_results.boxes(device_index).wheels(i).point();
                    int col = index % 640;
                    int row = index / 640;
                    seg_points.emplace_back(col, row);
                    vct_wheel_cloud[device_index]->emplace_back(pointMat.at<cv::Vec4f>(row, col)[0],
                                                                pointMat.at<cv::Vec4f>(row, col)[1],
                                                                pointMat.at<cv::Vec4f>(row, col)[2]);
                }
                for (int i = 0; i < seg_results.boxes(device_index).tires_size(); i++) {
                    int index = seg_results.boxes(device_index).tires(i).point();
                    int col = index % 640;
                    int row = index / 640;
                    seg_points.emplace_back(col, row);
                    vct_tires_cloud[device_index]->emplace_back(pointMat.at<cv::Vec4f>(row, col)[0],
                                                                pointMat.at<cv::Vec4f>(row, col)[1],
                                                                pointMat.at<cv::Vec4f>(row, col)[2]);
                }
                TransitData::get_instance_pointer()->setMask(seg_points, device_index);
            }
            cost = std::chrono::steady_clock::now() - t_start_time;
            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";

            // 判断当前识别状态是否需要进行测量
            int flag = IsInImageValidRegion(seg_results);
            if (flag == 1) {
                detect_statu = MeasureStatu::Measure_Border;
                continue;
            } else if (flag == -1) {
                detect_statu = MeasureStatu::Measure_Failture;
                continue;
            }

            // 下采样点云
            WheelCloudOptimize(vct_wheel_cloud, detect_result);
            WheelCloudOptimize(vct_tires_cloud, detect_result);
            cost = std::chrono::steady_clock::now() - t_start_time;
            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";

            bool ceres_detect_ret = false;
            auto wheel_detect = detect_wheel_ceres::iter();
            // 轮毂平面检测
            Car_pose_detector::CarDetectResult wheel_ransic_ret;
            wheel_detect->detect_ransic(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
                                        vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
                                        wheel_ransic_ret.wheels_center_x, wheel_ransic_ret.wheels_center_y,
                                        wheel_ransic_ret.theta, wheel_ransic_ret.wheel_base, wheel_ransic_ret.wheel_width,
                                        wheel_ransic_ret.front_wheels_theta, wheel_ransic_ret.loss);
            // 轮胎平面检测
            Car_pose_detector::CarDetectResult tires_ransic_ret;
            wheel_detect->detect_ransic(vct_tires_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_tires_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
                                        vct_tires_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_tires_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
                                        tires_ransic_ret.wheels_center_x, tires_ransic_ret.wheels_center_y,
                                        tires_ransic_ret.theta, tires_ransic_ret.wheel_base, tires_ransic_ret.wheel_width,
                                        tires_ransic_ret.front_wheels_theta, tires_ransic_ret.loss);

            // 双模型检测：采用上次测量结果的数据
            Car_pose_detector::CarDetectResult wheel_2step_ret;
            if (!m_detect_result_time_lock_data.timeout(0.5)) {
                wheel_2step_ret = m_detect_result_time_lock_data().car;
                wheel_2step_ret.wheel_width = MIN(wheel_2step_ret.wheel_width + 0.4, 2.5);
            } else {
                wheel_2step_ret.wheel_width = 3;
                wheel_2step_ret.wheel_base = 2.7;
            }
            if (m_detect_result_record.car.wheel_width < 1.0 || m_detect_result_record.car.wheel_base < 2.0 ||
                have_cloud.size() > 2) {
                ceres_detect_ret = wheel_detect->detect_2Step(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF],
                                                              vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
                                                              vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR],
                                                              vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
                                                              wheel_2step_ret.wheels_center_x, wheel_2step_ret.wheels_center_y,
                                                              wheel_2step_ret.theta, wheel_2step_ret.wheel_base,
                                                              wheel_2step_ret.wheel_width,
                                                              wheel_2step_ret.front_wheels_theta, wheel_2step_ret.loss);
                if ((m_detect_result_record.car.wheel_width < 1.0 && m_detect_result_record.car.wheel_base < 1.0 &&
                     have_cloud.size() == 2) || have_cloud.size() < 2) {
                    wheel_2step_ret.wheel_width = m_detect_result_record.car.wheel_width;
                    wheel_2step_ret.wheel_base = m_detect_result_record.car.wheel_base;
                } else if (have_cloud.size() == 2) {
                    for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                        if (!vct_wheel_cloud[device_index]->empty()) {
                            have_cloud.push_back(device_index);
                        }
                    }
                    if (have_cloud[0] % 2 == have_cloud[1] % 2) {
                        wheel_2step_ret.wheel_width = m_detect_result_record.car.wheel_width;
                    } else {
                        wheel_2step_ret.wheel_base = m_detect_result_record.car.wheel_base;
                    }
                }
            } else {
                ceres_detect_ret = wheel_detect->detect_dynP(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF],
                                                             vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
                                                             vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR],
                                                             vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
                                                             wheel_2step_ret.wheels_center_x, wheel_2step_ret.wheels_center_y,
                                                             wheel_2step_ret.theta, m_detect_result_record.car.wheel_base,
                                                             m_detect_result_record.car.wheel_width,
                                                             wheel_2step_ret.front_wheels_theta, wheel_2step_ret.loss);
                wheel_2step_ret.wheel_base = m_detect_result_record.car.wheel_base;
                wheel_2step_ret.wheel_width = m_detect_result_record.car.wheel_width;
            }

            // 综合测量结果
            if (MergeAllMeaureResult(detect_result.car, wheel_ransic_ret, tires_ransic_ret, wheel_2step_ret) == MeasureStatu::Measure_Failture) {
                detect_statu = MeasureStatu::Measure_Failture;
            }

            // 如果车辆运动，不断刷新运动时间，如果由运动转为静止则刷新运动状态到静止
            float move_distance = detect_result.car.wheels_center_y - m_detect_result_record.car.wheels_center_y;
            if (fabs(move_distance) > 0.1) {
                move_statu = true;
            } else if (move_statu()) {
                move_statu = false;
            }
            if (ceres_detect_ret) {
                // 记录下测量结果
                m_detect_result_record = detect_result;
                detect_statu = Measure_OK;
                m_detect_result_time_lock_data = m_detect_result_record;
            } else {
                detect_statu = Measure_Failture;
            }

        }
    }
    LOG(INFO) << "detect thread " << std::this_thread::get_id() << " closed.";
}

void DetectManager::stop() {
    //杀死线程,
    m_condit.kill_all();

    //在线程join结束之后, 就可以可以回收线程内存资源
    m_thread->join();
    delete m_thread;
    m_thread = nullptr;
}

Error_manager DetectManager::WheelCloudOptimize(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &vct_wheel_cloud,
                                                DetectResult &result) {
    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> vox;                     //创建滤波对象
    vox.setLeafSize(0.03f, 0.03f, 0.03f);     //设置滤波时创建的体素体积为1cm的立方体
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
        if (vct_wheel_cloud[device_index]->empty()) { continue; }

        vox.setInputCloud(vct_wheel_cloud[device_index]);                   //设置需要过滤的点云给滤波对象
        vox.filter(*vct_wheel_cloud[device_index]);                         //执行滤波处理，存储输出
    }

    return {};
}

bool isElliptical(float x, float y, float a2, float b2) {
    return (x * x / a2 + y * y / b2) < 1;
}

uint32_t getPixelIndex(int row, int col) {
    // LOG(INFO) << "col " << col << ", row " << row;
    col = MAX(0, MIN(1280, col)) % 640;
    row = MAX(0, MIN(960, row)) % 480;
    // LOG(INFO) << "col " << col << ", row " << row;
    return row * 640 + col;
}

Error_manager DetectManager::YoloSegMergeData(cv::Mat &merge_mat, JetStream::LabelYolo &seg_results) {
    auto t_start_time = std::chrono::steady_clock::now();
    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
    seg_results.add_boxes();
    seg_results.add_boxes();
    seg_results.add_boxes();
    seg_results.add_boxes();

    // 模型识别
    std::vector<Object> objs;
    cv::cvtColor(merge_mat, merge_mat, cv::COLOR_GRAY2RGB);
    detector->detect(merge_mat, objs, merge_mat);
    // cv::imshow("merge_mat", merge_mat);
    // cv::waitKey(1);
    cost = std::chrono::steady_clock::now() - t_start_time;
    DLOG(INFO) << __FUNCTION__ << "cost time is " << cost.count() * 1000 << " ms";

    // 分离识别结果
    auto t = std::chrono::steady_clock::now();
    for (auto iter = objs.begin(); iter != objs.end(); iter++) {
        auto seg_points = detector->getPointsFromObj(*iter);
        int device_index = (int(iter->rect.x / 640) * 0x01) | (int(iter->rect.y / 480) << 1);
        // 校验识别矩形框是否越界
        int device_index_check = (int((iter->rect.x + iter->rect.width) / 640) * 0x01) |
                                 (int((iter->rect.y + iter->rect.height) / 480) << 1);
        if (device_index != device_index_check) {
            // TODO:存图
            objs.erase(iter);
            iter--;
            continue;
        }
        int x_alpha = (device_index & 0x1);
        int y_alpha = ((device_index & 0x2) >> 1);
        seg_results.mutable_boxes(device_index)->set_x(iter->rect.x - x_alpha * merge_mat.cols / 2);
        seg_results.mutable_boxes(device_index)->set_y(iter->rect.y - y_alpha * merge_mat.rows / 2);
        seg_results.mutable_boxes(device_index)->set_width(iter->rect.width);
        seg_results.mutable_boxes(device_index)->set_height(iter->rect.height);
        seg_results.mutable_boxes(device_index)->set_confidence(iter->prob);
        if (iter->prob > 0.6) {
            // 内外椭圆参数
            float a1 = iter->rect.width * 0.5;
            float b1 = iter->rect.height * 0.5;
            float a2 = iter->rect.width * 0.57;
            float b2 = iter->rect.height * 0.57;

            //
            int center_x = iter->rect.x + iter->rect.width * 0.5;
            int center_y = iter->rect.y + iter->rect.height * 0.5;
            int min_x = MAX(0, center_x - a2);
            int min_y = MAX(0, center_y - b2);
            float a12 = a1 * a1;
            float b12 = b1 * b1;
            float a22 = a2 * a2;
            float b22 = b2 * b2;

            for (int x_value = min_x; x_value < center_x; x_value++) {
                auto t_x_value = x_value - center_x;
                for (int y_value = min_y; y_value < center_y; y_value++) {
                    auto t_y_value = y_value - center_y;

                    if (!isElliptical(t_x_value, t_y_value, a22, b22)) {
                        continue;
                    }
                    auto x_value_sym = 2 * center_x - x_value;
                    auto y_value_sym = 2 * center_y - y_value;
                    if (isElliptical(t_x_value, t_y_value, a12, b12)) {
                        seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value, x_value));
                        seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value, x_value_sym));
                        seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value_sym, x_value));
                        seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value_sym, x_value_sym));
                        continue;
                    }
                    seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value, x_value));
                    seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value, x_value_sym));
                    seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value_sym, x_value));
                    seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value_sym, x_value_sym));
                }
            }
        }
    }
    cost = std::chrono::steady_clock::now() - t_start_time;
    DLOG(INFO) << __FUNCTION__ << "cost time is " << cost.count() * 1000 << " ms";
    return {};
}

bool isCarDetectResultNearly(Car_pose_detector::CarDetectResult &a, Car_pose_detector::CarDetectResult &b, float x = 0.015, float y = 0.025, float w = 0.025, float wb = 0.02, float tt = 0.7) {
    bool wheels_center_x = fabs(a.wheels_center_x - b.wheels_center_x) < x * 2 ? true : false;
    bool wheels_center_y = fabs(a.wheels_center_y - b.wheels_center_y) < y * 2 ? true : false;
    bool wheel_width = fabs(a.wheel_width - b.wheel_width) < w * 2 ? true : false;
    bool wheel_base = fabs(a.wheel_base - b.wheel_base) < wb * 2 ? true : false;
    bool theta = fabs(a.theta - b.theta) * 180 / M_PI < tt * 2 ? true : false;
    return wheels_center_x && wheels_center_y && wheel_width && wheel_base && theta;
}

MeasureStatu DetectManager::MergeAllMeaureResult(Car_pose_detector::CarDetectResult &merge_result,
                                                 Car_pose_detector::CarDetectResult &wheel_ransic,
                                                 Car_pose_detector::CarDetectResult &tires_ransic,
                                                 Car_pose_detector::CarDetectResult &wheel_2step) {
    if (fabs(wheel_ransic.theta - tires_ransic.theta) * 180.0 / M_PI < 0.7) {
        merge_result.wheels_center_x = tires_ransic.wheels_center_x;
        merge_result.wheels_center_y = tires_ransic.wheels_center_y;
        merge_result.wheel_width = wheel_2step.wheel_width;
        merge_result.wheel_base = wheel_2step.wheel_base;
        merge_result.theta = tires_ransic.theta;
        merge_result.front_wheels_theta = tires_ransic.front_wheels_theta;
    } else  {
        if (isCarDetectResultNearly(wheel_2step, tires_ransic)) {
            merge_result.wheels_center_x = (tires_ransic.wheels_center_x + wheel_2step.wheels_center_x) * 0.5;
            merge_result.wheels_center_y = (tires_ransic.wheels_center_y + wheel_2step.wheels_center_y) * 0.5;
            merge_result.wheel_width = tires_ransic.wheel_width * 0.3 + wheel_2step.wheels_center_y * 0.7;
            merge_result.wheel_base = (tires_ransic.wheel_base + wheel_2step.wheel_base) * 0.5;
            merge_result.theta = tires_ransic.theta;
            merge_result.front_wheels_theta = tires_ransic.front_wheels_theta;
        } else if (isCarDetectResultNearly(wheel_2step, wheel_ransic)) {
            merge_result.wheels_center_x = (wheel_ransic.wheels_center_x + wheel_2step.wheels_center_x) * 0.5;
            merge_result.wheels_center_y = (wheel_ransic.wheels_center_y + wheel_2step.wheels_center_y) * 0.5;
            merge_result.wheel_width = wheel_ransic.wheel_width * 0.3 + wheel_2step.wheels_center_y * 0.7;
            merge_result.wheel_base = (wheel_ransic.wheel_base + wheel_2step.wheel_base) * 0.5;
            merge_result.theta = wheel_ransic.theta;
            merge_result.front_wheels_theta = wheel_ransic.front_wheels_theta;
        } else {
            return MeasureStatu::Measure_Failture;
        }
    }


    LOG(INFO) << wheel_ransic.info();
    LOG(INFO) << tires_ransic.info();
    LOG(INFO) << wheel_2step.info();
    LOG(INFO) << merge_result.info();
    return MeasureStatu::Measure_OK;
}

MeasureStatu DetectManager::getMeasureResult(DetectManager::DetectResult &result) {
    if (detect_statu() != MeasureStatu::Measure_OK) {
        return detect_statu();
    }
    if (m_detect_result_time_lock_data.timeout(0.3)) {
        return MeasureStatu::Measure_Failture;
    }
    result = m_detect_result_time_lock_data();
    return MeasureStatu::Measure_OK;
}

Error_manager DetectManager::carMoveStatus() {
    if (move_statu.timeout(3) && !move_statu()) {
        return {FAILED, NORMAL};
    }
    return {};
}

Error_manager DetectManager::Stop() {
    m_condit.kill_all();
    m_thread->join();
    delete m_thread;
    m_thread = nullptr;
    return {};
}

Error_manager DetectManager::Continue() {
    m_condit.reset();
    m_thread = new std::thread(&DetectManager::run, this);
    m_condit.notify_all(true);
    return {};
}
