#include "toid_behaviors/simple_rotate.hpp"

#include <cmath>

#include "angles/angles.h"
#include "tf2/convert.hpp"

namespace toid
{

SimpleRotateBehavior::SimpleRotateBehavior() : SimpleMove<RotateAction>() {}
SimpleRotateBehavior::~SimpleRotateBehavior() {}

void SimpleRotateBehavior::configureCB()
{
  auto node = node_.lock();
  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".max_angular_vel", rclcpp::ParameterValue(2.0));

  node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".min_angular_vel", rclcpp::ParameterValue(0.2));
  node->get_parameter(behavior_name_ + ".min_angular_vel", min_angular_speed_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".max_angular_accel", rclcpp::ParameterValue(4.0));
  node->get_parameter(behavior_name_ + ".max_angular_accel", max_angular_accel_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(4.0));
  node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_);
}

ResultStatus SimpleRotateBehavior::onStart(
  const std::shared_ptr<const RotateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
  const geometry_msgs::msg::Twist & vel)
{
  target_angle_ = angles::normalize_angle(command->angle);
  min_turn_angle_ = abs(command->min_angle);
  initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
  max_angular_speed_ = command->max_speed;

  if(command->max_speed == 0) {
    auto node = node_.lock();
    node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
  }

  last_angle_ = tf2::getYaw(pose.orientation);

  angular_speed_ = vel.angular.z;
  last_time_ = clock_->now().seconds();
  return ResultStatus{Status::SUCCEEDED};
}

ResultStatus SimpleRotateBehavior::updateVel(
  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
  geometry_msgs::msg::Twist & out_vel)
{
  const double current_yaw = tf2::getYaw(pose.orientation);
  const double angle_change = angles::shortest_angular_distance(last_angle_ , current_yaw);
  last_angle_ = current_yaw;

  double err = angles::shortest_angular_distance(current_yaw, target_angle_);
  double sign = (err < 0)? -1.0 : 1.0;
  err = std::abs(err);

  if (min_turn_angle_ > 0.0) {
    min_turn_angle_ = std::max(0.0, min_turn_angle_ - initial_direction_ * angle_change);
    err = std::max( initial_direction_ * sign * err, 0.0);
    err = std::max(min_turn_angle_, err);
    sign = initial_direction_;
  }

  const double upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_;
  const double lower_vel_ = angular_speed_ - max_angular_accel_ * control_duration_;

  const double speed_until_overshoot =
    std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));

  const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
  const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);

  if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.01) {
    return ResultStatus{Status::SUCCEEDED};
  }

  angular_speed_ = speed;
  out_vel.angular.z = speed;
  return ResultStatus{Status::RUNNING};
}

}  // namespace toid

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(toid::SimpleRotateBehavior, nav2_core::Behavior);