#include "racing_control/final_home_fallback.hpp"

#include <algorithm>
#include <cmath>

namespace racing_control
{

double normalizeAngle(double angle)
{
  while (angle > M_PI) {
    angle -= 2.0 * M_PI;
  }
  while (angle < -M_PI) {
    angle += 2.0 * M_PI;
  }
  return angle;
}

double yawFromPose(const geometry_msgs::msg::PoseStamped & pose)
{
  const auto & q = pose.pose.orientation;
  return std::atan2(
    2.0 * (q.w * q.z + q.x * q.y),
    1.0 - 2.0 * (q.y * q.y + q.z * q.z));
}

FinalHomeFallbackCommand computeFinalHomeFallbackCommand(
  const geometry_msgs::msg::PoseStamped & current,
  const geometry_msgs::msg::PoseStamped & home,
  const double distance_tolerance,
  const double yaw_tolerance,
  const double forward_speed,
  const double yaw_gain,
  const double max_turn_speed)
{
  FinalHomeFallbackCommand command;
  const auto dx = home.pose.position.x - current.pose.position.x;
  const auto dy = home.pose.position.y - current.pose.position.y;
  command.distance = std::hypot(dx, dy);

  if (command.distance <= distance_tolerance) {
    command.reached = true;
    return command;
  }

  const auto target_yaw = std::atan2(dy, dx);
  command.yaw_error = normalizeAngle(target_yaw - yawFromPose(current));
  command.twist.linear.x = forward_speed;

  if (std::abs(command.yaw_error) > yaw_tolerance) {
    command.twist.angular.z = command.yaw_error > 0.0 ? max_turn_speed : -max_turn_speed;
  } else {
    const auto raw_turn = command.yaw_error * yaw_gain;
    command.twist.angular.z = std::clamp(raw_turn, -max_turn_speed, max_turn_speed);
  }

  return command;
}

}  // namespace racing_control
