#include "toid_behaviors/rotate_acorns.hpp"

#include <cmath>

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

namespace toid
{

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

void RotateAcorns::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(1.0));
  node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_);

  nav2_util::declare_parameter_if_not_declared(
    node, behavior_name_ + ".lookahead", rclcpp::ParameterValue(0.5));
  node->get_parameter(behavior_name_ + ".lookahead", lookahead_);
}


void RotateAcorns::activateCB()
{
  auto node = node_.lock();
  using namespace std::placeholders;
  acorn_pose_sub_ = node->create_subscription<PoseStamped>(
    "closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&RotateAcorns::acorn_position_cb, this, _1));
  distance_ = 1.0;
}

void RotateAcorns::deactivateCB() { 
    acorn_pose_sub_.reset(); 
}

void RotateAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
{
  geometry_msgs::msg::PoseStamped pose_local;
  geometry_msgs::msg::PoseStamped pose_global;
  try {
    pose_local = tf_->transform(*msg, robot_base_frame_);
    pose_global = tf_->transform(*msg, global_frame_);
  } catch (const tf2::TransformException & e) {
    RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]...");
    return;
  }

  double x = pose_local.pose.position.x;
  double y = pose_local.pose.position.y;
  if (x * x + y * y > distance_ + 0.005) {
    return;
  }

  double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));

  if(tf2::getYaw(pose_local.pose.orientation) > 0) {
    yaw += M_PI;
  }

  yaw += M_PI/2;


  pose_global.pose.position.x += std::cos(yaw) * 0.0 - std::sin(yaw) * - 0.005;
  pose_global.pose.position.y += std::sin(yaw) * 0.0 + std::cos(yaw) * - 0.005;
  tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation);

  yaw = angles::normalize_angle(yaw);

  std::lock_guard _lock(mutex_);
  auto[a,b,c] = avg_.push(pose_global.pose);
  distance_ = x * x + y * y;
  new_target_angle_ = c;
  
}

ResultStatus RotateAcorns::onStart(
  const std::shared_ptr<const RotateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
  const geometry_msgs::msg::Twist & vel)
{
  std::lock_guard _lock(mutex_);
  target_angle_ = new_target_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;
  mode_ = command->mode;
  distance_ = 1.0;
  avg_.reset();

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

  geometry_msgs::msg::Pose2D pose2d;
  pose2d.x = pose.position.x;
  pose2d.y = pose.position.y;
  pose2d.theta = initial_direction_;
  local_collision_checker_->isCollisionFree(pose2d, true);

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

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

void RotateAcorns::calc_err_and_sign(
  double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign)
{
  err = angles::shortest_angular_distance(current_yaw, target_angle_);
  sign = (err < 0) ? -1.0 : 1.0;
  err = std::abs(err);

  if (min_turn_angle > 0.0) {
    const double angle_change = angles::shortest_angular_distance(last_angle, current_yaw);
    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_;
  }
}

double RotateAcorns::calc_speed(
  const double err, const double sign, const double angular_speed)
{
  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_decel_ * 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_);
  return speed;
}

ResultStatus RotateAcorns::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 min_turn_angle = min_turn_angle_;
  double angular_speed = angular_speed_;
  double err, sign;

  calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign);

  if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) {
    err = check_space(pose, err, sign);
  }

  double speed = 0.0;

  if (err != 0.0) {
    speed = calc_speed(err, sign, angular_speed);
  }

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

  min_turn_angle_ = min_turn_angle;
  last_angle_ = current_yaw;
  angular_speed_ = speed;
  out_vel.angular.z = speed;
  return ResultStatus{Status::RUNNING};
}

double RotateAcorns::check_space(
  const geometry_msgs::msg::Pose pose, const double e, const double sign)
{
  geometry_msgs::msg::Pose2D pose2d;
  pose2d.x = pose.position.x;
  pose2d.y = pose.position.y;
  double initial_theta = tf2::getYaw(pose.orientation);
  pose2d.theta = initial_theta;
  const double step_size = 0.1;
  const double err = std::min(e, 1.0);
  const bool check_map = !(mode_ & RotateAction::Goal::IGNORE_OBSTACLES);

  for (int i = 1; i < err / step_size; i++) {
    pose2d.theta += sign * step_size;

    if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
      return step_size * (i - 1);
    }

    if (check_rival_collision(pose2d)) {
      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
      return step_size * (i - 1);
    }
  }

  pose2d.theta = initial_theta + sign * err;

  if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
    return step_size * ((int)(err / step_size));
  }

  if (check_rival_collision(pose2d)) {
    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
    return step_size * ((int)(err / step_size));
  }

  return e;
}

}  // namespace toid

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