#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
#include <cmath>

#include <random>
#include <chrono>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>

class CloudFilterNode : public rclcpp::Node
{
  //Topic parameters
  std::string topic_in;
  std::string topic_out;

  // Filter parameters
  float x_min;
  float x_max;
  float z_min;
  float z_max;
  float tan_h_fov;
  float tan_v_fov;

  //Preallocate pointclouds
  pcl::PointCloud<pcl::PointXYZI>::Ptr input {new pcl::PointCloud<pcl::PointXYZI>};
  pcl::PointCloud<pcl::PointXYZI>::Ptr fov_filtered {new pcl::PointCloud<pcl::PointXYZI>};
  pcl::PointCloud<pcl::PointXYZI>::Ptr voxel_filtered {new pcl::PointCloud<pcl::PointXYZI>};

  //Preallocate filter
  pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;

public:
  CloudFilterNode()
  : Node("cloud_preprocessing"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_)
  {

    //Topic parameters
    this->declare_parameter<std::string>("topic_in", "test");
    this->declare_parameter<std::string>("topic_out", "test2");

    //Filter parameters
    this->declare_parameter<float>("x_min", 0.0f);
    this->declare_parameter<float>("x_max", 0.0f);
    this->declare_parameter<float>("z_min", 0.0f);
    this->declare_parameter<float>("z_max", 0.0f);
    this->declare_parameter<float>("tan_h_fov", 0.0f);  // ±45°
    this->declare_parameter<float>("tan_v_fov", 0.0f);  // ±30°

    this->get_parameter("topic_in", topic_in);
    this->get_parameter("topic_out", topic_out);

    this->get_parameter("x_min", x_min);
    this->get_parameter("x_max", x_max);
    this->get_parameter("z_min", z_min);
    this->get_parameter("z_max", z_max);
    this->get_parameter("tan_h_fov", tan_h_fov);
    this->get_parameter("tan_v_fov", tan_v_fov);

    voxel_filter.setLeafSize(0.03f, 0.03f, 0.03f);  // Adjust as needed

    RCLCPP_INFO(this->get_logger(), "Starting filter node with parameters:\n"
      "topic_in: %s\n"
      "topic_out: %s\n"
      "x_min: %f\n"
      "x_max: %f\n"
      "z_min: %f\n"
      "z_max: %f\n"
      "tan_h_fov: %f\n"
      "tan_v_fov: %f"
    , topic_in.c_str(), topic_out.c_str(), x_min, x_max, z_min, z_max, tan_h_fov, tan_v_fov
  );

    subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
      topic_in, rclcpp::SensorDataQoS(),
      std::bind(&CloudFilterNode::cloud_callback, this, std::placeholders::_1));

    filtered_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(topic_out, 10);
  }

private:
  void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
  {
    //auto start = std::chrono::high_resolution_clock::now();
    //Clear all pointclouds
    input->clear();
    fov_filtered->clear();
    voxel_filtered->clear();

    // Convert to PCL
    pcl::fromROSMsg(*msg, *input);

    fov_filtered->reserve(input->size());

    for (const auto& point : input->points)
    {
      if (point.x < x_min || point.x > x_max) continue;
      if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) continue;
      if (point.z < z_min || point.z > z_max) continue;

      float inv_x = 1.0f / point.x;
      if (fabsf(point.y * inv_x) > tan_h_fov) continue;
      if (fabsf(point.z * inv_x) > tan_v_fov) continue;

      fov_filtered->push_back(point);
    }

    // Apply voxel grid filter
    voxel_filter.setInputCloud(fov_filtered);
    voxel_filter.filter(*voxel_filtered);

    //Apply ransac
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZI> seg;
    // Optional
    seg.setOptimizeCoefficients(true);
    // Mandatory
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.02);
    seg.setMaxIterations(200);
  
    seg.setInputCloud(voxel_filtered);
    seg.segment (*inliers, *coefficients);
  
    if (inliers->indices.size () == 0)
    {
      RCLCPP_INFO(this->get_logger(), "Oh oh. No inliers");
      return;
    }

    //Select remaining points
    pcl::ExtractIndices<pcl::PointXYZI> extract;
    extract.setInputCloud(voxel_filtered);
    extract.setIndices(inliers);
    extract.setNegative(true);
    extract.filter(*voxel_filtered);

    // Convert and publish
    sensor_msgs::msg::PointCloud2 out_msg;
    pcl::toROSMsg(*voxel_filtered, out_msg);
    out_msg.header.stamp = this->get_clock()->now();
    out_msg.header.frame_id = msg->header.frame_id;
    filtered_publisher_->publish(out_msg);

    //RCLCPP_INFO(this->get_logger(), "Filtered %zu -> %zu points", input->points.size(), voxel_filtered->points.size());
    //auto stop = std::chrono::high_resolution_clock::now();
    //auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);

    // To get the value of duration use the count()
    // member function on the duration object
    //std::cout << duration.count() << std::endl;

  }

  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;

  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;

  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_publisher_;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr original_publisher_;
};

int main(int argc, char * argv[])
{
  srand((unsigned int) time (NULL));
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<CloudFilterNode>());
  rclcpp::shutdown();
  return 0;
}