#include "toid_behaviors/move_coords.hpp"

#include "angles/angles.h"

namespace toid
{

MoveCoords::MoveCoords() : SimpleMove<MoveAction>() {}
MoveCoords::~MoveCoords() {}

void MoveCoords::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.10));
  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_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".kphi", rclcpp::ParameterValue(5.0));
  node->get_parameter(behavior_name_ + ".kphi", k_phi_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".kdelta", rclcpp::ParameterValue(2.0));
  node->get_parameter(behavior_name_ + ".kdelta", k_delta_);

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

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".lambda", rclcpp::ParameterValue(2.0));
  node->get_parameter(behavior_name_ + ".lambda", lambda_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".slow_down_radius", rclcpp::ParameterValue(0.20));
  node->get_parameter(behavior_name_ + ".slow_down_radius", slowdown_radius_);
}

ResultStatus MoveCoords::onStart(
  const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose &,
  const geometry_msgs::msg::Twist & vel)
{
  target_pose_.position.x = command->x;
  target_pose_.position.y = command->y;
  backwards_ = command->backwards;

  target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta));
  target_angle_ = command->theta;
  target_sign_ = backwards_ ? -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_);
  }

  scl.k_phi = k_phi_;
  scl.k_delta = k_delta_;
  scl.bbeta = beta_;
  scl.lam = lambda_;
  scl.slowdown_radius = slowdown_radius_;
  scl.v_angular_max = max_angular_speed_;
  scl.v_linear_min = min_vel_speed_;
  scl.v_linear_max = max_vel_speed_;

  last_speed_ = vel.angular.x;

  return ResultStatus{Status::SUCCEEDED};
}

ResultStatus MoveCoords::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);
  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);

  if (backwards_) {
    angle_dist = angles::two_pi_complement(angle_dist);
  }

  const double dx = target_pose_.position.x - pose.position.x;
  const double dy = target_pose_.position.y - pose.position.y;

  const double dist_left = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_));

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

  if (dist_left <= 0.001) {
    out_vel.linear.x = 0;
    out_vel.angular.z = 0;
    return ResultStatus{Status::SUCCEEDED};
  }

  double vel = max_vel_speed_;
  double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
  vel = std::min(vel, max_vel_to_stop);


  if (dist_left <= 0.02) {
    out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound);
    out_vel.angular.z = std::clamp(target_sign_ * kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
    last_speed_ = out_vel.linear.x;
    return ResultStatus{Status::RUNNING};
  }

  scl.v_linear_max = target_sign_ * std::clamp(target_sign_ * vel, lower_bound, upper_bound);
  scl.calculate_vel(target_pose_, pose, out_vel, backwards_);
  last_speed_ = out_vel.linear.x;

  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); 
  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl.v_linear_max); 

  return ResultStatus{Status::RUNNING};
}

}  // namespace toid


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