#include "toid_spinner_controller/toid_spinner_controller.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "pluginlib/exceptions.hpp"
#include "rclcpp/logging.hpp"
#include "toid_spinner_controller/path_handler.hpp"

#include <algorithm>
#include <angles/angles.h>
#include <cmath>
#include <limits>
#include <memory>
#include <rclcpp/duration.hpp>
#include <tf2/time.hpp>
#include <tf2/utils.hpp>

namespace toid {
SpinnerController::SpinnerController()
    : lp_loader_("nav2_core", "nav2_core::Controller") {}

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

  position_goal_checker_ =
      std::make_unique<nav2_controller::PositionGoalChecker>();
  position_goal_checker_->initialize(parent, ".position_checker", costmap_ros);
  plugin_name_ = name;
  node_ = parent;
  auto node = node_.lock();

  tf_ = tf;
  logger_ = node->get_logger();
  clock_ = node->get_clock();

  std::string primary_controller;
  double control_frequency;

  nav2_util::declare_parameter_if_not_declared(
      node, plugin_name_ + ".angular_dist_threshold",
      rclcpp::ParameterValue(0.785));
  nav2_util::declare_parameter_if_not_declared(
      node, plugin_name_ + ".angular_disengage_threshold",
      rclcpp::ParameterValue(0.785 / 2.0));
  nav2_util::declare_parameter_if_not_declared(
      node, plugin_name_ + ".forward_sampling_distance",
      rclcpp::ParameterValue(0.5));
  nav2_util::declare_parameter_if_not_declared(
      node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
  nav2_util::declare_parameter_if_not_declared(
      node, plugin_name_ + ".rotate_to_heading", rclcpp::ParameterValue(1.0));
  nav2_util::declare_parameter_if_not_declared(
      node, plugin_name_ + ".primary_controller.plugin",
      rclcpp::PARAMETER_STRING);

  node->get_parameter(plugin_name_ + ".angular_dist_threshold",
                      angular_dist_threshold_);
  node->get_parameter(plugin_name_ + ".angular_disengage_threshold",
                      angular_disengage_threshold_);
  node->get_parameter(plugin_name_ + ".forward_sampling_distance",
                      forward_sampling_distance_);
  node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_);
  node->get_parameter(plugin_name_ + ".rotate_to_heading", rotate_to_heading_);

  primary_controller =
      node->get_parameter(plugin_name_ + ".primary_controller.plugin")
          .as_string();

  node->get_parameter("controller_frequency", control_frequency);
  control_duration_ = 1.0 / control_frequency;

  try {
    primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
    RCLCPP_INFO(logger_,
                "Created internal controller for multiplexer: %s of type %s",
                plugin_name_.c_str(), primary_controller.c_str());
  } catch (const pluginlib::PluginlibException &ex) {
    RCLCPP_FATAL(
        logger_,
        "Failed to create primary controller for multiplexer. Exception: %s",
        ex.what());
    return;
  }
  primary_controller_->configure(parent, name + ".primary_controller", tf,
                                 costmap_ros);

  path_handler_ = std::make_unique<PathHandler>(tf2::durationFromSec(0.1), tf_,
                                                costmap_ros);

  closest_pt_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
      "start_point", 1);
}

void SpinnerController::cleanup() {
  RCLCPP_INFO(logger_, "Cleaning SpinnerController");
  primary_controller_->cleanup();
  primary_controller_->reset();
  position_goal_checker_.reset();
  path_handler_.reset();
  closest_pt_pub_.reset();
}

void SpinnerController::activate() {
  RCLCPP_INFO(logger_, "Activating SpinnerController");
  last_angular_vel_ = std::numeric_limits<double>::max();
  new_path_recieved = false;
  auto node = node_.lock();
  closest_pt_pub_->on_activate();
  primary_controller_->activate();
  position_goal_checker_->reset();
}

void SpinnerController::deactivate() {
  RCLCPP_INFO(logger_, "Deactivating SpinnerController");
  closest_pt_pub_.reset();
  primary_controller_->deactivate();
  closest_pt_pub_->on_deactivate();
}

void SpinnerController::setPlan(const nav_msgs::msg::Path &path) {
  path_handler_->setPlan(path);
  new_path_recieved = true;
  primary_controller_->setPlan(path);
}

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

  auto path = path_handler_->transformGlobalPlan(pose, 1.0, M_PI / 8);

  if (path.poses.size() > 0) {
    geometry_msgs::msg::PointStamped pt;
    const auto &pose = path.poses.back();
    pt.point = pose.pose.position;
    pt.header.frame_id = path.header.frame_id;
    pt.header.stamp = path.header.stamp;
    closest_pt_pub_->publish(pt);
  }

  geometry_msgs::msg::PoseStamped robot_pose;
  path_handler_->tranformPose(path.header.frame_id, pose, robot_pose);
  const double robot_angle = tf2::getYaw(robot_pose.pose.orientation);

  if (path.poses.front().pose.position == path.poses.back().pose.position) {
    const double path_angle = tf2::getYaw(path.poses.back().pose.orientation);
    const double angle =
        angles::shortest_angular_distance(robot_angle, path_angle);
    return computeRotateToHeadingCommand(angle, pose);
  }

  if (new_path_recieved) {
    new_path_recieved = false;
    primary_controller_->setPlan(path);
  }

  auto twist = primary_controller_->computeVelocityCommands(pose, velocity,
                                                            goal_checker);
  last_angular_vel_ = twist.twist.angular.z;
  return twist;
}

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

  const double last_angular_vel =
      (last_angular_vel_ == std::numeric_limits<double>::max())
          ? 0
          : last_angular_vel_;
  const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
  const double speed_upper_bound =
      last_angular_vel + control_duration_ * max_angular_accel_;
  const double speed_lower_bound =
      last_angular_vel - control_duration_ * max_angular_accel_;

  const double speed_until_overshoot =
      0.9 * std::sqrt(2.0 * max_angular_accel_ *
                    std::fabs(angular_distance_to_heading));
  const double requested_speed = sign * std::fmin(speed_until_overshoot,rotate_to_heading_);
  const double speed =
      std::clamp(requested_speed, speed_lower_bound, speed_upper_bound);

  cmd.header.frame_id = pose.header.frame_id;
  cmd.header.stamp = pose.header.stamp;
  cmd.twist.angular.z = speed;
  last_angular_vel_ = speed;
  return cmd;
}

void SpinnerController::setSpeedLimit(const double &speed_limit,
                                      const bool &percentage) {
  primary_controller_->setSpeedLimit(speed_limit, percentage);
}

bool SpinnerController::cancel() { return primary_controller_->cancel(); }

} // namespace toid

PLUGINLIB_EXPORT_CLASS(toid::SpinnerController, nav2_core::Controller)