/*
 * @Description: 底盘z高度检测类,准确识别出平移,旋转,车宽,底盘高度，供3D车轮算法优化出车辆准确信息
 * @Author: yct
 * @Date: 2021-09-16 15:02:49
 * @LastEditTime: 2021-11-16 15:45:02
 * @LastEditors: yct
 */

#ifndef Z_DETECTOR_HH
#define Z_DETECTOR_HH

#include <iostream>
#include <chrono>
#include <ceres/ceres.h>
#include <pcl/common/common.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include <opencv2/opencv.hpp>

#include "tool/singleton.h"

class Car_pose_detector : public Singleton<Car_pose_detector> {
    friend class Singleton<Car_pose_detector>;

public:
    Car_pose_detector(const Car_pose_detector &) = delete;

    Car_pose_detector &operator=(const Car_pose_detector &) = delete;

    ~Car_pose_detector() = default;

    // 变换点云
    void inv_trans_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double x, double y, double theta);

    // 变换点云
    void trans_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double x, double y, double theta);

    // 创建两轴具体曲线
    void create_curve_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double width);

    // 检测底盘z方向值，去中心，
    bool detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
                     double &x, double &y, double &theta, double &width, double &z_value, bool debug_cloud = false);

    // 检测底盘z方向值，去中心，使用mat加速
    bool
    detect_pose_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
                    double &x, double &y, double &theta, bool debug_cloud = false);

    // 生成优化图像，降低优化耗时
    static cv::Mat
    create_mat(double min_x, double max_x, double min_y, double max_y, double resolution_x, double resolution_y);

private:
    // 父类的构造函数必须保护，子类的构造函数必须私有。
    Car_pose_detector() = default;

    static cv::Mat m_model;
};

#endif // !Z_DETECTOR_HH