#pragma once

#include "tool/singleton.hpp"
#include <algorithm>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>

#include "tool/point3D_tool.hpp"


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

public:
    struct WheelDetectResult {
        bool haveCloud = false;
        bool segmentOk = false;
        bool detectOk = false;
        bool detectOkOnTime = false;
        pcl::PointXYZ center;
        float confidence = 0;
        float theta = 0;
        float loss = 0;

        void reset() {
            haveCloud = false;
            segmentOk = false;
            detectOk = false;
            detectOkOnTime = false;
            center.x = 0;
            center.y = 0;
            center.z = 0;
            confidence = 0;
            theta = 0;
            loss = 0;
        }

        std::string info() {
            char str[512];
            sprintf(str, "x: %.3f, y: %.3f, z: %.3f, theta: %.3f loss: %.3f conf: %.3f\n",
                    center.x, center.y, center.z, theta, loss, confidence);
            return str;
        }
    };

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

    ~WheelCeresDetector() override = default;

    // 单轮检测
    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
                WheelDetectResult &result, bool left_model);
    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr left_in_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr left_out_cloud,
                WheelDetectResult &left_result,
                pcl::PointCloud<pcl::PointXYZ>::Ptr right_in_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr right_out_cloud,
                WheelDetectResult &right_result);
    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vct_wheels_cloud,
                                    pcl::PointCloud<pcl::PointXYZ>::Ptr left_out_cloud,
                                    WheelCeresDetector::WheelDetectResult *wheels_result);
protected:
    // 父类的构造函数必须保护，子类的构造函数必须私有。
    WheelCeresDetector() = default;

private:
    std::vector<float> singleWheelModel();
    std::vector<float> singleLeftWheelModel();
    std::vector<float> singleRightWheelModel();

    void doubleWheelModel( std::vector<float>& linel,std::vector<float>& liner);

private:
    std::vector<double> line_;
};