#ifndef RACING_CONTROL__FINAL_HOME_FALLBACK_HPP_
#define RACING_CONTROL__FINAL_HOME_FALLBACK_HPP_

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"

namespace racing_control
{

struct FinalHomeFallbackCommand
{
  geometry_msgs::msg::Twist twist;
  double distance{0.0};
  double yaw_error{0.0};
  bool reached{false};
};

double normalizeAngle(double angle);

double yawFromPose(const geometry_msgs::msg::PoseStamped & pose);

FinalHomeFallbackCommand computeFinalHomeFallbackCommand(
  const geometry_msgs::msg::PoseStamped & current,
  const geometry_msgs::msg::PoseStamped & home,
  double distance_tolerance,
  double yaw_tolerance,
  double forward_speed,
  double yaw_gain,
  double max_turn_speed);

}  // namespace racing_control

#endif  // RACING_CONTROL__FINAL_HOME_FALLBACK_HPP_
