#include <cmath>
#include <stdexcept>

#include "gtest/gtest.h"
#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, 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, 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);
}