#include "toid_behaviors/simple_translate_x.hpp"

#include <cmath>

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

namespace toid
{

SimpleTranslateXBehavior::SimpleTranslateXBehavior() : SimpleMove<TranslateAction>() {}
SimpleTranslateXBehavior::~SimpleTranslateXBehavior() {}

void SimpleTranslateXBehavior::configureCB()
{
  auto node = node_.lock();
  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".max_vel_accel", rclcpp::ParameterValue(2.0));
  node->get_parameter(behavior_name_ + ".max_vel_accel", max_vel_accel_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".max_vel_decel", rclcpp::ParameterValue(1.0));
  node->get_parameter(behavior_name_ + ".max_vel_decel", max_vel_decel_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".max_vel_speed", rclcpp::ParameterValue(0.4));
  node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".min_vel_speed", rclcpp::ParameterValue(0.007));
  node->get_parameter(behavior_name_ + ".min_vel_speed", min_vel_speed_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".max_angular_speed", rclcpp::ParameterValue(1.0));
  node->get_parameter(behavior_name_ + ".max_angular_speed", max_angular_speed_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".kp", rclcpp::ParameterValue(5.0));
  node->get_parameter(behavior_name_ + ".kp", kp_);
}

ResultStatus SimpleTranslateXBehavior::onStart(
  const std::shared_ptr<const TranslateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
  const geometry_msgs::msg::Twist & vel)
{
  target_distance_ = command->distance;
  target_angle_ = tf2::getYaw(pose.orientation);
  target_sign_ = (target_distance_ < 0) ? -1.0 : 1.0;
  max_vel_speed_ = command->max_speed;

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

  target_distance_ *= target_sign_;

  initial_pose_ = pose;
  speed_ = vel.angular.x;

  return ResultStatus{Status::SUCCEEDED};
}

ResultStatus SimpleTranslateXBehavior::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_dist = angles::shortest_angular_distance(current_yaw, target_angle_);

  const double dx = pose.position.x - initial_pose_.position.x;
  const double dy = pose.position.y - initial_pose_.position.y;
  const double dist_passed = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_));

  const double lower_bound = speed_ - control_duration_ * max_vel_accel_;
  const double upper_bound = speed_ + control_duration_ * max_vel_accel_;

  float vel = std::clamp(max_vel_speed_, lower_bound, upper_bound);
  float max_vel_to_stop =
    0.8 * std::sqrt(2.0 * max_vel_decel_ * std::abs(target_distance_ - dist_passed));

  vel = std::min(vel, max_vel_to_stop);
  const double w = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);

  if (dist_passed >= target_distance_) {
    out_vel.linear.x = 0;
    out_vel.angular.z = 0;
    return ResultStatus{Status::SUCCEEDED};
  }

  out_vel.linear.x = target_sign_ * vel;
  out_vel.angular.z = w;

  speed_ = vel;

  return ResultStatus{Status::RUNNING};
}

}  // namespace toid

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