#include <cmath>
#include <filesystem>
#include <fstream>
#include <stdexcept>

#include "gtest/gtest.h"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "racing_control/candidate_waypoint_selector.hpp"
#include "racing_control/final_home_fallback.hpp"
#include "racing_control/racing_control.hpp"

namespace
{

constexpr double kTolerance = 1e-6;

}  // namespace

TEST(RacingControlHelpers, ConvertsFlatTriplesToStampedPoses)
{
  const auto poses = racing_control::posesFromFlatDoubles(
    {1.0, 2.0, M_PI_2, -0.5, 0.25, -M_PI},
    "odom");

  ASSERT_EQ(poses.size(), 2U);
  EXPECT_EQ(poses[0].header.frame_id, "odom");
  EXPECT_DOUBLE_EQ(poses[0].pose.position.x, 1.0);
  EXPECT_DOUBLE_EQ(poses[0].pose.position.y, 2.0);
  EXPECT_NEAR(poses[0].pose.orientation.z, std::sin(M_PI_4), kTolerance);
  EXPECT_NEAR(poses[0].pose.orientation.w, std::cos(M_PI_4), kTolerance);
  EXPECT_DOUBLE_EQ(poses[1].pose.position.x, -0.5);
  EXPECT_DOUBLE_EQ(poses[1].pose.position.y, 0.25);
}

TEST(RacingControlHelpers, RejectsIncompletePoseTriples)
{
  EXPECT_THROW(
    racing_control::posesFromFlatDoubles({1.0, 2.0, 0.0, 3.0}, "odom"),
    std::invalid_argument);
}

TEST(RacingControlHelpers, ParsesQrDirectionFromText)
{
  EXPECT_EQ(
    racing_control::directionFromQrResult("7 顺时针"),
    racing_control::RouteDirection::Clockwise);
  EXPECT_EQ(
    racing_control::directionFromQrResult("8 逆时针"),
    racing_control::RouteDirection::Counterclockwise);
  EXPECT_EQ(
    racing_control::directionFromQrResult("5"),
    racing_control::RouteDirection::Clockwise);
  EXPECT_EQ(
    racing_control::directionFromQrResult("6"),
    racing_control::RouteDirection::Counterclockwise);
  EXPECT_EQ(
    racing_control::directionFromQrResult("未识别"),
    racing_control::RouteDirection::Unknown);
}

TEST(RacingControlHelpers, LatchesFirstKnownQrDirection)
{
  EXPECT_TRUE(
    racing_control::shouldAcceptQrDirection(
      racing_control::RouteDirection::Unknown,
      racing_control::RouteDirection::Clockwise));
  EXPECT_FALSE(
    racing_control::shouldAcceptQrDirection(
      racing_control::RouteDirection::Unknown,
      racing_control::RouteDirection::Unknown));
  EXPECT_FALSE(
    racing_control::shouldAcceptQrDirection(
      racing_control::RouteDirection::Clockwise,
      racing_control::RouteDirection::Clockwise));
  EXPECT_FALSE(
    racing_control::shouldAcceptQrDirection(
      racing_control::RouteDirection::Clockwise,
      racing_control::RouteDirection::Counterclockwise));
}

TEST(RacingControlHelpers, SelectsPostQrTransitTargetWhenEnabled)
{
  EXPECT_EQ(
    racing_control::qrTransitTargetAfterRecognition(true),
    racing_control::QrTransitTarget::PostQr);
  EXPECT_EQ(
    racing_control::qrTransitTargetAfterRecognition(false),
    racing_control::QrTransitTarget::Entry);
}

TEST(RacingControlHelpers, BuildsDirectionSpecificRouteCandidateNames)
{
  EXPECT_EQ(
    racing_control::routeCandidateSuffix(racing_control::RouteDirection::Clockwise),
    "_1");
  EXPECT_EQ(
    racing_control::routeCandidateSuffix(racing_control::RouteDirection::Counterclockwise),
    "_2");
  EXPECT_EQ(
    racing_control::routeCandidateGroupName(0, racing_control::RouteDirection::Clockwise),
    "goal_001_1");
  EXPECT_EQ(
    racing_control::routeCandidateGroupName(1, racing_control::RouteDirection::Clockwise),
    "goal_002_1");
  EXPECT_EQ(
    racing_control::routeCandidateGroupName(1, racing_control::RouteDirection::Counterclockwise),
    "goal_002_2");
  EXPECT_EQ(
    racing_control::routeCandidateGroupName(11, racing_control::RouteDirection::Counterclockwise),
    "goal_012_2");
  EXPECT_TRUE(
    racing_control::routeCandidateGroupName(0, racing_control::RouteDirection::Unknown).empty());
}

TEST(RacingControlHelpers, PublishesOneVlmImageFrameOnlyWhenEnabledAndAvailable)
{
  EXPECT_TRUE(racing_control::shouldPublishVlmImageFrame(true, true));
  EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, true));
  EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(true, false));
  EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, false));
}

TEST(RacingControlHelpers, DefaultsToPureNav2RouteExecution)
{
  EXPECT_FALSE(racing_control::defaultUseTrajectoryGuard());
}

TEST(RacingControlHelpers, RetriesActionServerWaitFourTimesBeforeFailing)
{
  int wait_calls = 0;
  const bool result = racing_control::retryActionServerWait(
    [&]() {
      ++wait_calls;
      return false;
    },
    3);

  EXPECT_FALSE(result);
  EXPECT_EQ(wait_calls, 4);
}

TEST(RacingControlHelpers, RetriesNavigateGoalOnlyAfterFailure)
{
  EXPECT_TRUE(racing_control::shouldRetryNavigateGoal(false, 3));
  EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(true, 3));
  EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(false, 0));
}

TEST(RacingControlHelpers, DefaultsToStartupQrEnableBurst)
{
  EXPECT_EQ(racing_control::defaultStartupQrEnableRepeats(), 5);
  EXPECT_EQ(racing_control::defaultStartupQrEnableIntervalMs(), 200);
}

TEST(RacingControlHelpers, DynamicReplanningStopsNearGoal)
{
  EXPECT_TRUE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 1.0, 1.0));
  EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.4, 0.5, 1.0, 1.0));
  EXPECT_FALSE(racing_control::shouldDynamicReplan(true, true, 0.6, 0.5, 1.0, 1.0));
  EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 0.5, 1.0));
  EXPECT_FALSE(racing_control::shouldDynamicReplan(false, false, 0.6, 0.5, 1.0, 1.0));
}

TEST(RacingControlHelpers, AdvancesNextWaypointOnlyAfterItIsReached)
{
  const auto waypoints = racing_control::posesFromFlatDoubles(
    {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0},
    "map");

  EXPECT_EQ(
    racing_control::advanceReachedWaypointIndex(
      waypoints, racing_control::poseFromXYYaw(0.40, 0.0, 0.0, "map"), 1, 0.50),
    1U);
  EXPECT_EQ(
    racing_control::advanceReachedWaypointIndex(
      waypoints, racing_control::poseFromXYYaw(1.05, 0.0, 0.0, "map"), 1, 0.50),
    2U);
}

TEST(RacingControlHelpers, DynamicReplanningKeepsRemainingUnreachedWaypoints)
{
  const auto waypoints = racing_control::posesFromFlatDoubles(
    {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0},
    "map");

  const auto remaining = racing_control::remainingWaypoints(waypoints, 1);

  ASSERT_EQ(remaining.size(), 2U);
  EXPECT_DOUBLE_EQ(remaining[0].pose.position.x, 1.0);
  EXPECT_DOUBLE_EQ(remaining[1].pose.position.x, 2.0);
  EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size()).empty());
  EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size() + 1).empty());
}

TEST(RacingControlHelpers, TrimsPassedWaypointsBeforeReplanning)
{
  const auto waypoints = racing_control::posesFromFlatDoubles(
    {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0},
    "map");

  const auto trimmed = racing_control::remainingWaypointsAfterProgress(
    waypoints, racing_control::poseFromXYYaw(1.02, 0.0, 0.0, "map"), 1, 0.50);

  ASSERT_EQ(trimmed.size(), 1U);
  EXPECT_DOUBLE_EQ(trimmed[0].pose.position.x, 2.0);
}

TEST(RacingControlHelpers, BuildsQrToVlmSegmentThroughEntry)
{
  const std::string frame({'m', 'a', 'p'});
  const auto entry = racing_control::poseFromXYYaw(10.0, 0.0, 0.0, frame);
  const auto route = racing_control::posesFromFlatDoubles(
    {1.0, 0.0, 0.0, 2.0, 0.0, 0.0, 3.0, 0.0, 0.0},
    frame);

  const auto segment = racing_control::routeWaypointsAfterQr(entry, route, 1, true);

  ASSERT_EQ(segment.size(), 3U);
  EXPECT_DOUBLE_EQ(segment[0].pose.position.x, 10.0);
  EXPECT_DOUBLE_EQ(segment[1].pose.position.x, 1.0);
  EXPECT_DOUBLE_EQ(segment[2].pose.position.x, 2.0);
}

TEST(RacingControlHelpers, CanConnectQrThroughAllRemainingRoute)
{
  const std::string frame({'m', 'a', 'p'});
  const auto entry = racing_control::poseFromXYYaw(10.0, 0.0, 0.0, frame);
  const auto route = racing_control::posesFromFlatDoubles(
    {1.0, 0.0, 0.0, 2.0, 0.0, 0.0, 3.0, 0.0, 0.0},
    frame);

  const auto segment = racing_control::routeWaypointsAfterQr(entry, route, 1, false);

  ASSERT_EQ(segment.size(), 4U);
  EXPECT_DOUBLE_EQ(segment[0].pose.position.x, 10.0);
  EXPECT_DOUBLE_EQ(segment[1].pose.position.x, 1.0);
  EXPECT_DOUBLE_EQ(segment[2].pose.position.x, 2.0);
  EXPECT_DOUBLE_EQ(segment[3].pose.position.x, 3.0);
}

TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout)
{
  EXPECT_TRUE(racing_control::recoveryBackupComplete(0.04, 0.04, 0.1, 0.2));
  EXPECT_TRUE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.2, 0.2));
  EXPECT_FALSE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.1, 0.2));
}

TEST(RacingControlHelpers, InitialPlanningFailureUsesOnePlanningBackup)
{
  EXPECT_EQ(
    racing_control::routeRecoveryAction(
      racing_control::RouteRecoveryPhase::Initial,
      racing_control::RouteFailureKind::Planning, 0, 2),
    racing_control::RouteRecoveryAction::PlanningBackup);
  EXPECT_EQ(
    racing_control::routeRecoveryAction(
      racing_control::RouteRecoveryPhase::AfterPlanningBackup,
      racing_control::RouteFailureKind::Planning, 0, 2),
    racing_control::RouteRecoveryAction::Fail);
}

TEST(RacingControlHelpers, FollowFailureClearsOnlyLocalCostmapBeforeReplanning)
{
  EXPECT_EQ(
    racing_control::routeRecoveryAction(
      racing_control::RouteRecoveryPhase::Initial,
      racing_control::RouteFailureKind::Following, 0, 2),
    racing_control::RouteRecoveryAction::ClearLocalAndReplan);
}

TEST(RacingControlHelpers, FailedLocalClearReplanUsesFinalBackup)
{
  EXPECT_EQ(
    racing_control::routeRecoveryAction(
      racing_control::RouteRecoveryPhase::AfterLocalClearReplan,
      racing_control::RouteFailureKind::Planning, 0, 2),
    racing_control::RouteRecoveryAction::FinalBackupReplan);
  EXPECT_EQ(
    racing_control::routeRecoveryAction(
      racing_control::RouteRecoveryPhase::AfterLocalClearReplan,
      racing_control::RouteFailureKind::Following, 0, 2),
    racing_control::RouteRecoveryAction::FinalBackupReplan);
}

TEST(RacingControlHelpers, FinalRecoveryFailsAfterConfiguredAttemptLimit)
{
  EXPECT_EQ(
    racing_control::routeRecoveryAction(
      racing_control::RouteRecoveryPhase::AfterFinalBackupReplan,
      racing_control::RouteFailureKind::Following, 1, 2),
    racing_control::RouteRecoveryAction::FinalBackupReplan);
  EXPECT_EQ(
    racing_control::routeRecoveryAction(
      racing_control::RouteRecoveryPhase::AfterFinalBackupReplan,
      racing_control::RouteFailureKind::Planning, 2, 2),
    racing_control::RouteRecoveryAction::Fail);
}

TEST(RacingControlHelpers, ParsesVlmCaptureMode)
{
  EXPECT_EQ(
    racing_control::vlmCaptureModeFromString("stop"),
    racing_control::VlmCaptureMode::Stop);
  EXPECT_EQ(
    racing_control::vlmCaptureModeFromString("pass_through"),
    racing_control::VlmCaptureMode::PassThrough);
  EXPECT_EQ(
    racing_control::vlmCaptureModeFromString("unknown"),
    racing_control::VlmCaptureMode::Unknown);
}

TEST(CandidateWaypointSelector, LoadsNamedCandidatesFromSavedPointJson)
{
  const auto dir = std::filesystem::temp_directory_path() / "racing_control_candidates_test";
  std::filesystem::remove_all(dir);
  std::filesystem::create_directories(dir);
  {
    std::ofstream json(dir / "main.json");
    json <<
      R"json({
      "points": [
        {
          "name": "qr",
          "yaw_degrees": 90.0,
          "odom": {
            "pose": {
              "pose": {
                "position": {"x": 1.0, "y": 2.0, "z": 0.0},
                "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
              }
            }
          }
        },
        {
          "name": "entry",
          "yaw_degrees": 0.0,
          "odom": {
            "pose": {
              "pose": {
                "position": {"x": 3.0, "y": 4.0, "z": 0.0},
                "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
              }
            }
          }
        }
      ]
    })json";
  }

  const auto groups = racing_control::loadCandidateWaypointGroups(dir.string(), "odom");

  ASSERT_EQ(groups.size(), 2U);
  ASSERT_EQ(groups.at("qr").size(), 1U);
  EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.x, 1.0);
  EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.y, 2.0);
  EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.z, std::sin(M_PI_4), kTolerance);
  EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.w, std::cos(M_PI_4), kTolerance);
  EXPECT_EQ(groups.at("entry")[0].source_file, "main.json");
}

TEST(CandidateWaypointSelector, SkipsOccupiedCandidates)
{
  nav_msgs::msg::OccupancyGrid costmap;
  costmap.info.resolution = 1.0;
  costmap.info.width = 5;
  costmap.info.height = 5;
  costmap.info.origin.position.x = 0.0;
  costmap.info.origin.position.y = 0.0;
  costmap.info.origin.orientation.w = 1.0;
  costmap.data.assign(25, 0);
  costmap.data[1 * 5 + 1] = 80;

  racing_control::CandidateWaypointSelector selector;
  selector.setGroups(
  {
    {"qr", {
        {"qr", "blocked.json", "qr_blocked", racing_control::poseFromXYYaw(1.2, 1.2, 0.0, "odom")},
        {"qr", "free.json", "qr_free", racing_control::poseFromXYYaw(3.2, 1.2, 0.0, "odom")},
      }},
  });
  selector.setCostmap(costmap);

  const auto selected = selector.select("qr", 50, false);

  ASSERT_TRUE(selected.has_value());
  EXPECT_EQ(selected->source_file, "free.json");
  EXPECT_EQ(selected->point_name, "qr_free");
}

TEST(CandidateWaypointSelector, FindsNearestFreeRecoveryCommand)
{
  nav_msgs::msg::OccupancyGrid costmap;
  costmap.info.resolution = 0.25;
  costmap.info.width = 9;
  costmap.info.height = 9;
  costmap.info.origin.position.x = -1.0;
  costmap.info.origin.position.y = -1.0;
  costmap.info.origin.orientation.w = 1.0;
  costmap.data.assign(81, 100);
  const auto set_free = [&](const int mx, const int my) {
      costmap.data[my * static_cast<int>(costmap.info.width) + mx] = 0;
    };
  set_free(6, 4);
  set_free(7, 4);

  const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
  const auto command = racing_control::nearestFreeRecoveryCommand(
    costmap, current, 50, false, 1.0);

  ASSERT_TRUE(command.has_value());
  EXPECT_GT(command->linear_x, 0.0);
  EXPECT_DOUBLE_EQ(command->angular_z, 0.0);
}

TEST(FinalHomeFallback, NormalizesAnglesToShortestRotation)
{
  EXPECT_NEAR(racing_control::normalizeAngle(3.5), -2.7831853071795862, 1e-6);
  EXPECT_NEAR(racing_control::normalizeAngle(-3.5), 2.7831853071795862, 1e-6);
  EXPECT_NEAR(racing_control::normalizeAngle(0.25), 0.25, 1e-6);
}

TEST(FinalHomeFallback, StopsInsideDistanceTolerance)
{
  const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
  const auto home = racing_control::poseFromXYYaw(0.1, 0.1, 0.0, "odom");

  const auto command = racing_control::computeFinalHomeFallbackCommand(
    current, home, 0.2, 0.3, 0.12, 0.5, 0.7);

  EXPECT_TRUE(command.reached);
  EXPECT_DOUBLE_EQ(command.twist.linear.x, 0.0);
  EXPECT_DOUBLE_EQ(command.twist.angular.z, 0.0);
}

TEST(FinalHomeFallback, TurnsTowardHomeWhenYawErrorIsLarge)
{
  const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
  const auto home = racing_control::poseFromXYYaw(0.0, 1.0, 0.0, "odom");

  const auto command = racing_control::computeFinalHomeFallbackCommand(
    current, home, 0.2, 0.3, 0.12, 0.5, 0.7);

  EXPECT_FALSE(command.reached);
  EXPECT_NEAR(command.distance, 1.0, 1e-6);
  EXPECT_NEAR(command.yaw_error, M_PI_2, 1e-6);
  EXPECT_DOUBLE_EQ(command.twist.linear.x, 0.12);
  EXPECT_DOUBLE_EQ(command.twist.angular.z, 0.7);
}

TEST(FinalHomeFallback, DrivesForwardWhenYawErrorIsSmall)
{
  const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
  const auto home = racing_control::poseFromXYYaw(1.0, 0.1, 0.0, "odom");

  const auto command = racing_control::computeFinalHomeFallbackCommand(
    current, home, 0.2, 0.3, 0.12, 0.5, 0.7);

  EXPECT_FALSE(command.reached);
  EXPECT_GT(command.twist.linear.x, 0.0);
  EXPECT_NEAR(command.twist.angular.z, command.yaw_error * 0.5, 1e-6);
}
