#pragma once
#include <memory>

#include "geometry_msgs/msg/point_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "nav2_controller/plugins/position_goal_checker.hpp"
#include "nav2_core/controller.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
#include "nav2_util/node_utils.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "pluginlib/class_loader.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/parameter_value.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "tf2_ros/buffer.hpp"
#include "toid_spinner_controller/path_handler.hpp"

namespace toid {
class SpinnerController : public nav2_core::Controller {
public:
  SpinnerController();
  ~SpinnerController() override = default;

  void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
                 std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
                 std::shared_ptr<nav2_costmap_2d::Costmap2DROS>) override;

  void cleanup() override;

  void activate() override;

  void deactivate() override;

  void setPlan(const nav_msgs::msg::Path &path) override;

  bool cancel() override;

  geometry_msgs::msg::TwistStamped
  computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose,
                          const geometry_msgs::msg::Twist &velocity,
                          nav2_core::GoalChecker *goal_checker) override;

  geometry_msgs::msg::TwistStamped
  computeRotateToHeadingCommand(const double &angular_distance_to_heading,
                                const geometry_msgs::msg::PoseStamped &pose);

  void setSpeedLimit(const double &speed_limit,
                     const bool &percentage) override;

protected:
  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
  rclcpp::Logger logger_{rclcpp::get_logger("SpinnerController")};
  rclcpp::Clock::SharedPtr clock_;
  std::unique_ptr<PathHandler> path_handler_;
  std::unique_ptr<nav2_controller::PositionGoalChecker> position_goal_checker_;
  std::unique_ptr<
      nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
      collision_checker_;
  rclcpp_lifecycle::LifecyclePublisher<
      geometry_msgs::msg::PointStamped>::SharedPtr closest_pt_pub_;

  std::shared_ptr<tf2_ros::Buffer> tf_;
  std::string plugin_name_;
  bool new_path_recieved = false;
  double last_angular_vel_;

  pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
  nav2_core::Controller::Ptr primary_controller_;

  // Params
  double angular_dist_threshold_;
  double angular_disengage_threshold_;
  double forward_sampling_distance_;
  double max_angular_accel_;
  double rotate_to_heading_;

  double control_duration_;
};
} // namespace toid