#include "board.hpp"
#include "test_helpers.hpp"

#include <cmath>

TEST(ObstacleNearest, ProjectionInsideSegment) {
  Obstacle o(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
  Vector2d nearest = o.find_nearest_obstacle_point(Vector2d(5.0, 3.0));
  EXPECT_VEC2_NEAR(nearest, Vector2d(5.0, 0.0));
}

TEST(ObstacleNearest, ProjectionBeforeStartIsClampedToStart) {
  Obstacle o(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
  Vector2d nearest = o.find_nearest_obstacle_point(Vector2d(-5.0, 3.0));
  EXPECT_VEC2_NEAR(nearest, Vector2d(0.0, 0.0));
}

TEST(ObstacleNearest, ProjectionAfterEndIsClampedToEnd) {
  Obstacle o(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
  Vector2d nearest = o.find_nearest_obstacle_point(Vector2d(15.0, 3.0));
  EXPECT_VEC2_NEAR(nearest, Vector2d(10.0, 0.0));
}

TEST(ObstacleNearest, QueryEqualToStartReturnsStart) {
  Obstacle o(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
  EXPECT_VEC2_NEAR(o.find_nearest_obstacle_point(Vector2d(0.0, 0.0)), Vector2d(0.0, 0.0));
}

TEST(ObstacleNearest, QueryEqualToEndReturnsEnd) {
  Obstacle o(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
  EXPECT_VEC2_NEAR(o.find_nearest_obstacle_point(Vector2d(10.0, 0.0)), Vector2d(10.0, 0.0));
}

TEST(ObstacleNearest, DiagonalSegment) {
  Obstacle o(Vector2d(0.0, 0.0), Vector2d(4.0, 4.0));
  Vector2d nearest = o.find_nearest_obstacle_point(Vector2d(4.0, 0.0));
  EXPECT_VEC2_NEAR(nearest, Vector2d(2.0, 2.0));
}

TEST(ObstacleNearest, PointOnSegmentReturnsItself) {
  Obstacle o(Vector2d(1.0, 1.0), Vector2d(5.0, 9.0));
  Vector2d on_segment(3.0, 5.0);
  Vector2d nearest = o.find_nearest_obstacle_point(on_segment);
  EXPECT_VEC2_NEAR(nearest, on_segment);
}

TEST(ObstacleNearest, PointOnSinglePointObstacleReturnsStart) {
  Obstacle o(Vector2d(7.0, 8.0), Vector2d(7.0, 8.0));
  Vector2d nearest = o.find_nearest_obstacle_point(Vector2d(100.0, -50.0));
  EXPECT_VEC2_NEAR(nearest, Vector2d(7.0, 8.0));
  EXPECT_TRUE(std::isfinite(nearest.x));
  EXPECT_TRUE(std::isfinite(nearest.y));
}