//
// Created by zx on 22-8-30.
//

#pragma once

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/point_representation.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/common_headers.h>
#include <glog/logging.h>
#include <iostream>
#include <cmath>

#if __VIEW__PCL
#include <pcl/visualization/pcl_visualizer.h>
#endif

//ax+by+c=0
typedef struct {
    float a;
    float b;
    float c;
    float cx;
    float cy;
    float theta_by_x;
    float cov;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
} Line;

class Safety_status {
public:
    Safety_status() {
        clamp_completed = false;
        wheel_exist = false;
        cx = 0;
        gap = 0;
        time_point = std::chrono::steady_clock::now();
        timeout_s = 0.5;
    };

    ~Safety_status() = default;

    Safety_status(const Safety_status &other) {
        clamp_completed = other.clamp_completed;
        wheel_exist = other.wheel_exist;
        cx = other.cx;
        gap = other.gap;
        time_point = other.time_point;
        timeout_s = other.timeout_s;
    }

    Safety_status &operator=(const Safety_status &other) {
        clamp_completed = other.clamp_completed;
        wheel_exist = other.wheel_exist;
        cx = other.cx;
        gap = other.gap;
        time_point = other.time_point;
        timeout_s = other.timeout_s;
        return *this;
    }

    void set_timeout(float s) {
        timeout_s = s;
    }

    bool is_timeout() const {
        auto t2 = std::chrono::steady_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - time_point);

        double time = double(duration.count()) * std::chrono::microseconds::period::num /
                      std::chrono::microseconds::period::den;
        DLOG(INFO) << "time " << time;
        return time > timeout_s;
    }

    bool clamp_completed;
    bool wheel_exist;           //点云直线是否是轮子
    float cx;
    float gap;                  //轮子距离雷达
    std::chrono::steady_clock::time_point time_point;
    float timeout_s;
    std::mutex mutex_;
};

class clamp_safety_detect {
public:
    clamp_safety_detect();

    ~clamp_safety_detect();

#if __VIEW__PCL
    void set_viewer(pcl::visualization::PCLVisualizer* m_viewer,int port);
#endif

    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Safety_status &safety);

    bool fit_line(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Line &line);

    bool check_wheel_line(const Line &line, Safety_status &safety);

protected:
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> classify_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);

    bool check_clamp_lines(const Line &line1, const Line &line2, Safety_status &safety);

#if __VIEW__PCL
    void view_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string name,int r,int g,int b);
     pcl::visualization::PCLVisualizer* m_viewer;
     int m_port;
#endif
};