/*
 * @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.hpp"

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

public:
    struct CarDetectResult {
        float wheels_center_x = 0;
        float wheels_center_y = 0;
        float width = 0;
        float wheel_width = 0;
        float length = 0;
        float wheel_length = 0;
        float front_wheels_theta = 0;
        float theta = 0;
        float wheel_base = 0;
        float loss = 0;

        void reset() {
            wheels_center_x = 0;
            wheels_center_y = 0;
            theta = 0;
            width = 0;
            length = 0;
            wheel_width = 0;
            wheel_length = 0;
            front_wheels_theta = 0;
            wheel_base = 0;
            loss = 0;
        }

        std::string info() {
            char str[512];
            sprintf(str, "x:%0.3f y:%0.3f theta:%0.3f w:%0.3f l:%0.3f ww:%0.3f wl:%0.3f ft:%0.3f wb:%0.3f loss:%0.3f\n",
                    wheels_center_x, wheels_center_y, theta, width, length, wheel_width, wheel_length, front_wheels_theta, wheel_base, loss);
            return str;
        }
    };

    Car_pose_detector(const Car_pose_detector &) = delete;

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

    ~Car_pose_detector() = default;


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


private:
    // 父类的构造函数必须保护，子类的构造函数必须私有。
    Car_pose_detector();
    std::vector<double> line_;

};

#endif // !Z_DETECTOR_HH