// Default C++ imports
#include <limits>
#include <utility>
#include <string.h>
#include <iostream>
#include <iterator>
#include <algorithm>
#include <fstream>

// Headerfiles of project
#include "cloud_clustering.hpp"
#include <hungarian.hpp>

// Ros2 import
#include <rclcpp/rclcpp.hpp>

#include <opencv2/video/video.hpp>
#include <pcl_conversions/pcl_conversions.h>

// PCL Types
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

// Ros2 messages
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/int32_multi_array.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/common/centroid.h>
#include <pcl/common/geometry.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_conversions/pcl_conversions.h>

namespace cloud_clustering
{
  static const rclcpp::Logger LOGGER = rclcpp::get_logger("cloud_clustering");

  CloudClustering::CloudClustering(
      const rclcpp::NodeOptions &options) : CloudClustering("", options)
  {
  }

  CloudClustering::CloudClustering(
      const std::string &name_space,
      const rclcpp::NodeOptions &options) : Node("CloudClustering", name_space, options)
  {
    // Topic parameters
    this->declare_parameter<std::string>("topic_in", "filtered_points");
    this->declare_parameter<std::string>("frame_id", "velodyne");

    // Cluster parameters
    this->declare_parameter<float>("z_dim_scale", 0);
    this->declare_parameter<float>("cluster_tolerance", 0.0f);
    this->declare_parameter<int>("min_cluster_size", 0);
    this->declare_parameter<int>("max_cluster_size", 0);
    this->declare_parameter<float>("min_width", 0.0f);
    this->declare_parameter<float>("min_height", 0.0f);
    this->declare_parameter<float>("min_length", 0.0f); 
    this->declare_parameter<float>("max_width", 0.0f);
    this->declare_parameter<float>("max_height", 0.0f);
    this->declare_parameter<float>("max_length", 0.0f);    

    this->get_parameter("topic_in", topic_in);
    this->get_parameter("frame_id", frame_id);
    this->get_parameter("z_dim_scale", z_dim_scale);
    this->get_parameter("cluster_tolerance", cluster_tolerance);
    this->get_parameter("min_cluster_size", min_cluster_size);
    this->get_parameter("max_cluster_size", max_cluster_size);
    this->get_parameter("min_width", min_width);
    this->get_parameter("min_height", min_height);
    this->get_parameter("min_length", min_length);
    this->get_parameter("max_width", max_width);
    this->get_parameter("max_height", max_height);
    this->get_parameter("max_length", max_length);

    // Initialize publishers
    objID_pub = this->create_publisher<std_msgs::msg::Int32MultiArray>("object_ids", 10);
    pub_cluster0 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster0", 10);
    pub_cluster1 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster1", 10);
    pub_cluster2 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster2", 10);
    pub_cluster3 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster3", 10);
    pub_cluster4 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster4", 10);
    pub_cluster5 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster5", 10);
    markerPub = this->create_publisher<visualization_msgs::msg::MarkerArray>("markers", 10);

    RCLCPP_INFO(this->get_logger(), "About to setup callback");
    clock_ = this->get_clock();

    // Create a ROS subscriber for the input point cloud
    sub = this->create_subscription<sensor_msgs::msg::PointCloud2>(
        topic_in,
        10, // queue size
        std::bind(&CloudClustering::cloud_cb, this, std::placeholders::_1));

    std::cout << "Started clustering node with parameters:\n"
         << "topic_in: " << topic_in << "\n"
         << "frame_id: " << frame_id << "\n"
         << "z_dim_scale: " << z_dim_scale << "\n"
         << "cluster_tolerance: " << cluster_tolerance << "\n"
         << "min_cluster_size: " << min_cluster_size << "\n"
         << "max_cluster_size: " << max_cluster_size << "\n";
  }

  // calculate euclidean distance of two points
  double CloudClustering::euclidean_distance(geometry_msgs::msg::Point &p1, geometry_msgs::msg::Point &p2)
  {
    return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) +
                (p1.z - p2.z) * (p1.z - p2.z) * z_dim_scale);
  }
  /*
  //Count unique object IDs. just to make sure same ID has not been assigned to
  two KF_Trackers. int countIDs(vector<int> v)
  {
      transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n =
  distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log
  n), worst case O(n^2) (usually implemented as quicksort.
      // To guarantee worst case O(n log n) replace with make_heap, then
  sort_heap.

      // Unique will take a sorted range, and move things around to get duplicated
      // items to the back and returns an iterator to the end of the unique
  section of the range auto unique_end = unique(v.begin(), v.end()); // Again n
  comparisons return distance(unique_end, v.begin()); // Constant time for random
  access iterators (like vector's)
  }
  */

  /*

  objID: vector containing the IDs of the clusters that should be associated with
  each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc.
  */

  std::pair<int, int> CloudClustering::findIndexOfMin(std::vector<std::vector<float>> distMat)
  {
    std::pair<int, int> minIndex;
    float minEl = std::numeric_limits<float>::max();
    for (int i = 0; i < distMat.size(); i++)
      for (int j = 0; j < distMat.at(0).size(); j++)
      {
        if (distMat[i][j] < minEl)
        {
          minEl = distMat[i][j];
          minIndex = std::make_pair(i, j);
        }
      }
    return minIndex;
  }

  void CloudClustering::KFT(const std_msgs::msg::Float32MultiArray ccs)
  {

    // First predict, to update the internal statePre variable

    std::vector<cv::Mat> pred{KF0.predict(), KF1.predict(), KF2.predict(),
                              KF3.predict(), KF4.predict(), KF5.predict()};

    // cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
    // cout<<"Prediction 1
    // ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";

    // Get measurements
    // Extract the position of the clusters forom the multiArray. To check if the
    // data coming in, check the .z (every third) coordinate and that will be 0.0
    std::vector<geometry_msgs::msg::Point> clusterCenters; // clusterCenters

    int i = 0;
    for (std::vector<float>::const_iterator it = ccs.data.begin();
         it != ccs.data.end(); it += 3)
    {
      geometry_msgs::msg::Point pt;
      pt.x = *it;
      pt.y = *(it + 1);
      pt.z = *(it + 2);

      clusterCenters.push_back(pt);
    }

    std::vector<geometry_msgs::msg::Point> KFpredictions;
    i = 0;
    for (auto it = pred.begin(); it != pred.end(); it++)
    {
      geometry_msgs::msg::Point pt;
      pt.x = (*it).at<float>(0);
      pt.y = (*it).at<float>(1);
      pt.z = (*it).at<float>(2);

      KFpredictions.push_back(pt);
    }

    // Find the cluster that is more probable to be belonging to a given KF.
    objID.clear();   // Clear the objID vector
    objID.resize(6); // Allocate default elements so that [i] doesnt segfault.
                     // Should be done better
    // Copy clusterCentres for modifying it and preventing multiple assignments of
    // the same ID
    std::vector<geometry_msgs::msg::Point> copyOfClusterCenters(clusterCenters);
    std::vector<std::vector<float>> distMat;

    for (int filterN = 0; filterN < 6; filterN++)
    {
      std::vector<float> distVec;
      for (int n = 0; n < 6; n++)
      {
        distVec.push_back(
            euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[n]));
      }

      distMat.push_back(distVec);
      /*// Based on distVec instead of distMat (global min). Has problems with the
      person's leg going out of scope int
      ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()));
       //cout<<"finterlN="<<filterN<<"   minID="<<ID
       objID.push_back(ID);
      // Prevent assignment of the same object ID to multiple clusters
       copyOfClusterCenters[ID].x=100000;// A large value so that this center is
      not assigned to another cluster copyOfClusterCenters[ID].y=10000;
       copyOfClusterCenters[ID].z=10000;
      */
      // cout << "filterN=" << filterN << "\n";
    }

    Hungarian hungarian(distMat);
    std::vector<int> assignment = hungarian.solve();

    for (int i = 0; i < assignment.size(); i++) {
        objID[i] = assignment[i];
    }

    /*
    for (int clusterCount = 0; clusterCount < 6; clusterCount++)
    {
      // 1. Find min(distMax)==> (i,j);
      std::pair<int, int> minIndex(findIndexOfMin(distMat));
      // 2. objID[i]=clusterCenters[j]; counter++
      objID[minIndex.first] = minIndex.second;

      // 3. distMat[i,:]=10000; distMat[:,j]=10000
      distMat[minIndex.first] =
          std::vector<float>(6, 10000.0); // Set the row to a high number.
      for (int row = 0; row < distMat.size();
           row++) // set the column to a high number
      {
        distMat[row][minIndex.second] = 10000.0;
      }
      // 4. if(counter<6) got to 1.
      // cout << "clusterCount=" << clusterCount << "\n";
    }
    */
    

    // cout<<"Got object IDs"<<"\n";
    // countIDs(objID);// for verif/corner cases

    // display objIDs
    /* DEBUG
      cout<<"objID= ";
      for(auto it=objID.begin();it!=objID.end();it++)
          cout<<*it<<" ,";
      cout<<"\n";
      */

    visualization_msgs::msg::MarkerArray clusterMarkers;

    for (int i = 0; i < 6; i++)
    {
      visualization_msgs::msg::Marker m;

      m.id = i;
      m.type = visualization_msgs::msg::Marker::CUBE;
      m.header.frame_id = frame_id;
      m.scale.x = 0.3;
      m.scale.y = 0.3;
      m.scale.z = 0.3;
      m.action = visualization_msgs::msg::Marker::ADD;
      m.color.a = 1.0;
      m.color.r = i % 2 ? 1 : 0;
      m.color.g = i % 3 ? 1 : 0;
      m.color.b = i % 4 ? 1 : 0;

      // geometry_msgs::msg::Point clusterC(clusterCenters.at(objID[i]));
      geometry_msgs::msg::Point clusterC(KFpredictions[i]);
      m.pose.position.x = clusterC.x;
      m.pose.position.y = clusterC.y;
      m.pose.position.z = clusterC.z;

      clusterMarkers.markers.push_back(m);
    }

    prevClusterCenters = clusterCenters;

    markerPub->publish(clusterMarkers);

    std_msgs::msg::Int32MultiArray obj_id;
    for (auto it = objID.begin(); it != objID.end(); it++)
      obj_id.data.push_back(*it);
    // Publish the object IDs
    objID_pub->publish(obj_id);
    // convert clusterCenters from geometry_msgs::msg::Point to floats
    std::vector<std::vector<float>> cc;
    for (int i = 0; i < 6; i++)
    {
      std::vector<float> pt;
      pt.push_back(clusterCenters[objID[i]].x);
      pt.push_back(clusterCenters[objID[i]].y);
      pt.push_back(clusterCenters[objID[i]].z);

      cc.push_back(pt);
    }
    // cout<<"cc[5][0]="<<cc[5].at(0)<<"cc[5][1]="<<cc[5].at(1)<<"cc[5][2]="<<cc[5].at(2)<<"\n";
    float meas0[2] = {cc[0].at(0), cc[0].at(1)};
    float meas1[2] = {cc[1].at(0), cc[1].at(1)};
    float meas2[2] = {cc[2].at(0), cc[2].at(1)};
    float meas3[2] = {cc[3].at(0), cc[3].at(1)};
    float meas4[2] = {cc[4].at(0), cc[4].at(1)};
    float meas5[2] = {cc[5].at(0), cc[5].at(1)};

    // The update phase
    cv::Mat meas0Mat = cv::Mat(2, 1, CV_32F, meas0);
    cv::Mat meas1Mat = cv::Mat(2, 1, CV_32F, meas1);
    cv::Mat meas2Mat = cv::Mat(2, 1, CV_32F, meas2);
    cv::Mat meas3Mat = cv::Mat(2, 1, CV_32F, meas3);
    cv::Mat meas4Mat = cv::Mat(2, 1, CV_32F, meas4);
    cv::Mat meas5Mat = cv::Mat(2, 1, CV_32F, meas5);

    // cout<<"meas0Mat"<<meas0Mat<<"\n";
    if (!(meas0Mat.at<float>(0, 0) == 0.0f || meas0Mat.at<float>(1, 0) == 0.0f))
      cv::Mat estimated0 = KF0.correct(meas0Mat);
    if (!(meas1[0] == 0.0f || meas1[1] == 0.0f))
      cv::Mat estimated1 = KF1.correct(meas1Mat);
    if (!(meas2[0] == 0.0f || meas2[1] == 0.0f))
      cv::Mat estimated2 = KF2.correct(meas2Mat);
    if (!(meas3[0] == 0.0f || meas3[1] == 0.0f))
      cv::Mat estimated3 = KF3.correct(meas3Mat);
    if (!(meas4[0] == 0.0f || meas4[1] == 0.0f))
      cv::Mat estimated4 = KF4.correct(meas4Mat);
    if (!(meas5[0] == 0.0f || meas5[1] == 0.0f))
      cv::Mat estimated5 = KF5.correct(meas5Mat);

    // Publish the point clouds belonging to each clusters

    // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
    // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
    // cout<<"DONE KF_TRACKER\n";
  }
  void CloudClustering::publish_cloud(
      rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub,
      pcl::PointCloud<pcl::PointXYZI>::Ptr cluster)
  {
    // Create a PointCloud2 message
    auto clustermsg = std::make_shared<sensor_msgs::msg::PointCloud2>();

    // Convert PCL cloud to ROS 2 message
    pcl::toROSMsg(*cluster, *clustermsg);

    // Set header info
    clustermsg->header.frame_id = frame_id;
    clustermsg->header.stamp = clock_->now();

    // Publish the message
    pub->publish(*clustermsg);
  }

  void CloudClustering::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)
  {

    pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*input, *input_cloud);

    // Setup KdTree
    pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
    tree->setInputCloud(input_cloud);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
    ec.setClusterTolerance(cluster_tolerance);
    ec.setMinClusterSize(min_cluster_size);
    ec.setMaxClusterSize(max_cluster_size);
    ec.setSearchMethod(tree);
    ec.setInputCloud(input_cloud);
    /* Extract the clusters out of pc and save indices in cluster_indices.*/
    ec.extract(cluster_indices);

    std::vector<pcl::PointIndices>::const_iterator it;
    std::vector<int>::const_iterator pit;

    for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
    {

      pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster(
          new pcl::PointCloud<pcl::PointXYZI>);
      float x = 0.0;
      float y = 0.0;
      int numPts = 0;
      float intensity_sum = 0.0;

      float min_x = std::numeric_limits<float>::max();
      float min_y = std::numeric_limits<float>::max();
      float min_z = std::numeric_limits<float>::max();
      float max_x = -std::numeric_limits<float>::max();
      float max_y = -std::numeric_limits<float>::max();
      float max_z = -std::numeric_limits<float>::max();

      for (pit = it->indices.begin(); pit != it->indices.end(); pit++)
      {
        auto &pt = input_cloud->points[*pit];
        cloud_cluster->points.push_back(pt);
        x += pt.x;
        y += pt.y;
        numPts++;

        intensity_sum += input_cloud->points[*pit].intensity;
        // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
        //                                          origin);
        // mindist_this_cluster = std::min(dist_this_point,
        // mindist_this_cluster);

        min_x = std::min(min_x, pt.x);
        min_y = std::min(min_y, pt.y);
        min_z = std::min(min_z, pt.z);
        max_x = std::max(max_x, pt.x);
        max_y = std::max(max_y, pt.y);
        max_z = std::max(max_z, pt.z);
      }

      if (numPts == 0)
        continue;


      float width  = max_x - min_x;
      float length = max_y - min_y;
      float height = max_z - min_z;
    
      // Reject clusters outside size limits
      if (width < min_width || width > max_width ||
          height < min_height || height > max_height ||
          length < min_length || length > max_length)
      {
          continue;
      }
      


      pcl::PointXYZI centroid;
      centroid.x = x / numPts;
      centroid.y = y / numPts;
      centroid.z = 0.0;

      cluster_vec.push_back(std::make_pair(cloud_cluster, intensity_sum / numPts));

      // Get the centroid of the cluster
      clusterCentroids.push_back(centroid);
    }

    // Ensure at least 6 clusters exist to publish (later clusters may be empty)
    while (cluster_vec.size() < 6)
    {
      pcl::PointCloud<pcl::PointXYZI>::Ptr empty_cluster(
          new pcl::PointCloud<pcl::PointXYZI>);

      pcl::PointXYZI pt;
      pt.x = pt.y = pt.z = pt.intensity = 0.0f;

      empty_cluster->points.push_back(pt);

      pcl::PointXYZI centroid;
      centroid.x = 0.0;
      centroid.y = 0.0;
      centroid.z = 0.0;
      centroid.intensity = 0.0;

      cluster_vec.push_back(std::make_pair(empty_cluster, 0.0f));
    }

    while (clusterCentroids.size() < 6)
    {
      pcl::PointXYZI centroid;
      centroid.x = 0.0;
      centroid.y = 0.0;
      centroid.z = 0.0;
      centroid.intensity = 0.0;

      clusterCentroids.push_back(centroid);
    }
  }

  void CloudClustering::cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input)

  {
    // Vector of cluster pointclouds
    std::vector<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, float>> cluster_vec;
    // Cluster centroids
    std::vector<pcl::PointXYZI> clusterCentroids;

    if (firstFrame)
    {
      // If this is the first frame, initialize kalman filters for the clustered objects
      // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset.
      // Could be made generic by creating a Kalman Filter only when a new object
      // is detected

      float dvx = 0.01f; // 1.0
      float dvy = 0.01f; // 1.0
      float dx = 1.0f;
      float dy = 1.0f;
      KF0.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
                              dvx, 0, 0, 0, 0, dvy);
      KF1.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
                              dvx, 0, 0, 0, 0, dvy);
      KF2.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
                              dvx, 0, 0, 0, 0, dvy);
      KF3.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
                              dvx, 0, 0, 0, 0, dvy);
      KF4.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
                              dvx, 0, 0, 0, 0, dvy);
      KF5.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
                              dvx, 0, 0, 0, 0, dvy);

      cv::setIdentity(KF0.measurementMatrix);
      cv::setIdentity(KF1.measurementMatrix);
      cv::setIdentity(KF2.measurementMatrix);
      cv::setIdentity(KF3.measurementMatrix);
      cv::setIdentity(KF4.measurementMatrix);
      cv::setIdentity(KF5.measurementMatrix);
      // Process Noise Covariance Matrix Q
      // [ Ex 0  0    0 0    0 ]
      // [ 0  Ey 0    0 0    0 ]
      // [ 0  0  Ev_x 0 0    0 ]
      // [ 0  0  0    1 Ev_y 0 ]
      //// [ 0  0  0    0 1    Ew ]
      //// [ 0  0  0    0 0    Eh ]
      float sigmaP = 0.01;
      float sigmaQ = 0.1;
      setIdentity(KF0.processNoiseCov, cv::Scalar::all(sigmaP));
      setIdentity(KF1.processNoiseCov, cv::Scalar::all(sigmaP));
      setIdentity(KF2.processNoiseCov, cv::Scalar::all(sigmaP));
      setIdentity(KF3.processNoiseCov, cv::Scalar::all(sigmaP));
      setIdentity(KF4.processNoiseCov, cv::Scalar::all(sigmaP));
      setIdentity(KF5.processNoiseCov, cv::Scalar::all(sigmaP));
      // Meas noise cov matrix R
      cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ)); // 1e-1
      cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ));
      cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(sigmaQ));
      cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(sigmaQ));
      cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(sigmaQ));
      cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ));

      // Process the point cloud
      parse_cloud(input, cluster_vec, clusterCentroids);

      // Set initial state
      KF0.statePre.at<float>(0) = clusterCentroids.at(0).x;
      KF0.statePre.at<float>(1) = clusterCentroids.at(0).y;
      KF0.statePre.at<float>(2) = 0; // initial v_x
      KF0.statePre.at<float>(3) = 0; // initial v_y

      // Set initial state
      KF1.statePre.at<float>(0) = clusterCentroids.at(1).x;
      KF1.statePre.at<float>(1) = clusterCentroids.at(1).y;
      KF1.statePre.at<float>(2) = 0; // initial v_x
      KF1.statePre.at<float>(3) = 0; // initial v_y

      // Set initial state
      KF2.statePre.at<float>(0) = clusterCentroids.at(2).x;
      KF2.statePre.at<float>(1) = clusterCentroids.at(2).y;
      KF2.statePre.at<float>(2) = 0; // initial v_x
      KF2.statePre.at<float>(3) = 0; // initial v_y

      // Set initial state
      KF3.statePre.at<float>(0) = clusterCentroids.at(3).x;
      KF3.statePre.at<float>(1) = clusterCentroids.at(3).y;
      KF3.statePre.at<float>(2) = 0; // initial v_x
      KF3.statePre.at<float>(3) = 0; // initial v_y

      // Set initial state
      KF4.statePre.at<float>(0) = clusterCentroids.at(4).x;
      KF4.statePre.at<float>(1) = clusterCentroids.at(4).y;
      KF4.statePre.at<float>(2) = 0; // initial v_x
      KF4.statePre.at<float>(3) = 0; // initial v_y

      // Set initial state
      KF5.statePre.at<float>(0) = clusterCentroids.at(5).x;
      KF5.statePre.at<float>(1) = clusterCentroids.at(5).y;
      KF5.statePre.at<float>(2) = 0; // initial v_x
      KF5.statePre.at<float>(3) = 0; // initial v_y

      firstFrame = false;

      for (int i = 0; i < 6; i++)
      {
        geometry_msgs::msg::Point pt;
        pt.x = clusterCentroids.at(i).x;
        pt.y = clusterCentroids.at(i).y;
        prevClusterCenters.push_back(pt);
      }
    }

    else
    {
      parse_cloud(input, cluster_vec, clusterCentroids);

      std_msgs::msg::Float32MultiArray cc;
      for (int i = 0; i < 6; i++)
      {
        cc.data.push_back(clusterCentroids.at(i).x);
        cc.data.push_back(clusterCentroids.at(i).y);
        cc.data.push_back(clusterCentroids.at(i).z);
      }

      // cc_pos->publish(cc);// Publish cluster mid-points.
      KFT(cc);
      int i = 0;

      //std::sort(objID.begin(), objID.end(), [cluster_vec](const int &a, const int &b)
      //          { return cluster_vec[a].second < cluster_vec[b].second; });

      std::vector<std::pair<int, float>> kf_intensity;

      for (int i = 0; i < objID.size(); i++) {
          int cluster_index = objID[i];
          float mean_intensity = cluster_vec[cluster_index].second;
          kf_intensity.push_back(std::make_pair(i, mean_intensity));
      }

      std::sort(kf_intensity.begin(), kf_intensity.end(),
                [](const auto &a, const auto &b) {
                    return a.second > b.second;
                });

      for (auto &[kf_idx, intensity] : kf_intensity) {
        int cluster_index = objID[kf_idx];
        auto &cluster = cluster_vec[cluster_index].first;
        switch (i)
        {
        case 0:
        {
          publish_cloud(pub_cluster0, cluster_vec[cluster_index].first);
          i++;
          break;
        }
        case 1:
        {
          publish_cloud(pub_cluster1, cluster_vec[cluster_index].first);
          i++;
          break;
        }
        case 2:
        {
          publish_cloud(pub_cluster2, cluster_vec[cluster_index].first);
          i++;
          break;
        }
        case 3:
        {
          publish_cloud(pub_cluster3, cluster_vec[cluster_index].first);
          i++;
          break;
        }
        case 4:
        {
          publish_cloud(pub_cluster4, cluster_vec[cluster_index].first);
          i++;
          break;
        }

        case 5:
        {
          publish_cloud(pub_cluster5, cluster_vec[cluster_index].first);
          i++;
          break;
        }
        default:
          break;
        }
      }
    }
  }
}
