#ifndef RACING_CONTROL__RACING_CONTROL_HPP_
#define RACING_CONTROL__RACING_CONTROL_HPP_

#include <cmath>
#include <cctype>
#include <cstddef>
#include <iomanip>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>

#include "geometry_msgs/msg/pose_stamped.hpp"

namespace racing_control
{

enum class RouteDirection
{
  Unknown,
  Clockwise,
  Counterclockwise
};

enum class VlmCaptureMode
{
  Unknown,
  Stop,
  PassThrough
};

enum class QrTransitTarget
{
  Entry,
  PostQr
};

enum class RouteRecoveryPhase
{
  Initial,
  AfterPlanningBackup,
  AfterLocalClearReplan,
  AfterFinalBackupReplan
};

enum class RouteFailureKind
{
  Planning,
  Following
};

enum class RouteRecoveryAction
{
  Fail,
  PlanningBackup,
  ClearLocalAndReplan,
  FinalBackupReplan
};

inline RouteRecoveryAction routeRecoveryAction(
  const RouteRecoveryPhase phase,
  const RouteFailureKind failure,
  const int final_recovery_attempts,
  const int max_final_recovery_attempts)
{
  const auto finalBackupOrFail = [&]() {
      return final_recovery_attempts < max_final_recovery_attempts ?
             RouteRecoveryAction::FinalBackupReplan : RouteRecoveryAction::Fail;
    };

  if (failure == RouteFailureKind::Planning) {
    if (phase == RouteRecoveryPhase::Initial) {
      return RouteRecoveryAction::PlanningBackup;
    }
    if (phase == RouteRecoveryPhase::AfterLocalClearReplan ||
      phase == RouteRecoveryPhase::AfterFinalBackupReplan)
    {
      return finalBackupOrFail();
    }
    return RouteRecoveryAction::Fail;
  }

  if (phase == RouteRecoveryPhase::Initial ||
    phase == RouteRecoveryPhase::AfterPlanningBackup)
  {
    return RouteRecoveryAction::ClearLocalAndReplan;
  }
  if (phase == RouteRecoveryPhase::AfterLocalClearReplan ||
    phase == RouteRecoveryPhase::AfterFinalBackupReplan)
  {
    return finalBackupOrFail();
  }
  return RouteRecoveryAction::Fail;
}

inline VlmCaptureMode vlmCaptureModeFromString(const std::string & value)
{
  std::string normalized;
  normalized.reserve(value.size());
  for (const auto ch : value) {
    normalized.push_back(static_cast<char>(std::tolower(static_cast<unsigned char>(ch))));
  }

  if (normalized == "stop") {
    return VlmCaptureMode::Stop;
  }
  if (normalized == "pass_through") {
    return VlmCaptureMode::PassThrough;
  }
  return VlmCaptureMode::Unknown;
}

inline RouteDirection directionFromQrResult(const std::string & text)
{
  if (text.find("逆") != std::string::npos) {
    return RouteDirection::Counterclockwise;
  }
  if (text.find("顺") != std::string::npos) {
    return RouteDirection::Clockwise;
  }

  for (auto it = text.rbegin(); it != text.rend(); ++it) {
    const auto ch = static_cast<unsigned char>(*it);
    if (std::isdigit(ch)) {
      return ((*it - '0') % 2 == 1) ? RouteDirection::Clockwise :
             RouteDirection::Counterclockwise;
    }
  }
  return RouteDirection::Unknown;
}

inline bool shouldAcceptQrDirection(
  const RouteDirection current_direction, const RouteDirection incoming_direction)
{
  return current_direction == RouteDirection::Unknown &&
         incoming_direction != RouteDirection::Unknown;
}

inline QrTransitTarget qrTransitTargetAfterRecognition(const bool use_post_qr_pose)
{
  return use_post_qr_pose ? QrTransitTarget::PostQr : QrTransitTarget::Entry;
}

inline std::string routeCandidateSuffix(const RouteDirection direction)
{
  if (direction == RouteDirection::Clockwise) {
    return "_1";
  }
  if (direction == RouteDirection::Counterclockwise) {
    return "_2";
  }
  return "";
}

inline std::string routeCandidateGroupName(
  const std::size_t route_waypoint_index,
  const RouteDirection direction)
{
  const auto suffix = routeCandidateSuffix(direction);
  if (suffix.empty()) {
    return "";
  }

  std::ostringstream out;
  out << "goal_" << std::setw(3) << std::setfill('0') << (route_waypoint_index + 1) << suffix;
  return out.str();
}

inline bool shouldPublishVlmImageFrame(
  const bool enable_vlm_image_relay, const bool has_latest_image)
{
  return enable_vlm_image_relay && has_latest_image;
}

template<typename WaitForActionServerFn>
inline bool retryActionServerWait(
  WaitForActionServerFn && wait_for_action_server,
  const int retry_count)
{
  for (int attempt = 0; attempt <= retry_count; ++attempt) {
    if (wait_for_action_server()) {
      return true;
    }
  }
  return false;
}

inline bool shouldRetryNavigateGoal(const bool succeeded, const int retries_remaining)
{
  return !succeeded && retries_remaining > 0;
}

inline int defaultStartupQrEnableRepeats()
{
  return 5;
}

inline int defaultStartupQrEnableIntervalMs()
{
  return 200;
}

inline bool shouldDynamicReplan(
  const bool enabled,
  const bool replan_in_flight,
  const double distance_to_goal,
  const double stop_distance,
  const double elapsed_since_last_replan,
  const double replan_interval)
{
  return enabled && !replan_in_flight && distance_to_goal > stop_distance &&
         elapsed_since_last_replan >= replan_interval;
}

inline bool recoveryBackupComplete(
  const double backup_distance,
  const double target_distance,
  const double elapsed_sec,
  const double timeout_sec)
{
  return backup_distance >= target_distance || elapsed_sec >= timeout_sec;
}

inline bool defaultUseTrajectoryGuard()
{
  return false;
}

inline geometry_msgs::msg::PoseStamped poseFromXYYaw(
  const double x, const double y, const double yaw, const std::string & frame_id)
{
  geometry_msgs::msg::PoseStamped pose;
  pose.header.frame_id = frame_id;
  pose.pose.position.x = x;
  pose.pose.position.y = y;
  pose.pose.position.z = 0.0;
  pose.pose.orientation.z = std::sin(yaw * 0.5);
  pose.pose.orientation.w = std::cos(yaw * 0.5);
  return pose;
}

inline double poseDistance2d(
  const geometry_msgs::msg::PoseStamped & a,
  const geometry_msgs::msg::PoseStamped & b)
{
  const auto dx = a.pose.position.x - b.pose.position.x;
  const auto dy = a.pose.position.y - b.pose.position.y;
  return std::hypot(dx, dy);
}

inline std::size_t advanceReachedWaypointIndex(
  const std::vector<geometry_msgs::msg::PoseStamped> & waypoints,
  const geometry_msgs::msg::PoseStamped & current,
  const std::size_t next_waypoint_index,
  const double tolerance)
{
  auto next = next_waypoint_index;
  while (next < waypoints.size() && poseDistance2d(current, waypoints[next]) <= tolerance) {
    ++next;
  }
  return next;
}

inline std::vector<geometry_msgs::msg::PoseStamped> remainingWaypoints(
  const std::vector<geometry_msgs::msg::PoseStamped> & waypoints,
  const std::size_t next_waypoint_index)
{
  if (next_waypoint_index >= waypoints.size()) {
    return {};
  }
  return {waypoints.begin() + static_cast<std::ptrdiff_t>(next_waypoint_index), waypoints.end()};
}

inline std::vector<geometry_msgs::msg::PoseStamped> remainingWaypointsAfterProgress(
  const std::vector<geometry_msgs::msg::PoseStamped> & waypoints,
  const geometry_msgs::msg::PoseStamped & current,
  const std::size_t next_waypoint_index,
  const double tolerance)
{
  return remainingWaypoints(
    waypoints,
    advanceReachedWaypointIndex(waypoints, current, next_waypoint_index, tolerance));
}

inline std::vector<geometry_msgs::msg::PoseStamped> routeWaypointsAfterQr(
  const geometry_msgs::msg::PoseStamped & entry_pose,
  const std::vector<geometry_msgs::msg::PoseStamped> & route_waypoints,
  const std::size_t vlm_waypoint_index,
  const bool split_at_vlm)
{
  std::vector<geometry_msgs::msg::PoseStamped> segment;
  segment.reserve(route_waypoints.size() + 1);
  segment.push_back(entry_pose);

  std::size_t route_end = route_waypoints.size();
  if (split_at_vlm && vlm_waypoint_index + 1 < route_end) {
    route_end = vlm_waypoint_index + 1;
  }
  segment.insert(
    segment.end(), route_waypoints.begin(), route_waypoints.begin() +
    static_cast<std::ptrdiff_t>(route_end));
  return segment;
}

inline std::vector<geometry_msgs::msg::PoseStamped> posesFromFlatDoubles(
  const std::vector<double> & values, const std::string & frame_id)
{
  if (values.size() % 3 != 0) {
    throw std::invalid_argument("pose arrays must contain x/y/yaw triples");
  }

  std::vector<geometry_msgs::msg::PoseStamped> poses;
  poses.reserve(values.size() / 3);
  for (std::size_t i = 0; i < values.size(); i += 3) {
    poses.push_back(poseFromXYYaw(values[i], values[i + 1], values[i + 2], frame_id));
  }
  return poses;
}

}  // namespace racing_control

#endif  // RACING_CONTROL__RACING_CONTROL_HPP_
