/**
  * @project ALG_WHeelData
  * @brief   TODO: add a brief
  * @author  lz
  * @data    2023/4/21
 **/
#pragma once

#include "define.pb.h"
#include "defines.hpp"
#include "wheel_ceres_3d.hpp"

// 测试用代码
#include "tool/static_tool.hpp"
#include "error_code/error_code.hpp"
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/impl/common.hpp>
#include <json/json.h>
#include "google/protobuf/util/json_util.h"
#include "car_pose_detector.h"
#include "chassis_ceres_solver.h"

#define RECORD_DETECT_CLOUD_DATA 0

class DetectWheel {
public:
    DetectWheel();

    ~DetectWheel() = default;

    Error_manager init(const Region &region);

    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);

    Error_manager getDetectResult(DetectResult &result);

    Error_manager upStatus(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);

    int borderStatus();

    int groundStatus();

protected:
    bool cutFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud);

    bool voxeFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud,
                    pcl::PointXYZ leaf);

    bool outlierFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud);

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud);

private:

    bool classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
                               DetectResult &result);

    bool isRect(std::vector<cv::Point2f> &points);

    Error_manager upBorderStatus();

    Error_manager isRangeXY();

    Error_manager isRangeCarWidth();

    Error_manager isRangeCarWheelbase();

    Error_manager isAngleClock();

private:
    int m_border_status;
    int m_ground_status;
    DetectResult m_detect_result;
    std::chrono::steady_clock::time_point m_t = std::chrono::steady_clock::now();
    pcl::PointCloud<pcl::PointXYZ>::Ptr mp_cloud_filtered;      // 区域切除后的点
    pcl::PointCloud<pcl::PointXYZ>::Ptr mp_cloud_detect_z;      // 底盘切除后的点

    Region m_region;
    WheelCeres3D *m_detector; // 检测器

};

