#ifndef CLOUD_CLUSTERING_HPP
#define CLOUD_CLUSTERING_HPP

#include "rclcpp/rclcpp.hpp"

#include <geometry_msgs/msg/point.hpp>
#include "visualization_msgs/msg/marker_array.hpp"

#include "opencv2/video/tracking.hpp"

#include "std_msgs/msg/float32_multi_array.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"

#include "sensor_msgs/msg/point_cloud2.hpp"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/segmentation/extract_clusters.h>

namespace cloud_clustering
{
    // KF init
    int stateDim = 4; // [x,y,v_x,v_y]//,w,h]
    int measDim = 2;  // [z_x,z_y,z_w,z_h]
    int ctrlDim = 0;

    std::string frame_id;
    std::string topic_in;

    float z_dim_scale;
    float cluster_tolerance;
    int min_cluster_size;
    int max_cluster_size;
    float min_width;
    float min_height;
    float min_length;
    float max_width;
    float max_height;
    float max_length;

    cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F);
    cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F);
    cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F);
    cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F);
    cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F);
    cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F);

    std::vector<geometry_msgs::msg::Point> prevClusterCenters;

    cv::Mat state(stateDim, 1, CV_32F);
    cv::Mat_<float> measurement(2, 1);

    std::vector<int> objID; // Output of the data association using KF
                            // measurement.setTo(Scalar(0));

    bool firstFrame = true;
    /*
    cv::Mat state(stateDim, 1, CV_32F);
    cv::Mat_<float> measurement(2, 1);
    */

    class CloudClustering : public rclcpp::Node
    {
    public:
        CloudClustering(
            const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
        CloudClustering(
            const std::string &name_space,
            const rclcpp::NodeOptions &options = rclcpp::NodeOptions());

    private:
        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub;
        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr markerPub;
        rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr objID_pub;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster0;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster1;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster2;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster3;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster4;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster5;

        std::vector<geometry_msgs::msg::Point> prevClusterCenters;

        std::vector<int> objID; // Output of the data association using KF
                                // measurement.setTo(Scalar(0));

        bool firstFrame = true;
        rclcpp::Clock::SharedPtr clock_;
        std::string frame_id;
        std::string filtered_cloud;

        double euclidean_distance(geometry_msgs::msg::Point &p1, geometry_msgs::msg::Point &p2);
        std::pair<int, int> findIndexOfMin(std::vector<std::vector<float>> distMat);
        void KFT(const std_msgs::msg::Float32MultiArray ccs);
        void publish_cloud(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub,
                           pcl::PointCloud<pcl::PointXYZI>::Ptr cluster);

        void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
                         std::vector<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, float>> &cluster_vec,
                         std::vector<pcl::PointXYZI> &clusterCentroids);
        void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input);
    };

}
#endif // CLOUD_CLUSTERING_HPP
