/**
  * @project ALG_WHeelData
  * @brief   TODO: add a brief
  * @author  lz
  * @data    2023/4/21
 **/
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/common/transforms.h>
#include <pcl/segmentation/extract_clusters.h>
#include "detect_wheel.h"

DetectWheel::DetectWheel() {
    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_detector = new WheelCeres3D();
}

Error_manager DetectWheel::init(const Region &region) {
    std::map<std::string, float> m_modules;
    m_modules.insert(std::pair<std::string, float>("../etc/model/left_model.txt", 1.0f));
    m_modules.insert(std::pair<std::string, float>("../etc/model/right_model.txt", 1.0f));
    m_detector->init(m_modules);
    m_region = region;
    LOG(INFO) << "\n" << m_region.DebugString();
    return Error_code::SUCCESS;
}

Error_manager DetectWheel::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
    auto t = std::chrono::steady_clock::now();
    std::chrono::duration<double> diff{};

    // 1.*********点云合法性检验*********
    if (cloud->empty()) {
        return Error_code::VELODYNE_REGION_EMPTY_CLOUD;
    }
#if RECORD_DETECT_CLOUD_DATA
    WriteTxtCloud("../etc/test/cloud.txt", cloud);
#endif

    // 2.*********点云预处理*********
    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>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_detect_z(new pcl::PointCloud<pcl::PointXYZ>);

    // 过滤大部分无效点
    if (!cutFilter(cloud, cloud_cut)) {
        LOG(WARNING) << "cloud_cut cloud falture.";
        return Error_code::FAILED;
    }

#if RECORD_DETECT_CLOUD_DATA
    WriteTxtCloud("../etc/test/cloud_cut.txt", cloud_cut);
#endif

    // 下采样
    if (!voxeFilter(cloud_cut, cloud_clean, pcl::PointXYZ(0.02, 0.02, 0.02))) {
        LOG(WARNING) << "cloud_clean cloud falture.";
        return Error_code::FAILED;
    }
#if RECORD_DETECT_CLOUD_DATA
    WriteTxtCloud("../etc/test/cloud_clean.txt", cloud_clean);
#endif

    // 离群点过滤
    if (!outlierFilter(cloud_clean, cloud_filtered)) {
        LOG(WARNING) << "cloud_filtered cloud falture.";
        return Error_code::FAILED;
    }
    if (cloud_filtered->size() <= 10) {
        LOG(WARNING) << "cloud_filtered cloud number too small.";
        return Error_code::FAILED;
    }
#if RECORD_DETECT_CLOUD_DATA
    WriteTxtCloud("../etc/test/cloud_filtered.txt", cloud_filtered);
#endif

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

    // 3.*********位姿优化，获取中心xy与角度*********
    double car_pose_x, car_pose_y, car_pose_theta;
    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)) {
        LOG(WARNING) << "detect pose mat falture.";
        return Error_code::FAILED;
    }

    if (fabs(car_pose_theta * 180.0 / M_PI) > 8) {
        LOG(WARNING) << "Pose theta > 8, update detect result and return false.";
        m_detect_result.set_cx((float) car_pose_x);
        m_detect_result.set_cy((float) car_pose_y);
        m_detect_result.set_theta((float) (-car_pose_theta * 180.0f / M_PI));
        m_t = std::chrono::steady_clock::now();
        return Error_code::FAILED;
    }

    // 4.*********xoz优化获取底盘高度*********
    // 重新取包含地面点的点云,用于底盘优化
    double z_solver_x = car_pose_x;
    double z_solver_y = car_pose_y;
    double z_solver_theta = car_pose_theta;
    chassis_ceres_solver t_solver;
    // ！！！重新筛选地面点并滤波，融入第一步位姿使用的滤波后点云，传入第二步获取底盘高度
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_z_solver = cloud_cut;

    //离群点过滤
    if (!outlierFilter(cloud_z_solver, cloud_z_solver)) {
        LOG(WARNING) << "outlierFilter falture.";
        return Error_code::FAILED;
    }
#if RECORD_DETECT_CLOUD_DATA
    WriteTxtCloud("../etc/test/cloud_z_solver.txt", cloud_z_solver);
#endif

    cloud_z_solver->operator+=(*mp_cloud_filtered);
#if RECORD_DETECT_CLOUD_DATA
    WriteTxtCloud("../etc/test/cloud_z_solver2.txt", cloud_z_solver);
#endif

    // 去中心，角度调正
    Car_pose_detector::get_instance_references().inv_trans_cloud(cloud_z_solver, z_solver_x, z_solver_y,
                                                                 z_solver_theta);
    if (cloud_z_solver->empty()) {
        LOG(WARNING) << "inv trans cloud falture.";
        return Error_code::FAILED;
    }
#if RECORD_DETECT_CLOUD_DATA
    WriteTxtCloud("../etc/test/cloud_z_solver3.txt", cloud_z_solver);
#endif

    double mid_z = 0.24 + m_region.cut_region().minz();
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud_z_solver);
    pass.setFilterFieldName("z");
    pass.setFilterLimits((float) m_region.cut_region().minz(), (float) mid_z);
    pass.setFilterLimitsNegative(false);
    pass.filter(*cloud_z_solver);
#if RECORD_DETECT_CLOUD_DATA
    WriteTxtCloud("../etc/test/cloud_z_solver4.txt", cloud_z_solver);
#endif

    mp_cloud_detect_z->clear();
    mp_cloud_detect_z->operator+=(*cloud_filtered);

    std::vector<DetectResult> results;
    DetectResult result;
    double chassis_z = mid_z;
    if (chassis_z > m_region.cut_region().maxz() || chassis_z < m_region.cut_region().minz()) {
        LOG(WARNING) << "chassis_z valure error.";
        return Error_code::FAILED;
    }
    bool ret;

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

    ///  to be
    float min_mean_loss = 1.0;
    for (int i = 0; i < results.size(); ++i) {
        DetectResult result_loss = results[i];
        std::vector<double> loss;
        loss.push_back(result_loss.loss().lf_loss());
        loss.push_back(result_loss.loss().rf_loss());
        loss.push_back(result_loss.loss().lb_loss());
        loss.push_back(result_loss.loss().rb_loss());
        double mean =
                (result_loss.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) {
            m_detect_result = result_loss;
            min_mean_loss = (float) mean;
            m_t = std::chrono::steady_clock::now();
        }
    }

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

    // 车宽精度优化
    {
        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;
        //add by zzw
        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);
            }
        }

        // rough width for human detect
        {
            //离群点过滤
            if (!outlierFilter(cloud_for_width_upwheel, cloud_for_width_upwheel)) {
                LOG(WARNING) << "outlierFilter error.";
                return Error_code::FAILED;
            }

            Eigen::Affine3d t_affine = Eigen::Affine3d::Identity();
            t_affine.prerotate(
                    Eigen::AngleAxisd((90 - m_detect_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);
        }
        double rough_width = t_rough_max_p.x - t_rough_min_p.x;

        //离群点过滤
        if (!outlierFilter(t_width_extract_cloud, t_width_extract_cloud)) {
            LOG(WARNING) << "outlierFilter error.";
            return Error_code::FAILED;
        }

        Eigen::Affine3d t_affine = Eigen::Affine3d::Identity();
        t_affine.prerotate(Eigen::AngleAxisd((90 - m_detect_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);

        double accurate_width = t_max_p.x - t_min_p.x;
        m_detect_result.set_width(accurate_width);
        m_t = std::chrono::steady_clock::now();

        if (fabs(rough_width - accurate_width) > 0.15) {
            std::string disp_str =
                    std::string("width difference too large, assume noise situation, ") + std::to_string(rough_width) +
                    ", " + std::to_string(accurate_width);
            LOG(WARNING) << disp_str;
            return Error_code::FAILED;
        }
    }

//    diff = std::chrono::steady_clock::now() - t;
//    LOG(INFO) << "cost time : " << diff.count() * 1000 << "ms.";

    return Error_code::SUCCESS;
}

Error_manager DetectWheel::getDetectResult(DetectResult &result) {
    std::chrono::duration<double> t = std::chrono::steady_clock::now() - m_t;

    if (t.count() * 1000 > 1000) {
        return Error_code::FAILED;
    }

    result = m_detect_result;

    return Error_code::SUCCESS;
}

int DetectWheel::borderStatus() {
    return m_border_status;
}

int DetectWheel::groundStatus() {
    return m_ground_status;
}

bool
DetectWheel::cutFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud) {
    for (auto &pt: cloud->points) {
        if (pt.x > m_region.cut_region().minx() && pt.x < m_region.cut_region().maxx() && pt.y > m_region.cut_region().miny() && pt.y < m_region.cut_region().maxy() &&
            pt.z > m_region.cut_region().minz() && pt.z < m_region.cut_region().maxz()) {
            out_cloud->push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
        }
    }
    if (out_cloud->empty()) {
        DLOG(WARNING) << "Filter all points, please check region or cloud data.";
        return false;
    }
    return true;
}

bool DetectWheel::voxeFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
                             pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud, pcl::PointXYZ leaf) {
    pcl::VoxelGrid<pcl::PointXYZ> vox;             //创建滤波对象
    vox.setInputCloud(input_cloud);          //设置需要过滤的点云给滤波对象
    vox.setLeafSize((float) leaf.x, (float) leaf.y, (float) leaf.z); //设置滤波时创建的体素体积为1cm的立方体
    vox.filter(*out_cloud);                     //执行滤波处理，存储输出
    if (out_cloud->empty()) {
        DLOG(WARNING) << "Voxe Filter all points, please check LeafSize or cloud data.";
        return false;
    }
    return true;
}

bool DetectWheel::outlierFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
                                pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud) {
    //离群点过滤
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(input_cloud);
    sor.setMeanK(5);                           //K近邻搜索点个数
    sor.setStddevMulThresh(3.0);         //标准差倍数
    sor.setNegative(false);                        //保留未滤波点（内点）
    sor.filter(*out_cloud);                     //保存滤波结果到cloud_filter
    if (out_cloud->empty()) {
        DLOG(WARNING) << "Outlier filter all points, please check K、Stddev、Negative or cloud data.";
        return false;
    }
    return true;
}

//欧式聚类*******************************************************
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
DetectWheel::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud) {
    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(0.07);
    ece.setMinClusterSize(20);
    ece.setMaxClusterSize(20000);
    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;
}

bool DetectWheel::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
                                        DetectResult &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) {
        if (cloud->points[i].z < thresh_z) {
            cloud_filtered->push_back(cloud->points[i]);
        }
    }
    static int i = 0;
    //下采样
    if (!voxeFilter(cloud_filtered, cloud_filtered, pcl::PointXYZ(0.01, 0.01, 0.01))) {
        return false;
    }
#if RECORD_DETECT_CLOUD_DATA
    WriteTxtCloud("../etc/test/classify_voxe_cloud_filtered_" + std::to_string(i) + ".txt", cloud_filtered);
#endif

    // 离群滤波
    if (!outlierFilter(cloud_filtered, cloud_filtered)) {
        return false;
    }
#if RECORD_DETECT_CLOUD_DATA
    WriteTxtCloud("../etc/test/classify_outlier_cloud_filtered_" + std::to_string(i) + ".txt", cloud_filtered);
#endif

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

    if (!(seg_clouds.size() == 4 || seg_clouds.size() == 3)) {
        DLOG(INFO) << seg_clouds.size();
        return false;
    }
    std::vector<cv::Point2f> centers;
    pcl::PointCloud<pcl::PointXYZ>::Ptr record(new pcl::PointCloud<pcl::PointXYZ>);
    cv::Point2f temp_centers(0, 0);
    for (const auto &i: seg_clouds) {
        record->operator+=(*i);
    }
    for (int i = 0; i < seg_clouds.size(); ++i) {
        Eigen::Vector4f centroid;
        pcl::compute3DCentroid(*seg_clouds[i], centroid);
        centers.push_back(cv::Point2f(centroid[0], centroid[1]));
        temp_centers += cv::Point2f(centroid[0], centroid[1]);
    }
#if RECORD_DETECT_CLOUD_DATA
    WriteTxtCloud("../etc/test/segmentation_cloud_filtered_" + std::to_string(i) + ".txt", record);
#endif
    temp_centers /= 4.0f;
    bool ret = isRect(centers);
    DLOG(INFO) << "isRect ret :" << ret;
    if (ret) {
        std::string error_str;
        ret = m_detector->detect(seg_clouds, result, error_str);
#if RECORD_DETECT_CLOUD_DATA
        record->clear();
        record->operator+=(m_detector->m_left_front_transformed_cloud);
        WriteTxtCloud("../etc/test/m_left_front_transformed_cloud" + std::to_string(i) + ".txt", record);
        record->clear();
        record->operator+=(m_detector->m_right_front_transformed_cloud);
        WriteTxtCloud("../etc/test/m_right_front_transformed_cloud" + std::to_string(i) + ".txt", record);
        record->clear();
        record->operator+=(m_detector->m_left_rear_transformed_cloud);
        WriteTxtCloud("../etc/test/m_left_rear_transformed_cloud" + std::to_string(i) + ".txt", record);
        record->clear();
        record->operator+=(m_detector->m_right_rear_transformed_cloud);
        WriteTxtCloud("../etc/test/m_right_rear_transformed_cloud" + std::to_string(i) + ".txt", record);
#endif
        if (!ret) {
            LOG(INFO) << "error_str :" << error_str;
        }
        return ret;
    }
    return ret;
}

bool DetectWheel::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))) {
            return false;
        }

        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.15) {
            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) {
            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) {
            return false;
        }

        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))) {
            return false;
        }

        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
        if (fabs(cosa) >= 0.15) {
            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);
            return true;
        } else {
            return false;
        }
    }

    return false;
}

Error_manager DetectWheel::upStatus(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
    upBorderStatus();
    if (!m_detect_result.success())
    {
        if (cloud->empty())
            m_ground_status = Ground_statu::Nothing;
        else
            m_ground_status = Ground_statu::Noise;
    }
    else if (m_border_status == int(Range_status::Range_correct))
    {
        m_ground_status = Ground_statu::ok;
    }
    else {
        m_ground_status = Ground_statu::Car_border_reached;
    }
    return Error_code::SUCCESS;
}

Error_manager DetectWheel::upBorderStatus() {
    m_border_status = Range_status::Range_correct;
    isRangeXY();
    isRangeCarWidth();
    isRangeCarWheelbase();
    isAngleClock();

    return Error_code::SUCCESS;
}

Error_manager DetectWheel::isRangeXY() {
    Eigen::Vector2d car_center(m_detect_result.cx(), m_detect_result.cy());
    Eigen::Vector2d car_uniform_center;
    double theta_rad = m_detect_result.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);

    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*(m_detect_result.wheel_base() - 2.7) < m_region.plc_border_maxy()-0.000    &&
        t_center_y - 4.88 - 0.5*(m_detect_result.wheel_base() - 2.7) < m_region.plc_border_miny()-0.000 )
    {
        // y未超界
        m_border_status &= (~Range_status::Range_back);
    }
    else if( t_center_y - 4.88 + 0.5*(m_detect_result.wheel_base() - 2.7) > m_region.plc_border_maxy()+0.010    ||
             t_center_y - 4.88 - 0.5*(m_detect_result.wheel_base() - 2.7) > m_region.plc_border_miny()+0.010 )
    {
        // y超界
        m_border_status |= (Range_status::Range_back);
    }

    float t_center_x = car_uniform_center.y() + m_region.plc_offsetx();
    if(t_center_x < m_region.border_minx()-0.000)
    {
        m_border_status |= Range_status::Range_left;
    }
    else if(t_center_x > m_region.border_minx()+0.010)
    {
        m_border_status &= (~Range_status::Range_left);
    }

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

    return Error_code::SUCCESS;
}

Error_manager DetectWheel::isRangeCarWidth() {    // 判断车辆宽度超界情况
    if (m_detect_result.width() < m_region.car_min_width()-0.000 ||
            m_detect_result.width() > m_region.car_max_width()+0.000)
    {
        m_border_status |= Range_status::Range_car_width;
    }
    else if (m_detect_result.width() > m_region.car_min_width()+0.010 &&
            m_detect_result.width() < m_region.car_max_width()-0.010)
    {
        m_border_status &= (~Range_status::Range_car_width);
    }
    return Error_code::SUCCESS;
}

Error_manager DetectWheel::isRangeCarWheelbase() {    // 判断车辆轴距超界情况
    if (m_detect_result.wheel_base() < m_region.car_min_wheelbase()-0.000 ||
            m_detect_result.wheel_base() > m_region.car_max_wheelbase()+0.000)
    {
        m_border_status |= Range_status::Range_car_wheelbase;
    }
    else if (m_detect_result.wheel_base() > m_region.car_min_wheelbase()+0.010 ||
            m_detect_result.wheel_base() < m_region.car_max_wheelbase()-0.010)
    {
        m_border_status &= (~Range_status::Range_car_wheelbase);
    }
    return Error_code::SUCCESS;
}

Error_manager DetectWheel::isAngleClock() {
    double t_dtheta = m_detect_result.theta() + m_region.plc_offset_degree();
    if (t_dtheta < m_region.turnplate_angle_limit_anti_clockwise()+0.00)
    {
        m_border_status |= Range_status::Range_angle_anti_clock;
    }
    else if (t_dtheta > m_region.turnplate_angle_limit_anti_clockwise() + 0.10)
    {
        m_border_status &= (~Range_status::Range_angle_anti_clock);
    }

    if (t_dtheta > m_region.turnplate_angle_limit_clockwise()-0.00)
    {
        m_border_status |= Range_status::Range_angle_clock;
    }
    else if (t_dtheta < m_region.turnplate_angle_limit_clockwise() - 0.10)
    {
        m_border_status &= (~Range_status::Range_angle_clock);
    }

    return Error_code::SUCCESS;
}

