#pragma once

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

#pragma once

#include <thread>
#include <fcntl.h>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include "tool/point_tool.h"
#include "tool/measure_filter.h"
#include "tool/region_status_checker.h"
#include "tool/thread_condition.h"
#include "tool/common_data.h"
#include "tool/point3D_tool.h"
#include "tool/static_tool.hpp"
#include "tool/memory_box.hpp"
#include "system/system_communication_mq.h"
#include "error_code/error_code.hpp"
#include "match3d/detect_wheel_ceres3d.h"
#include "velodyne_config.pb.h"
#include "message/measure_message.pb.h"
#include "verify/Verify_result.h"
#include "ceres/car_pose_detector.h"
#include "ceres/chassis_ceres_solver.h"
#include "match3d/WheelFactor.h"

#include "communication/rslidar_mqtt_async.h"

class GroundRegion {
public:
    // 点云类型，用于外部获取存储的不同的点云
    enum Region_cloud_type {
        total = 0,
        left_front = 1,
        left_back = 2,
        right_front = 3,
        right_back = 4,
        filtered = 5,
        failure = 6,
        trans = 7,
    };
    // 工作状态
    enum Ground_region_status {
        E_UNKNOWN = 0, //未知
        E_READY = 1,   //准备，待机
        E_BUSY = 2,    //工作正忙
        E_FAULT = 10, //故障
    };

#define GROUND_REGION_DETECT_CYCLE_MS 500

    GroundRegion();

    ~GroundRegion();

    // 区域类初始化
    Error_manager init(velodyne::Region region, pcl::PointCloud<pcl::PointXYZ>::Ptr left_model,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr right_model);

    // 公有检测函数,根据初始化设置的区域进行裁剪，并识别获得结果
    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &result);

    // 公有点云更新函数，传入最新点云获得结果
    Error_manager update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);

private:
    /**
     * @description 对点云进行裁减滤波，然后通过欧式聚类判断是否存在四块点云，如果存在且可以构成矩形，通过ceres对点云进行车轮参数寻优；
     * @param cloud 输入点云；
     * @param thresh_z 保留该值以下的点云；
     * @param result ceres库寻优结果；
     * @return true：成功，false：失败；
    **/
    bool classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
                               detect_wheel_ceres3d::Detect_result &result, std::string &error);

    /**
     * @description 算法测量线程函数，在这个线程中不断对点云进行算法检测；
    **/
    void thread_measure_func();

    /**
     * @description 欧式聚类，pcl接口二次封装；
     * @param sor_cloud 输入点云；
     * @param tolerance 聚类距离参数
     * @return 各个类的点云，存储在std的vector中；
    **/
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr &sor_cloud, const float &tolerance);

    /**
     * @description 判断输入点是否是一个近似的矩形（直角边与坐标轴的夹角 <20°）；
     * @param points 四个点的坐标信息（如果是三个点会模拟补全）；
     * @return true:类矩形，false：非矩形；
    **/
    bool isRect(std::vector<cv::Point2f> &points);

    /**
     * @description 对坐标经过plc偏移转换过的算法检测结果根据输入的超界限制给出超界判断结果；
     * @param measure_result 算法检测的结果；
     * @param out_info 超界范围限制信息；
     * @return 超界结果，按Range_status进行位组合；
    **/
    int outOfRangeDetection(Common_data::Car_wheel_information &measure_result, const velodyne::outOfRangeInfo &out_info, detect_wheel_ceres3d::Detect_result &result);

    void getMSE(MemoryBox<double> &box, float &value);

    /**
     * @description 对输入的两个测量结果作对比，给出误差率；
     * @param first 测量结果1；
     * @param second 测量结果2；
     * @return 测量结果平均波动与波动标准的比；
    **/
    bool CompareCeresResult(detect_wheel_ceres3d::Detect_result &first, detect_wheel_ceres3d::Detect_result &second);

    Error_manager ClippingAndFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered);

    Error_manager CalculateInitialPose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered, detect_wheel_ceres3d::Detect_result &pose, pcl::PointCloud<pcl::PointXYZ>::Ptr car_clean_cloud);
private:

    std::chrono::steady_clock::time_point m_last_move_time = std::chrono::steady_clock::now();
    velodyne::Region m_region;              // 区域配置
    detect_wheel_ceres3d *m_detector;       // 检测器
    Ground_region_status m_region_status;   // 区域状态

    // 自动检测线程
    std::thread *m_measure_thread;
    Thread_condition m_measure_condition;

    std::mutex m_cloud_collection_mutex;                                // 点云更新互斥锁, 内存由上级管理
    pcl::PointCloud<pcl::PointXYZ>::Ptr mp_cloud_collection;            // 扫描点的点云集合, 内存由上级管理
    std::chrono::system_clock::time_point m_cloud_collection_time;      // 点云更新时间

    std::mutex m_detect_failture_cloud_collection_mutex;                                // 点云检测失败更新互斥锁, 内存由上级管理
    pcl::PointCloud<pcl::PointXYZ>::Ptr mp_detect_failture_cloud_collection;            // 扫描点的点云检测失败集合, 内存由上级管理
    std::chrono::steady_clock::time_point m_detect_failture_cloud_collection_time;      // 检测失败的点云更新时间

    std::mutex m_filtered_cloud_mutex;                          // 点云更新互斥锁, 内存由上级管理
    pcl::PointCloud<pcl::PointXYZ>::Ptr mp_cloud_filtered;      // 区域切除后的点

    std::mutex m_detect_z_cloud_mutex;                          // 点云更新互斥锁, 内存由上级管理
    pcl::PointCloud<pcl::PointXYZ>::Ptr mp_cloud_detect_z;      // 底盘切除后的点

    std::mutex m_car_result_mutex;                                              // 测量结果互斥锁
    std::chrono::system_clock::time_point m_detect_update_time;                 // 定位结果的更新时间.
    Common_data::Car_wheel_information m_car_wheel_forward_information;         // 车轮的定位结果,
    Common_data::Car_wheel_information m_car_wheel_reverse_information;         // 车轮的定位结果,
    Common_data::Car_wheel_information m_car_wheel_terminal_information;        // 车轮的定位结果,
    Common_data::Car_information m_car_information;

    // 裁剪块
    std::vector<pcl::Vertices> m_polygons;                    //设置动态数组用于保存凸包顶点
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_surface_hull;       //设置点云用于描述凸包的形状

    // 位姿记录
    detect_wheel_ceres3d::Detect_result m_pose_record;
    double m_cut_line;
};


