#include "toid_behaviors/scl.hpp"

#include <cmath>

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


namespace toid {
void SmoothControlLaw::calculate_vel(
  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
  geometry_msgs::msg::Twist &out_speed, bool backwards)
{
  double r, phi, delta;
  egocentric_polar(target, current, backwards, r, phi, delta);

  double curvature = this->curvature(r, phi, delta);
  curvature = backwards? -curvature : curvature;

  double v = v_linear_max / (1.0 + bbeta * std::pow(fabs(curvature), lam));

  v = std::min(v_linear_max * (r / slowdown_radius), v);
  v = std::clamp(v, v_linear_min, v_linear_max);
  v = backwards ? -v : v;

  const double w = curvature * v;
  const double w_bound = std::clamp(w, -v_angular_max, v_angular_max);

  v = (curvature != 0.0) ? (w_bound / curvature) : v;
  out_speed.linear.x = v;
  out_speed.angular.z = w_bound;
}

double SmoothControlLaw::curvature(double r, double phi, double delta)
{
  if (r < 1e-6) {
    return 0.0;
  }
  const float prop = k_delta * (delta - std::atan(-k_phi * phi));
  const float feedback = (1 + k_phi / (1 + (k_phi * phi) * (k_phi * phi))) * std::sin(delta);

  return -1.0 / r * (prop + feedback);
}

void SmoothControlLaw::egocentric_polar(
  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards, double & r,
  double & phi, double & delta)
{
  const double dx = target.position.x - current.position.x;
  const double dy = target.position.y - current.position.y;
  const double los = backwards ? (std::atan2(-dy, dx) + M_PI) : std::atan2(-dy, dx);

  const double ttheta = tf2::getYaw(target.orientation);
  const double ctheta = tf2::getYaw(current.orientation);

  r = std::hypot(dx, dy);
  phi = angles::normalize_angle(ttheta + los);
  delta = angles::normalize_angle(ctheta + los);
}

}