//
// Created by zx on 2021/5/20.
//

#include "ground_region.h"

#include <pcl/common/transforms.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/extract_clusters.h>
#include <fcntl.h>

#include "tool/point_tool.h"

// 测量结果滤波,不影响现有结构
#include "tool/measure_filter.h"

// 增加车辆停止状态判断
#include "tool/region_status_checker.h"
#include "system/system_communication_mq.h"

//欧式聚类*******************************************************
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
Ground_region::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud, float tolerance) {
    std::vector<pcl::PointIndices> ece_inlier;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ece;
    ece.setInputCloud(sor_cloud);
    ece.setClusterTolerance(tolerance);
    ece.setMinClusterSize(20);
    ece.setMaxClusterSize(sor_cloud->size());
    ece.setSearchMethod(tree);
    ece.extract(ece_inlier);

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation_clouds;
    for (int i = 0; i < ece_inlier.size(); i++) {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
        std::vector<int> ece_inlier_ext = ece_inlier[i].indices;
        copyPointCloud(*sor_cloud, ece_inlier_ext, *cloud_copy); //按照索引提取点云数据
        segmentation_clouds.push_back(cloud_copy);
    }
    return segmentation_clouds;
}

/**
 * @description: point rectangle detect
 * @param  points detect if points obey the rectangle rule
 * @return wether forms a rectangle
 */
bool Ground_region::isRect(std::vector<cv::Point2f> &points) {
    if (points.size() == 4) {
        double L[3] = {0.0};
        L[0] = distance(points[0], points[1]);
        L[1] = distance(points[1], points[2]);
        L[2] = distance(points[0], points[2]);
        double max_l = L[0];
        double l1 = L[1];
        double l2 = L[2];
        cv::Point2f ps = points[0], pt = points[1];
        cv::Point2f pc = points[2];
        for (int i = 1; i < 3; ++i) {
            if (L[i] > max_l) {
                max_l = L[i];
                l1 = L[abs(i + 1) % 3];
                l2 = L[abs(i + 2) % 3];
                ps = points[i % 3];
                pt = points[(i + 1) % 3];
                pc = points[(i + 2) % 3];
            }
        }

        //直角边与坐标轴的夹角 <20°
        float thresh = 20.0 * M_PI / 180.0;
        cv::Point2f vct(pt.x - pc.x, pt.y - pc.y);
        float angle = atan2(vct.y, vct.x);
        if (!(fabs(angle) < thresh || (M_PI_2 - fabs(angle) < thresh))) {
            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
            return false;
        }

        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.15) {
            /*char description[255]={0};
            sprintf(description,"angle cos value(%.2f) >0.13 ",cosa);
            std::cout<<description<<std::endl;*/
            return false;
        }

        float width = std::min(l1, l2);
        float length = std::max(l1, l2);
        if (width < 1.400 || width > 1.900 || length > 3.300 || length < 2.200) {
            /*char description[255]={0};
            sprintf(description,"width<1400 || width >1900 || length >3300 ||length < 2200 l:%.1f,w:%.1f",length,width);
            std::cout<<description<<std::endl;*/
            return false;
        }

        double d = distance(pc, points[3]);
        cv::Point2f center1 = (ps + pt) * 0.5;
        cv::Point2f center2 = (pc + points[3]) * 0.5;
        if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150) {
            /*std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;
            char description[255]={0};
            sprintf(description,"Verify failed-4  fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150 ");
            std::cout<<description<<std::endl;*/
            return false;
        }
        //std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;

        return true;
    } else if (points.size() == 3) {
        double L[3] = {0.0};
        L[0] = distance(points[0], points[1]);
        L[1] = distance(points[1], points[2]);
        L[2] = distance(points[0], points[2]);
        double max_l = L[0];
        double l1 = L[1];
        double l2 = L[2];
        int max_index = 0;
        cv::Point2f ps = points[0], pt = points[1];
        cv::Point2f pc = points[2];
        for (int i = 1; i < 3; ++i) {
            if (L[i] > max_l) {
                max_index = i;
                max_l = L[i];
                l1 = L[abs(i + 1) % 3];
                l2 = L[abs(i + 2) % 3];
                ps = points[i % 3];
                pt = points[(i + 1) % 3];
                pc = points[(i + 2) % 3];
            }
        }

        //直角边与坐标轴的夹角 <20°
        float thresh = 20.0 * M_PI / 180.0;
        cv::Point2f vct(pt.x - pc.x, pt.y - pc.y);
        float angle = atan2(vct.y, vct.x);
        if (!(fabs(angle) < thresh || (M_PI_2 - fabs(angle) < thresh))) {
            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
            return false;
        }

        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.15) {
            /*char description[255]={0};
            sprintf(description,"3 wheels angle cos value(%.2f) >0.13 ",cosa);
            std::cout<<description<<std::endl;*/
            return false;
        }

        double l = std::max(l1, l2);
        double w = std::min(l1, l2);
        if (l > 2.100 && l < 3.300 && w > 1.400 && w < 2.100) {
            //生成第四个点
            cv::Point2f vec1 = ps - pc;
            cv::Point2f vec2 = pt - pc;
            cv::Point2f point4 = (vec1 + vec2) + pc;
            points.push_back(point4);

            /*char description[255]={0};
            sprintf(description,"3 wheels rectangle cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
            std::cout<<description<<std::endl;*/
            return true;
        } else {

            /*char description[255]={0};
            sprintf(description,"3 wheels rectangle verify Failed  cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
            std::cout<<description<<std::endl;*/
            return false;
        }
    }
    //std::cout<<" default false"<<std::endl;
    return false;
}

/**
 * @description: 3d wheel detect core func
 * @param cloud  input cloud for measure
 * @param thresh_z  z value to cut wheel
 * @param result  detect result
 * @return wether successfully detected
 */
bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
                                          detect_wheel_ceres3d::Detect_result &result) {
    if (m_detector == nullptr)
        return false;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i = 0; i < cloud->size(); ++i) {
        pcl::PointXYZ pt = cloud->points[i];
        if (pt.z < thresh_z) {
            cloud_filtered->push_back(pt);
        }
    }
    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> vox;   //创建滤波对象
    vox.setInputCloud(cloud_filtered);   //设置需要过滤的点云给滤波对象
    vox.setLeafSize(0.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体
    vox.filter(*cloud_filtered);         //执行滤波处理，存储输出
    if (cloud_filtered->size() == 0) {
        return false;
    }

    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; //创建滤波器对象
    sor.setInputCloud(cloud_filtered);                 //设置待滤波的点云
    sor.setMeanK(5);                                   //设置在进行统计时考虑的临近点个数
    sor.setStddevMulThresh(3.0);                       //设置判断是否为离群点的阀值，用来倍乘标准差，也就是上面的std_mul
    sor.filter(*cloud_filtered);                       //滤波结果存储到cloud_filtered

    if (cloud_filtered->size() == 0) {
        return false;
    }

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
    seg_clouds = segmentation(cloud_filtered, 0.07);

    if (!(seg_clouds.size() == 4 || seg_clouds.size() == 3)) {
        return false;
    }
    std::vector<cv::Point2f> centers;
    cv::Point2f temp_centers(0, 0);
    for (int i = 0; i < seg_clouds.size(); ++i) {
        Eigen::Vector4f centroid;
//        WriteTxtCloud(ETC_PATH"/setting/data/seg_clouds" + std::to_string(i) + ".txt", seg_clouds[i]);
        pcl::compute3DCentroid(*seg_clouds[i], centroid);
        centers.push_back(cv::Point2f(centroid[0], centroid[1]));
        temp_centers += cv::Point2f(centroid[0], centroid[1]);
    }
    temp_centers /= 4.0f;
    bool ret = isRect(centers);
    if (ret) {
        std::string error_str;
        // LOG(WARNING) << "region id: "<< m_region.region_id();
        if (m_detector->detect(seg_clouds, result, error_str)) {
            return true;
        } else {
            // LOG(WARNING) << error_str;
            return false;
        }
    }
    return ret;
}

// constructor
Ground_region::Ground_region() {
    m_region_status = E_UNKNOWN;
    m_detector = nullptr;
    m_measure_thread = nullptr;
}

// deconstructor
Ground_region::~Ground_region() {
    // LOG(WARNING) << "start deconstruct ground region";
    if (m_measure_thread) {
        m_measure_condition.kill_all();
        // Close Capturte Thread
        if (m_measure_thread->joinable()) {
            m_measure_thread->join();
            delete m_measure_thread;
            m_measure_thread = nullptr;
        }
    }
    // LOG(WARNING) << "thread released";
    // 将创建的检测器析构
    if (m_detector) {
        delete m_detector;
        m_detector = nullptr;
    }

    Region_status_checker::get_instance_references().Region_status_checker_uninit();
    // LOG(WARNING) << "detector released";
}

Error_manager Ground_region::init(velodyne::Region region, pcl::PointCloud<pcl::PointXYZ>::Ptr left_model,
                                  pcl::PointCloud<pcl::PointXYZ>::Ptr right_model) {
    m_range_status_last = 0;
    Region_status_checker::get_instance_references().Region_status_checker_init();
    m_region = region;
    m_detector = new detect_wheel_ceres3d(left_model, right_model);
    mp_cloud_collection = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    mp_cloud_filtered = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    mp_cloud_detect_z = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    m_measure_thread = new std::thread(&Ground_region::thread_measure_func, this);
    m_measure_condition.reset();
    m_region_status = E_READY;
    return SUCCESS;
}

Error_manager
Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &last_result) {
    // 1.*********点云合法性检验*********
    if (cloud->size() == 0) {
        // 更新过滤点
        m_filtered_cloud_mutex.lock();
        mp_cloud_filtered->clear();
        m_filtered_cloud_mutex.unlock();
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "no input point");
    }
    WriteTxtCloud(ETC_PATH"/setting/data/cloud.txt", cloud);

    // 2.*********点云预处理*********
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_clean(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i = 0; i < cloud->size(); ++i) {
        pcl::PointXYZ pt = cloud->points[i];
        if (pt.x > m_region.minx() && pt.x < m_region.maxx() && pt.y > m_region.miny() && pt.y < m_region.maxy() &&
            pt.z > m_region.minz() && pt.z < m_region.maxz()) {
            cloud_cut->push_back(pt);
        }
    }
    if (cloud_cut->size() <= 0) {
        // 更新过滤点
        m_filtered_cloud_mutex.lock();
        mp_cloud_filtered->clear();
        m_filtered_cloud_mutex.unlock();
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "cut no point");
    }
   WriteTxtCloud(ETC_PATH"/setting/data/cloud_cut.txt", cloud_cut);

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
    seg_clouds = segmentation(cloud_cut, 0.3);
    std::vector<cv::Point2f> cv_data;
    if (seg_clouds.size() > 0) {
        cloud_cut = seg_clouds[0];
    }
    WriteTxtCloud(ETC_PATH"/setting/data/cloud_cut_2.txt", cloud_cut);

    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> vox;   //创建滤波对象
    vox.setInputCloud(cloud_cut);   //设置需要过滤的点云给滤波对象
    vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体
    vox.filter(*cloud_clean);         //执行滤波处理，存储输出
    if (cloud_clean->size() == 0) {
        // 更新过滤点
        m_filtered_cloud_mutex.lock();
        mp_cloud_filtered->clear();
        m_filtered_cloud_mutex.unlock();
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "ApproximateVoxelGrid no point");
    }

    //离群点过滤
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud_clean);
    sor.setMeanK(5);            //K近邻搜索点个数
    sor.setStddevMulThresh(3.0); //标准差倍数
    sor.setNegative(false);      //保留未滤波点（内点）
    sor.filter(*cloud_filtered); //保存滤波结果到cloud_filter

    // 20221211 changed by yct, 10 point threshold
    if (cloud_filtered->size() <= 10) {
        // 更新过滤点
        m_filtered_cloud_mutex.lock();
        mp_cloud_filtered->clear();
        m_filtered_cloud_mutex.unlock();
        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "StatisticalOutlierRemoval no point");
    }

    // 更新过滤点
    m_filtered_cloud_mutex.lock();
    mp_cloud_filtered->clear();
    mp_cloud_filtered->operator+=(*cloud_filtered);
    m_filtered_cloud_mutex.unlock();

    // 3.*********位姿优化，获取中心xy与角度*********
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_detect_z(new pcl::PointCloud<pcl::PointXYZ>);
    double car_pose_x, car_pose_y, car_pose_theta, car_pose_width, z_value = 0.2;
    // if(!Car_pose_detector::get_instance_references().detect_pose(cloud_filtered, cloud_detect_z, car_pose_x, car_pose_y, car_pose_theta, car_pose_width, z_value, false))
    if (!Car_pose_detector::get_instance_references().detect_pose_mat(cloud_filtered, cloud_detect_z, car_pose_x,
                                                                      car_pose_y, car_pose_theta, false)) {
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR,
                             "find general car pose value failed.");
    }
    // 20230112 added by yct for rough angle border detection
    if (fabs(car_pose_theta * 180.0 / M_PI) > 8) {
        last_result.cx = car_pose_x;
        last_result.cy = car_pose_y;
        last_result.theta = -car_pose_theta * 180.0 / M_PI;
        return Error_manager(VELODYNE_REGION_ANGLE_PRECHECK_FAILED, NEGLIGIBLE_ERROR,
                             "find general car pose value failed.");
    }

    // 二分法存在错误弃用！！！直接使用底盘z值
    std::vector<detect_wheel_ceres3d::Detect_result> results;
    detect_wheel_ceres3d::Detect_result result;
    double chassis_z = 0.24 + m_region.minz(); // + 0.02;
    if (chassis_z > m_region.maxz() || chassis_z < m_region.minz()) {
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR,
                             (std::string("optimized chassis z value out of range: ") +
                              std::to_string(chassis_z)).c_str());
    }

    while (chassis_z > (m_region.minz())) {
        // 初值中x使用第一步优化的值
        result.cx = car_pose_x;
        // 传入整车角度，用于准确判断轮宽，此处需要的是点云的角度，通常逆时针90度到x轴
        result.theta = -(M_PI_2 - car_pose_theta);
//        WriteTxtCloud(ETC_PATH"/setting/data/cloud_filtered.txt", cloud_filtered);
        bool ret = classify_ceres_detect(cloud_filtered, chassis_z, result);
        // changed by yct, 根据底盘z识别，失败则向下移动直至成功或超过底盘内空中点
        if (ret) {
            results.push_back(result);
            break;
        } else {
            chassis_z -= 0.01;
        }
    }

    if (results.size() == 0) {
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "3d wheel detect failed.");
    }

    float min_mean_loss = 1.0;
//    for (int i = 0; i < results.size(); ++i) {
    for (auto &result:results) {
        std::vector<double> loss;
        loss.push_back(result.loss.lf_loss);
        loss.push_back(result.loss.rf_loss);
        loss.push_back(result.loss.lb_loss);
        loss.push_back(result.loss.rb_loss);
        double mean = (result.loss.lf_loss + result.loss.rf_loss + result.loss.lb_loss + result.loss.rb_loss) / 4.0;
        double var = -1.;
        computer_var(loss, var);
        if (mean < min_mean_loss) {
            last_result = result;
            min_mean_loss = mean;
        }
    }

    // 角度校验，理论上car pose检测使用车身点云，应与车轮点云识别得到的车身 x偏移(0.035)与角度(<1°)一致
    double car_pose_theta_deg = 91 + car_pose_theta * 180.0 / M_PI;
    if (fabs(car_pose_x - last_result.cx) > 0.035 || fabs(car_pose_theta_deg - last_result.theta) > 100) {
        char valid_info[255];
        sprintf(valid_info,
                "validation failed for x and theta, carpose: (%.3f, %.3f), result: (%.3f, %.3f), deta: (%.3f, %.3f)",
                car_pose_x, car_pose_theta_deg, last_result.cx, last_result.theta, car_pose_x - last_result.cx,
                car_pose_theta_deg - last_result.theta);
        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, valid_info);
    }

    // 车宽精度优化
    {
        double rough_width, rough_length, extract_width, extract_length;
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_width_rough_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
                new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_width_extract_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
                new pcl::PointCloud<pcl::PointXYZ>);
        t_width_rough_cloud->operator+=(*mp_cloud_filtered);
        t_width_extract_cloud->operator+=(m_detector->m_left_front_cloud);
        t_width_extract_cloud->operator+=(m_detector->m_right_front_cloud);
        t_width_extract_cloud->operator+=(m_detector->m_left_rear_cloud);
        t_width_extract_cloud->operator+=(m_detector->m_right_rear_cloud);

        pcl::PointXYZ t_rough_min_p, t_rough_max_p;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for_width_upwheel(new pcl::PointCloud<pcl::PointXYZ>);
        for (int i = 0; i < t_width_rough_cloud->size(); ++i) {
            pcl::PointXYZ pt = t_width_rough_cloud->points[i];
            if (pt.z > chassis_z) {
                cloud_for_width_upwheel->push_back(pt);
            }
        }

        // 获取车身模糊长宽
        {
            //离群点过滤
            sor.setInputCloud(cloud_for_width_upwheel);
            sor.setMeanK(5);                               //K近邻搜索点个数
            sor.setStddevMulThresh(1.0);            //标准差倍数
            sor.setNegative(false);                             //保留未滤波点（内点）
            sor.filter(*cloud_for_width_upwheel);            //保存滤波结果到cloud_filter

            Eigen::Affine3d t_affine = Eigen::Affine3d::Identity();
            t_affine.prerotate(Eigen::AngleAxisd((90 - last_result.theta) * M_PI / 180.0, Eigen::Vector3d::UnitZ()));
            pcl::transformPointCloud(*cloud_for_width_upwheel, *cloud_for_width_upwheel, t_affine.matrix());

            // 车宽重计算，并赋值到当前车宽
            pcl::getMinMax3D(*cloud_for_width_upwheel, t_rough_min_p, t_rough_max_p);
            rough_width = t_rough_max_p.x - t_rough_min_p.x;
            rough_length = t_rough_max_p.y - t_rough_min_p.y;
        }

        // 获取车轮精确长宽
        {
            //离群点过滤
            sor.setInputCloud(t_width_extract_cloud);
            sor.setMeanK(5);                           //K近邻搜索点个数
            sor.setStddevMulThresh(1.0);        //标准差倍数
            sor.setNegative(false);                         //保留未滤波点（内点）
            sor.filter(*t_width_extract_cloud);          //保存滤波结果到cloud_filter

            Eigen::Affine3d t_affine = Eigen::Affine3d::Identity();
            t_affine.prerotate(Eigen::AngleAxisd((90 - last_result.theta) * M_PI / 180.0, Eigen::Vector3d::UnitZ()));
            pcl::transformPointCloud(*t_width_extract_cloud, *t_width_extract_cloud, t_affine.matrix());

            // 车宽重计算，并赋值到当前车宽
            pcl::PointXYZ t_min_p, t_max_p;
            pcl::getMinMax3D(*t_width_extract_cloud, t_min_p, t_max_p);

            extract_width = t_max_p.x - t_min_p.x;
            extract_length = t_max_p.y - t_min_p.y;
        }

        last_result.wheel_width = extract_width;
        last_result.width = rough_width;
        last_result.length = rough_length;

        if (fabs(rough_width - extract_width) > 0.15) {
            std::string disp_str = std::string("rough data and extract difference is too large: ") +
                    "(" + std::to_string(rough_width) + ", " + std::to_string(rough_length) + "), " +
                    "(" + std::to_string(extract_width) + ", " + std::to_string(extract_length) + ")";
            LOG(WARNING) << disp_str;
            return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, MINOR_ERROR, disp_str.c_str());
        }
    }

    return SUCCESS;
}

int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double cx, double cy, double plate_cx,
                                       double plate_cy, double theta) {
    // LOG(WARNING) << m_range_status_last;
    if (cloud->size() <= 0)
        return Range_status::Range_correct;
    int res = Range_status::Range_correct;
    // 计算转盘旋转后点云坐标
    double theta_rad = theta * M_PI / 180.0;
    pcl::PointCloud<pcl::PointXYZ> t_cloud;
    for (size_t i = 0; i < cloud->size(); i++) {
        Eigen::Vector2d t_point(cloud->points[i].x - plate_cx, cloud->points[i].y - plate_cy);
        pcl::PointXYZ t_pcl_point;
        t_pcl_point.x = (t_point.x() * cos(theta_rad) - t_point.y() * sin(theta_rad)) + plate_cx;
        t_pcl_point.y = (t_point.x() * sin(theta_rad) + t_point.y() * cos(theta_rad)) + plate_cy;
        t_pcl_point.z = cloud->points[i].z;
        t_cloud.push_back(t_pcl_point);
    }
    pcl::PointXYZ min_p, max_p;
    pcl::getMinMax3D(t_cloud, min_p, max_p);

#ifndef ANTI_SHAKE
    // 判断左右超界情况
    if(min_p.x < m_region.border_minx())
        res |= Range_status::Range_left;
    if(max_p.x > m_region.border_maxx())
        res |= Range_status::Range_right;
    // LOG_IF(WARNING, m_region.region_id() == 4)<< "border minmax x: "<< min_p.x<<", "<<max_p.x<<", res: "<<res;
    return res;
#else
    //huli 不再使用汽车左右边界来判断，直接使用中心点x值判断左右超界。
    //plc坐标，1 3 5入口x为-1.8左右， 2 4 6入口为1.8左右
    //要求x取值范围，1650～2150左右
    float t_center_x = cx + m_region.plc_offsetx();
    if (t_center_x < m_region.border_minx() - 0.000) {
        m_range_status_last |= Range_status::Range_left;
    } else if (t_center_x > m_region.border_minx() + 0.010) {
        int t = Range_status::Range_left;
        m_range_status_last &= (~t);
    }
    //else      m_range_status_last no change

    if (t_center_x > m_region.border_maxx() + 0.000) {
        m_range_status_last |= Range_status::Range_right;
    } else if (t_center_x < m_region.border_maxx() - 0.010) {
        int t = Range_status::Range_right;
        m_range_status_last &= (~t);
    }

    return m_range_status_last;
#endif //ANTI_SHAKE
}

int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                       Common_data::Car_wheel_information measure_result, double plate_cx,
                                       double plate_cy, double theta) {
    // int res = Range_status::Range_correct;
    if (cloud->size() <= 0)
        return Range_status::Range_correct;

    // 计算转盘旋转后车辆中心点坐标
    Eigen::Vector2d car_center(measure_result.car_center_x, measure_result.car_center_y);
    Eigen::Vector2d car_uniform_center;
    double theta_rad = theta * M_PI / 180.0;
    car_uniform_center.x() = car_center.x() * cos(theta_rad) - car_center.y() * sin(theta_rad);
    car_uniform_center.y() = car_center.x() * sin(theta_rad) + car_center.y() * cos(theta_rad);

#ifndef ANTI_SHAKE
    // 车位位于y负方向，判断后轮超界情况
    //yct old program 20221101
//    double rear_wheel_y = car_uniform_center.y() + m_region.plc_offsety() - 0.5 * measure_result.car_wheel_base;
//    if(rear_wheel_y < m_region.plc_border_miny())
//    {
//        res |= Range_status::Range_back;
//    }
    //huli new program 20221101
    double t_center_y = 0.0 - (car_uniform_center.y() + m_region.plc_offsety()); // 汽车中心y， 参照plc坐标，并且进行偏移， 绝对值。结果大约为 5～6左右
    if( t_center_y - 4.88 + 0.5*(measure_result.car_wheel_base - 2.7) < m_region.plc_border_maxy()    &&
            t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) < m_region.plc_border_miny() )
    {
        //normal
    } else
    {
        res |= Range_status::Range_back;
    }



    // 判断车辆宽度超界情况
    if (measure_result.car_wheel_width < m_region.car_min_width() || measure_result.car_wheel_width > m_region.car_max_width())
    {
        res |= Range_status::Range_car_width;
    }
    // 判断车辆轴距超界情况
    if (measure_result.car_wheel_base < m_region.car_min_wheelbase() || measure_result.car_wheel_base > m_region.car_max_wheelbase())
    {
        res |= Range_status::Range_car_wheelbase;
    }


    //yct old program 20221103 , 使用 velodyne_manager.prototxt 里面的 turnplate_angle_limit 来做边界判断
    // 判断车辆旋转角超界情况
//    double dtheta = 90-measure_result.car_angle;
//    if (dtheta > m_region.turnplate_angle_limit_anti_clockwise())
//    {
//        res |= Range_status::Range_angle_clock;
//    }
//    if (dtheta < -m_region.turnplate_angle_limit_clockwise())
//    {
//        res |= Range_status::Range_angle_anti_clock;
//    }
    //huli new program 20221103, 使用 plc  dispatch_1_statu_port 里面的  来做边界判断
//    double t_dtheta = 90-measure_result.car_angle;    // 车身的旋转角， 修正到正负5度，
    double t_dtheta = measure_result.car_angle + m_region.plc_offset_degree();    // 车身的旋转角， 修正到正5度， (m_region.plc_offset_degree():-89)
    int t_terminal_id = m_region.region_id() +1;        //region_id:0~5,   terminal_id:1~6
    //turnplate_angle_min = -5, turnplate_angle_max = 5,
    if (t_dtheta > System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max())
    {
        res |= Range_status::Range_angle_anti_clock;
    }
    if (t_dtheta < System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min())
    {
        res |= Range_status::Range_angle_clock;
    }

    // // 判断车辆前轮角回正情况
    // if (fabs(measure_result.car_front_theta) > 8.0)
    // {
    //     res |= Range_status::Range_steering_wheel_nozero;
    // }
    res |= outOfRangeDetection(cloud, plate_cx, plate_cy, theta);
    return res;
#else
    // 车位位于y负方向，判断后轮超界情况
    double t_center_y =
            0.0 - (car_uniform_center.y() + m_region.plc_offsety()); // 汽车中心y， 参照plc坐标，并且进行偏移， 绝对值。结果大约为 5～6左右
    // 4.80是搬运器大Y的运动极限, 2.7是同侧两个小y夹持杆中心距离
    DLOG(INFO) << t_center_y - 4.80 + 0.5 * (measure_result.car_wheel_base - 2.7) << " " << m_region.plc_border_maxy();
    DLOG(INFO) << t_center_y - 4.80 - 0.5 * (measure_result.car_wheel_base - 2.7) << " " << m_region.plc_border_miny();
    if (t_center_y - 4.80 + 0.5 * (measure_result.car_wheel_base - 2.7) < m_region.plc_border_maxy() - 0.000 &&
        t_center_y - 4.80 - 0.5 * (measure_result.car_wheel_base - 2.7) < m_region.plc_border_miny() - 0.000) {
        // y未超界
        int t = Range_status::Range_back;
        m_range_status_last &= (~t);
    } else if (t_center_y - 4.80 + 0.5 * (measure_result.car_wheel_base - 2.7) > m_region.plc_border_maxy() + 0.010 ||
               t_center_y - 4.80 - 0.5 * (measure_result.car_wheel_base - 2.7) > m_region.plc_border_miny() + 0.010) {
        // y超界
        m_range_status_last |= Range_status::Range_back;
    }

    // 判断车辆宽度超界情况
    if (measure_result.car_wheel_width < m_region.car_min_width() - 0.000 ||
        measure_result.car_wheel_width > m_region.car_max_width() + 0.000) {
        m_range_status_last |= Range_status::Range_car_width;
    } else if (measure_result.car_wheel_width > m_region.car_min_width() + 0.010 &&
               measure_result.car_wheel_width < m_region.car_max_width() - 0.010) {
        int t = Range_status::Range_car_width;
        m_range_status_last &= (~t);
    }

    // 判断车辆轴距超界情况
    if (measure_result.car_wheel_base < m_region.car_min_wheelbase() - 0.000 ||
        measure_result.car_wheel_base > m_region.car_max_wheelbase() + 0.000) {
        m_range_status_last |= Range_status::Range_car_wheelbase;
    } else if (measure_result.car_wheel_base > m_region.car_min_wheelbase() + 0.010 ||
               measure_result.car_wheel_base < m_region.car_max_wheelbase() - 0.010) {
        int t = Range_status::Range_car_wheelbase;
        m_range_status_last &= (~t);
    }

    // 判断车辆旋转角超界情况
    // huli new program 20221103, 使用 plc  dispatch_1_statu_port 里面的  来做边界判断
    // double t_dtheta = 90-measure_result.car_angle;    // 车身的旋转角， 修正到正负5度，
    double t_dtheta = measure_result.car_angle + m_region.plc_offset_degree();
    int t_terminal_id = m_region.region_id();
    DLOG(INFO) << t_dtheta;
    DLOG(INFO) << "id: " << t_terminal_id << ", max: "
               << System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max();
    if (t_dtheta >
        System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max() +
        0.00) {
        m_range_status_last |= Range_status::Range_angle_anti_clock;
    } else if (t_dtheta <
               System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max() -
               0.10) {
        int t = Range_status::Range_angle_anti_clock;
        m_range_status_last &= (~t);
    }

    if (t_dtheta <
        System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min() -
        0.00) {
        m_range_status_last |= Range_status::Range_angle_clock;
    } else if (t_dtheta >
               System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min() +
               0.10) {
        int t = Range_status::Range_angle_clock;
        m_range_status_last &= (~t);
    }

    m_range_status_last |= outOfRangeDetection(cloud, measure_result.car_center_x, measure_result.car_center_y,
                                               plate_cx, plate_cy, theta);
    return m_range_status_last;
#endif
}

//外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待
Error_manager Ground_region::get_current_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information,
                                                           std::chrono::system_clock::time_point command_time) {
    if (p_car_wheel_information == NULL) {
        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                             "  POINTER IS NULL ");
    }
    //获取指令时间之后的信息, 如果没有就会报错, 不会等待
    if (m_detect_update_time > command_time) {
        std::lock_guard<std::mutex> lck(m_car_result_mutex);
        *p_car_wheel_information = m_car_wheel_information;
        if (m_car_wheel_information.correctness)
            return Error_code::SUCCESS;
        else
            return Error_manager(Error_code::VELODYNE_REGION_CERES_SOLVE_ERROR, Error_level::MINOR_ERROR,
                                 " Ground_region detect error");
    } else {
        return Error_manager(Error_code::VELODYNE_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR,
                             " Ground_region::get_current_wheel_information error ");
    }
}

//外部调用获取最新的车轮定位信息, 获取指令时间往前一个周期内的车轮定位信息, 如果没有就会报错, 不会等待
Error_manager Ground_region::get_last_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information,
                                                        std::chrono::system_clock::time_point command_time) {
    if (p_car_wheel_information == nullptr) {
        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                             "  POINTER IS NULL ");
    }
    //获取指令时间之后的信息, 如果没有就会报错, 不会等待
//    LOG(WARNING) << std::chrono::duration_cast<std::chrono::milliseconds>(command_time-m_detect_update_time).count()/1000.0 <<", "
//                <<std::chrono::duration_cast<std::chrono::milliseconds>(command_time-m_cloud_collection_time).count()/1000.0;
    if (m_detect_update_time > command_time - std::chrono::milliseconds(GROUND_REGION_DETECT_CYCLE_MS)) {
        std::lock_guard<std::mutex> lck(m_car_result_mutex);
        *p_car_wheel_information = m_car_wheel_information;
        if (m_car_wheel_information.correctness)
            return Error_code::SUCCESS;
        else
            return Error_manager(Error_code::VELODYNE_REGION_CERES_SOLVE_ERROR, Error_level::MINOR_ERROR,
                                 " Ground_region detect error");
    } else {
        return Error_manager(Error_code::VELODYNE_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR,
                             " Ground_region::get_current_wheel_information error ");
    }
}

Error_manager Ground_region::update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    // 限定锁区域，解决因条件变量在析构时无法获得内部mutex导致此互斥锁卡住，造成死锁无法结束程序的问题。
    {
        std::lock_guard<std::mutex> lck(m_cloud_collection_mutex);
        mp_cloud_collection = cloud;
        DLOG(WARNING) << "update region cloud size: " << mp_cloud_collection->size() << ",,, input size: "
                      << cloud->size();
        m_cloud_collection_time = std::chrono::system_clock::now();
    }
    m_measure_condition.notify_one(false, true);

    return SUCCESS;
}

void Ground_region::thread_measure_func() {
    LOG(INFO) << " Ground_region::thread_measure_func() start " << this;
    while (m_measure_condition.is_alive()) {
        m_measure_condition.wait();
        if (m_measure_condition.is_alive()) {
            m_region_status = E_BUSY;
            detect_wheel_ceres3d::Detect_result t_result;

            Error_manager ec = detect(mp_cloud_collection, t_result);
            LOG(WARNING) << t_result.to_string();
            if (ec != SUCCESS) {
                LOG(ERROR) << ec.to_string();
            } else {
                DLOG(WARNING) << ec.to_string();
            }
            std::lock_guard<std::mutex> lck(m_car_result_mutex);
            m_detect_update_time = std::chrono::system_clock::now();
            // 增加滤波轴距
            Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
            // 车辆移动检测
            Common_data::Car_wheel_information_stamped t_wheel_info_stamped_for_car_move;
            DLOG(INFO) << "m_car_wheel_information.correctness: " << m_car_wheel_information.correctness;
            if (ec == SUCCESS) {
                m_car_wheel_information.correctness = true;
                m_car_wheel_information.car_center_x = t_result.cx;
                m_car_wheel_information.car_center_y = t_result.cy;
                m_car_wheel_information.car_angle = t_result.theta;
                m_car_wheel_information.car_wheel_base = t_result.wheel_base;
                m_car_wheel_information.car_wheel_width = t_result.wheel_width;
                m_car_wheel_information.car_width = t_result.width;
                m_car_wheel_information.car_length = t_result.length;
                m_car_wheel_information.car_front_theta = t_result.front_theta;
                // 220110 added by yct, single wheel width filtering
                m_car_wheel_information.single_wheel_width = t_result.single_wheel_width;
                m_car_wheel_information.single_wheel_length = t_result.single_wheel_length;

                m_car_wheel_information.theta_uniform(m_region.turnplate_cx(), m_region.turnplate_cy());
                // 超界校验
                {
                    std::lock_guard<std::mutex> lck(m_filtered_cloud_mutex);
                    int res = outOfRangeDetection(mp_cloud_filtered, m_car_wheel_information, m_region.turnplate_cx(),
                                                  m_region.turnplate_cy(), 90.0 - t_result.theta);
                    m_car_wheel_information.range_status = res;
                    // LOG_IF(INFO, m_region.region_id() == 4) << res;
                }
                // 添加plc偏移
                m_car_wheel_information.car_center_x += m_region.plc_offsetx();
                m_car_wheel_information.car_center_y += m_region.plc_offsety();
                m_car_wheel_information.uniform_car_x += m_region.plc_offsetx();
                m_car_wheel_information.uniform_car_y += m_region.plc_offsety();
                m_car_wheel_information.car_angle += m_region.plc_offset_degree();

                t_wheel_info_stamped.wheel_data = m_car_wheel_information;
                t_wheel_info_stamped.measure_time = m_detect_update_time;
                Measure_filter::get_instance_references().update_data(m_region.region_id(), t_wheel_info_stamped);

                // 20211228 added by yct, car movement checking, human and door detection
                t_wheel_info_stamped_for_car_move = t_wheel_info_stamped;
                t_wheel_info_stamped_for_car_move.wheel_data.car_center_x -= m_region.plc_offsetx();
                t_wheel_info_stamped_for_car_move.wheel_data.car_center_y -= m_region.plc_offsety();
                t_wheel_info_stamped_for_car_move.wheel_data.uniform_car_x -= m_region.plc_offsetx();
                t_wheel_info_stamped_for_car_move.wheel_data.uniform_car_y -= m_region.plc_offsety();
                t_wheel_info_stamped_for_car_move.wheel_data.car_angle -= m_region.plc_offset_degree();
                Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(
                        m_region.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_filtered);
                // success means car stable
                if (car_status_res == SUCCESS) {
                    m_car_wheel_information.range_status &= ~((int) Range_status::Range_car_moving);
                } else {
                    m_car_wheel_information.range_status |= Range_status::Range_car_moving;
                }
                Region_status_checker::get_instance_references().add_measure_data(m_region.region_id(),
                                                                                  t_wheel_info_stamped_for_car_move,
                                                                                  *mp_cloud_filtered);

                ec = Measure_filter::get_instance_references().get_filtered_wheel_information(m_region.region_id(),
                                                                                              t_wheel_info_stamped.wheel_data);
                if (ec == SUCCESS) {
                    m_car_wheel_information.car_wheel_base = t_wheel_info_stamped.wheel_data.car_wheel_base;
                    m_car_wheel_information.car_front_theta = t_wheel_info_stamped.wheel_data.car_front_theta;
                    // 临时添加，滤波后前轮超界
                    if (fabs(m_car_wheel_information.car_front_theta) > 8.0) {
                        m_car_wheel_information.range_status |= Range_status::Range_steering_wheel_nozero;
                    }
                }
            }
            else if (ec.get_error_code() == VELODYNE_REGION_ANGLE_PRECHECK_FAILED) {
                m_car_wheel_information.correctness = false;
                m_car_wheel_information.car_center_x = t_result.cx;
                m_car_wheel_information.car_center_y = t_result.cy;
                m_car_wheel_information.car_angle = t_result.theta;
                m_car_wheel_information.car_wheel_base = 0.0;
                m_car_wheel_information.car_wheel_width = 0.0;
                m_car_wheel_information.car_front_theta = 0.0;
                // 220110 added by yct, single wheel width filtering
                m_car_wheel_information.single_wheel_width = 0.0;
                m_car_wheel_information.single_wheel_length = 0.0;
                m_car_wheel_information.car_width = 0;
                m_car_wheel_information.car_length = 0;

                // 添加plc偏移
                m_car_wheel_information.car_center_x += m_region.plc_offsetx();
                m_car_wheel_information.car_center_y += m_region.plc_offsety();
                m_car_wheel_information.uniform_car_x += m_region.plc_offsetx();
                m_car_wheel_information.uniform_car_y += m_region.plc_offsety();

                m_car_wheel_information.theta_uniform(m_region.turnplate_cx(), m_region.turnplate_cy());
                m_car_wheel_information.range_status |=
                        m_car_wheel_information.car_angle < 0 ? Range_status::Range_angle_clock
                                                              : Range_status::Range_angle_anti_clock;
            } else {
                m_car_wheel_information.correctness = false;

                // 20211228 added by yct, car movement checking, human and door detection
                Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(
                        m_region.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_filtered);
                // success means car stable
                if (car_status_res == SUCCESS) {
                    m_car_wheel_information.range_status &= ~((int) Range_status::Range_car_moving);
                } else {
                    m_car_wheel_information.range_status |= Range_status::Range_car_moving;
                }
            }

            // On June 24, 2023, at the request of huli, LiuZe added the following code for the class.
            // 连续静止3秒, 发出静止状态
            if (!(m_car_wheel_information.range_status & Range_status::Range_car_moving)) {
                std::chrono::duration<double> pass_time = std::chrono::steady_clock::now() - m_last_move_time;
                if (pass_time.count() > 3) {
                    m_car_wheel_information.motion_statu = 1;
                }
            } else {
                m_last_move_time = std::chrono::steady_clock::now();
                m_car_wheel_information.motion_statu = 0;
            }

            DLOG(INFO) << m_car_wheel_information.motion_statu;
            // On June 24, 2023, LiuZe added the following code for the class.
            // 存储三秒内的车轮信息, 后面用于计算数据,防止出现较大的误差误.
            m_car_wheel_information_map.save(m_car_wheel_information);
        }
        m_region_status = E_READY;
    }
    LOG(INFO) << " Ground_region::thread_measure_func() end " << this;
}
