#include <gtest/gtest.h>

#include <limits>
#include <vector>

#include "origincar_birdseye/ground_projection.hpp"
#include "origincar_birdseye/birdseye_geometry.hpp"
#include "origincar_birdseye/configuration_validation.hpp"
#include "origincar_birdseye/image_validation.hpp"

namespace origincar_birdseye
{

TEST(GroundProjection, ProjectsGroundPointFromVehicleCoordinates)
{
  CameraIntrinsics intrinsics{800.0, 800.0, 400.0, 225.0, 800, 450};
  RigidTransform camera_from_base(
    {0.0, -1.0, 0.0,
      0.0, 0.0, -1.0,
      1.0, 0.0, 0.0},
    {0.0, 0.0, 0.0});
  GroundProjection projection(intrinsics, camera_from_base);

  const auto pixel = projection.projectBaseGroundPoint({1.0, 0.0});

  ASSERT_TRUE(pixel.has_value());
  EXPECT_NEAR(pixel->x, 400.0, 1e-6);
  EXPECT_NEAR(pixel->y, 225.0, 1e-6);
}

}  // namespace origincar_birdseye

namespace origincar_birdseye
{

TEST(BirdseyeGeometry, KeepsVehicleAtConfiguredCanvasPosition)
{
  BirdseyeGeometry geometry({600, 800, 0.01, 2.0, 3.0});

  const auto vehicle = geometry.basePointToCanvasPixel({0.0, 0.0});
  const auto one_meter_ahead = geometry.basePointToCanvasPixel({1.0, 0.0});

  ASSERT_TRUE(vehicle.has_value());
  ASSERT_TRUE(one_meter_ahead.has_value());
  EXPECT_NEAR(vehicle->x, 300.0, 1e-6);
  EXPECT_NEAR(vehicle->y, 200.0, 1e-6);
  EXPECT_NEAR(one_meter_ahead->x, 300.0, 1e-6);
  EXPECT_NEAR(one_meter_ahead->y, 100.0, 1e-6);
}

TEST(BirdseyeGeometry, PreservesPointsOutsideCanvasForHomography)
{
  BirdseyeGeometry geometry({600, 800, 0.01, 2.0, 3.0});

  const auto four_meters_ahead = geometry.basePointToCanvasPixel({4.0, 2.0});

  ASSERT_TRUE(four_meters_ahead.has_value());
  EXPECT_NEAR(four_meters_ahead->x, 100.0, 1e-6);
  EXPECT_NEAR(four_meters_ahead->y, -200.0, 1e-6);
}

TEST(ConfigurationValidation, RejectsInvalidCanvas)
{
  const auto zero_scale = validateCanvas({600, 800, 0.0, 2.0, 3.0}, "vehicle");
  const auto invalid_width = validateCanvas({0, 800, 0.01, 2.0, 3.0}, "vehicle");

  EXPECT_TRUE(zero_scale.has_value());
  EXPECT_TRUE(invalid_width.has_value());
}

TEST(ConfigurationValidation, RejectsNonFiniteCameraParameters)
{
  CameraIntrinsics intrinsics{800.0, 800.0, 400.0, 225.0, 800, 450};
  std::vector<double> rotation{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
    std::numeric_limits<double>::quiet_NaN()};
  const std::vector<double> translation{0.0, 0.0, 0.2};

  EXPECT_TRUE(validateCameraConfiguration(intrinsics, rotation, translation).has_value());
}

}  // namespace origincar_birdseye

namespace origincar_birdseye
{

TEST(ImageValidation, AcceptsConfiguredInputSize)
{
  EXPECT_FALSE(validateImageSize(1280, 720, 1280, 720).has_value());
}

TEST(ImageValidation, ReportsConfiguredAndActualInputSizes)
{
  const auto error = validateImageSize(1280, 720, 800, 450);

  ASSERT_TRUE(error.has_value());
  EXPECT_NE(error->find("expected 1280x720"), std::string::npos);
  EXPECT_NE(error->find("received 800x450"), std::string::npos);
}

TEST(ImageValidation, AcceptsRawAndCompressedMessageTypes)
{
  EXPECT_FALSE(validateImageMessageType("raw").has_value());
  EXPECT_FALSE(validateImageMessageType("compressed").has_value());
}

TEST(ImageValidation, RejectsUnknownMessageType)
{
  const auto error = validateImageMessageType("mjpeg");

  ASSERT_TRUE(error.has_value());
  EXPECT_NE(error->find("raw or compressed"), std::string::npos);
}

}  // namespace origincar_birdseye
