// Unit tests for the Social Force Model.
//
// Build (from this directory):
//   g++ -std=c++17 -O2 tests.cpp forces.cpp -lgtest -lgtest_main -lpthread -o tests
//
// Run:
//   ./tests

#include "board.hpp"
#include "forces.hpp"
#include "pedestrian.hpp"
#include "vector2d.hpp"

#include <cmath>
#include <gtest/gtest.h>

// ---------------------------------------------------------------------------
// Helpers
// ---------------------------------------------------------------------------

namespace {

// Tolerance for floating-point comparisons. The model uses exp/sqrt/division
// and chains several of them, so we leave some headroom over machine epsilon.
constexpr double kEps = 1e-9;
constexpr double kEpsLoose = 1e-6;

::testing::AssertionResult Vec2Near(const Vector2d& a, const Vector2d& b,
                                    double eps = kEps) {
  if (std::fabs(a.x - b.x) <= eps && std::fabs(a.y - b.y) <= eps) {
    return ::testing::AssertionSuccess();
  }
  return ::testing::AssertionFailure()
         << "vectors differ: got (" << a.x << ", " << a.y << "), expected ("
         << b.x << ", " << b.y << "), eps=" << eps;
}

// Build a pedestrian at `pos` heading toward +x with the given desired speed.
Pedestrian MakePedestrian(Vector2d pos, Vector2d vel, Vector2d dir,
                          double v0 = 1.34) {
  Pedestrian p;
  p.curr_pos = pos;
  p.curr_vel = vel;
  p.w = vel;
  p.e = dir;
  p.v_max = 1.3 * v0;
  p.desired_v0 = v0;
  p.destination = Vector2d(100.0, pos.y);
  p.f_i = Vector2d(0.0, 0.0);
  return p;
}

}  // namespace

// ===========================================================================
// Vector2d
// ===========================================================================

TEST(Vector2dTest, DefaultConstructorIsZero) {
  Vector2d v;
  EXPECT_DOUBLE_EQ(v.x, 0.0);
  EXPECT_DOUBLE_EQ(v.y, 0.0);
}

TEST(Vector2dTest, ValueConstructor) {
  Vector2d v(3.0, -4.0);
  EXPECT_DOUBLE_EQ(v.x, 3.0);
  EXPECT_DOUBLE_EQ(v.y, -4.0);
}

TEST(Vector2dTest, AddAndSubtract) {
  Vector2d a(1.0, 2.0);
  Vector2d b(3.0, 5.0);
  EXPECT_TRUE(Vec2Near(a + b, Vector2d(4.0, 7.0)));
  EXPECT_TRUE(Vec2Near(a - b, Vector2d(-2.0, -3.0)));
}

TEST(Vector2dTest, ScalarMulAndDiv) {
  Vector2d a(2.0, -3.0);
  EXPECT_TRUE(Vec2Near(a * 2.5, Vector2d(5.0, -7.5)));
  EXPECT_TRUE(Vec2Near(a / 2.0, Vector2d(1.0, -1.5)));
}

TEST(Vector2dTest, ScalarAddAndSub) {
  Vector2d a(2.0, -3.0);
  EXPECT_TRUE(Vec2Near(a + 1.0, Vector2d(3.0, -2.0)));
  EXPECT_TRUE(Vec2Near(a - 1.0, Vector2d(1.0, -4.0)));
}

TEST(Vector2dTest, CompoundAssignment) {
  Vector2d a(1.0, 2.0);
  a += Vector2d(0.5, -1.0);
  EXPECT_TRUE(Vec2Near(a, Vector2d(1.5, 1.0)));
}

TEST(Vector2dTest, ReverseSign) {
  Vector2d a(1.5, -2.5);
  EXPECT_TRUE(Vec2Near(a.reverse_sign(), Vector2d(-1.5, 2.5)));
  Vector2d zero(0.0, 0.0);
  EXPECT_TRUE(Vec2Near(zero.reverse_sign(), Vector2d(0.0, 0.0)));
}

TEST(Vector2dTest, NormBasic) {
  EXPECT_NEAR(Vector2d(3.0, 4.0).norm(), 5.0, kEps);
  EXPECT_NEAR(Vector2d(0.0, 0.0).norm(), 0.0, kEps);
  EXPECT_NEAR(Vector2d(-3.0, -4.0).norm(), 5.0, kEps);
}

TEST(Vector2dTest, Dot) {
  Vector2d a(1.0, 2.0);
  Vector2d b(3.0, 4.0);
  EXPECT_NEAR(a.dot(b), 11.0, kEps);
  // orthogonal
  EXPECT_NEAR(Vector2d(1.0, 0.0).dot(Vector2d(0.0, 1.0)), 0.0, kEps);
  // anti-parallel
  EXPECT_NEAR(Vector2d(1.0, 0.0).dot(Vector2d(-1.0, 0.0)), -1.0, kEps);
}

TEST(Vector2dTest, Reset) {
  Vector2d a(7.0, -8.0);
  a.reset();
  EXPECT_TRUE(Vec2Near(a, Vector2d(0.0, 0.0)));
}

// ===========================================================================
// Obstacle::find_nearest_obstacle_point
// ===========================================================================

TEST(ObstacleTest, ProjectionOntoMiddleOfSegment) {
  // Horizontal segment from (0,0) to (10,0); point above the middle.
  Obstacle obs(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
  Vector2d nearest = obs.find_nearest_obstacle_point(Vector2d(5.0, 3.0));
  EXPECT_TRUE(Vec2Near(nearest, Vector2d(5.0, 0.0)));
}

TEST(ObstacleTest, ProjectionClampedToStart) {
  // Point lies "before" the segment start -> nearest is start.
  Obstacle obs(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
  Vector2d nearest = obs.find_nearest_obstacle_point(Vector2d(-5.0, 2.0));
  EXPECT_TRUE(Vec2Near(nearest, Vector2d(0.0, 0.0)));
}

TEST(ObstacleTest, ProjectionClampedToEnd) {
  // Point lies "after" the segment end -> nearest is end.
  Obstacle obs(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
  Vector2d nearest = obs.find_nearest_obstacle_point(Vector2d(20.0, 2.0));
  EXPECT_TRUE(Vec2Near(nearest, Vector2d(10.0, 0.0)));
}

TEST(ObstacleTest, ProjectionOnDiagonalSegment) {
  // Diagonal from (0,0) to (4,4); query (4,0) projects onto (2,2).
  Obstacle obs(Vector2d(0.0, 0.0), Vector2d(4.0, 4.0));
  Vector2d nearest = obs.find_nearest_obstacle_point(Vector2d(4.0, 0.0));
  EXPECT_TRUE(Vec2Near(nearest, Vector2d(2.0, 2.0)));
}

TEST(ObstacleTest, PointOnSegmentItself) {
  Obstacle obs(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
  Vector2d nearest = obs.find_nearest_obstacle_point(Vector2d(7.0, 0.0));
  EXPECT_TRUE(Vec2Near(nearest, Vector2d(7.0, 0.0)));
}

// ===========================================================================
// Board
// ===========================================================================

TEST(BoardTest, AddsFourBorderObstacles) {
  Obstacle inner[1] = {Obstacle(Vector2d(2.0, 2.0), Vector2d(3.0, 3.0))};
  Board b(10.0, 5.0, 1, inner);

  ASSERT_EQ(b.n_obstacles, 5);
  // First entry is the user-supplied obstacle.
  EXPECT_TRUE(Vec2Near(b.obstacles[0].start, Vector2d(2.0, 2.0)));
  EXPECT_TRUE(Vec2Near(b.obstacles[0].end, Vector2d(3.0, 3.0)));

  // Borders: bottom, right, top, left.
  EXPECT_TRUE(Vec2Near(b.obstacles[1].start, Vector2d(0.0, 0.0)));
  EXPECT_TRUE(Vec2Near(b.obstacles[1].end, Vector2d(10.0, 0.0)));
  EXPECT_TRUE(Vec2Near(b.obstacles[2].start, Vector2d(10.0, 0.0)));
  EXPECT_TRUE(Vec2Near(b.obstacles[2].end, Vector2d(10.0, 5.0)));
  EXPECT_TRUE(Vec2Near(b.obstacles[3].start, Vector2d(10.0, 5.0)));
  EXPECT_TRUE(Vec2Near(b.obstacles[3].end, Vector2d(0.0, 5.0)));
  EXPECT_TRUE(Vec2Near(b.obstacles[4].start, Vector2d(0.0, 5.0)));
  EXPECT_TRUE(Vec2Near(b.obstacles[4].end, Vector2d(0.0, 0.0)));
}

TEST(BoardTest, ZeroInnerObstaclesYieldsFourBorders) {
  Board b(8.0, 4.0, 0, nullptr);
  EXPECT_EQ(b.n_obstacles, 4);
}

// ===========================================================================
// w(e, f)  --  field-of-view weighting
// ===========================================================================

TEST(WeightingTest, FullWeightWhenForceIsInForwardCone) {
  // e points along +x; force along +x is dead-ahead -> w = 1.
  Vector2d e(1.0, 0.0);
  Vector2d f(2.0, 0.0);
  EXPECT_NEAR(w(e, f), 1.0, kEps);
}

TEST(WeightingTest, ReducedWeightWhenForceIsBehind) {
  // Force points behind the pedestrian -> w = c.
  Vector2d e(1.0, 0.0);
  Vector2d f(-2.0, 0.0);
  EXPECT_NEAR(w(e, f), c, kEps);
}

TEST(WeightingTest, BoundaryAtPhi) {
  // Vector exactly at +phi (45 deg) from e: dot product equals |f|*cos(phi),
  // so the condition `>= |f|*cos(phi)` is true and weight is 1.
  Vector2d e(1.0, 0.0);
  double angle = phi;
  Vector2d f(std::cos(angle), std::sin(angle));
  EXPECT_NEAR(w(e, f), 1.0, kEps);
}

TEST(WeightingTest, JustOutsideConeReturnsC) {
  // Slightly beyond phi -> outside cone -> weight is c.
  Vector2d e(1.0, 0.0);
  double angle = phi + 1e-3;
  Vector2d f(std::cos(angle), std::sin(angle));
  EXPECT_NEAR(w(e, f), c, kEps);
}

// ===========================================================================
// g(v_max, |w|)
// ===========================================================================

TEST(GFunctionTest, ReturnsOneWhenWithinMaxSpeed) {
  EXPECT_NEAR(g(2.0, 1.5), 1.0, kEps);
  EXPECT_NEAR(g(2.0, 2.0), 1.0, kEps);  // boundary
}

TEST(GFunctionTest, ReturnsRatioWhenAboveMaxSpeed) {
  EXPECT_NEAR(g(2.0, 4.0), 0.5, kEps);
  EXPECT_NEAR(g(1.0, 5.0), 0.2, kEps);
}

// ===========================================================================
// add_acceleration_term
// ===========================================================================

TEST(AccelerationTest, ZeroWhenAtDesiredVelocity) {
  // Desired velocity = e * v0; if curr_vel matches, the term is zero.
  Pedestrian p = MakePedestrian(Vector2d(0, 0), Vector2d(1.34, 0.0),
                                Vector2d(1.0, 0.0), 1.34);
  add_acceleration_term(&p);
  EXPECT_TRUE(Vec2Near(p.f_i, Vector2d(0.0, 0.0)));
}

TEST(AccelerationTest, PointsTowardDesiredDirectionFromRest) {
  // At rest, the term should be (e*v0 - 0)/tau = (v0/tau, 0).
  Pedestrian p = MakePedestrian(Vector2d(0, 0), Vector2d(0.0, 0.0),
                                Vector2d(1.0, 0.0), 1.34);
  add_acceleration_term(&p);
  EXPECT_TRUE(Vec2Near(p.f_i, Vector2d(1.34 / tau_alpha, 0.0)));
}

TEST(AccelerationTest, AccumulatesIntoExistingForce) {
  // Pre-load f_i; verify that the function adds (does not overwrite).
  Pedestrian p = MakePedestrian(Vector2d(0, 0), Vector2d(0.0, 0.0),
                                Vector2d(1.0, 0.0), 1.34);
  p.f_i = Vector2d(0.5, -0.25);
  add_acceleration_term(&p);
  EXPECT_TRUE(Vec2Near(p.f_i, Vector2d(0.5 + 1.34 / tau_alpha, -0.25)));
}

// ===========================================================================
// add_repulsive_effects_from_pedestrians
// ===========================================================================

TEST(PedestrianRepulsionTest, NoSelfInteraction) {
  // Single pedestrian, N=1: loop body must skip self and leave f_i untouched.
  Pedestrian peds[1];
  peds[0] = MakePedestrian(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0));
  peds[0].f_i = Vector2d(0.0, 0.0);
  add_repulsive_effects_from_pedestrians(peds, &peds[0], 1);
  EXPECT_TRUE(Vec2Near(peds[0].f_i, Vector2d(0.0, 0.0)));
}

TEST(PedestrianRepulsionTest, ForceOnAlphaPushesAwayFromBeta) {
  // Beta is to the right of alpha -> force on alpha should push it left.
  Pedestrian peds[2];
  peds[0] = MakePedestrian(Vector2d(0.0, 0.0), Vector2d(0.0, 0.0),
                           Vector2d(1.0, 0.0));
  peds[1] = MakePedestrian(Vector2d(1.0, 0.0), Vector2d(0.0, 0.0),
                           Vector2d(-1.0, 0.0));
  peds[0].f_i = Vector2d(0.0, 0.0);

  add_repulsive_effects_from_pedestrians(peds, &peds[0], 2);

  // Repulsion should push alpha in the -x direction.
  EXPECT_LT(peds[0].f_i.x, 0.0);
  // By symmetry of the configuration (both stationary, beta directly in front
  // along x), the y-component should be ~0.
  EXPECT_NEAR(peds[0].f_i.y, 0.0, kEpsLoose);
}

TEST(PedestrianRepulsionTest, ForceMagnitudeDecreasesWithDistance) {
  // Same setup, two distances; the closer pair should produce a larger |f_i|.
  auto force_magnitude_at = [](double distance) {
    Pedestrian peds[2];
    peds[0] = MakePedestrian(Vector2d(0.0, 0.0), Vector2d(0.0, 0.0),
                             Vector2d(1.0, 0.0));
    peds[1] = MakePedestrian(Vector2d(distance, 0.0), Vector2d(0.0, 0.0),
                             Vector2d(-1.0, 0.0));
    peds[0].f_i = Vector2d(0.0, 0.0);
    add_repulsive_effects_from_pedestrians(peds, &peds[0], 2);
    return peds[0].f_i.norm();
  };

  EXPECT_GT(force_magnitude_at(0.5), force_magnitude_at(2.0));
  EXPECT_GT(force_magnitude_at(2.0), force_magnitude_at(5.0));
}

TEST(PedestrianRepulsionTest, SuperpositionMirrorSymmetryOnPerpendicularAxis) {
  // Two betas placed symmetrically on +y and -y of alpha, with alpha facing +x.
  // The configuration is mirror-symmetric across the x-axis (alpha's facing
  // axis) so the y-components must cancel exactly. The x-components stack
  // (both betas contribute equally and lie in alpha's forward field of view)
  // and should each push alpha in -x.
  Pedestrian peds[3];
  peds[0] = MakePedestrian(Vector2d(0.0, 0.0), Vector2d(0.0, 0.0),
                           Vector2d(1.0, 0.0));
  peds[1] = MakePedestrian(Vector2d(0.0, 1.0), Vector2d(0.0, 0.0),
                           Vector2d(0.0, -1.0));
  peds[2] = MakePedestrian(Vector2d(0.0, -1.0), Vector2d(0.0, 0.0),
                           Vector2d(0.0, 1.0));
  peds[0].f_i = Vector2d(0.0, 0.0);

  add_repulsive_effects_from_pedestrians(peds, &peds[0], 3);

  // y-components cancel by mirror symmetry across alpha's facing axis.
  EXPECT_NEAR(peds[0].f_i.y, 0.0, kEpsLoose);
  // No net x-push: r_ab is purely along ±y for both betas, and beta velocities
  // are zero, so the pedestrian repulsion has no x-component.
  EXPECT_NEAR(peds[0].f_i.x, 0.0, kEpsLoose);
}

TEST(PedestrianRepulsionTest, FieldOfViewAttenuatesBackwardNeighbor) {
  // Two betas symmetric on ±x of alpha, who is facing +x. The front beta is
  // in the FOV cone (w=1); the back beta is outside (w=c). The forces do NOT
  // cancel: alpha gets pushed backward (in -x, i.e. away from the front beta,
  // attenuated repulsion from behind).
  Pedestrian peds_front_only[2];
  peds_front_only[0] = MakePedestrian(Vector2d(0.0, 0.0), Vector2d(0.0, 0.0),
                                      Vector2d(1.0, 0.0));
  peds_front_only[1] = MakePedestrian(Vector2d(1.0, 0.0), Vector2d(0.0, 0.0),
                                      Vector2d(-1.0, 0.0));
  peds_front_only[0].f_i = Vector2d(0.0, 0.0);
  add_repulsive_effects_from_pedestrians(peds_front_only, &peds_front_only[0],
                                         2);
  double f_front = peds_front_only[0].f_i.x;  // negative

  Pedestrian peds_both[3];
  peds_both[0] = MakePedestrian(Vector2d(0.0, 0.0), Vector2d(0.0, 0.0),
                                Vector2d(1.0, 0.0));
  peds_both[1] = MakePedestrian(Vector2d(1.0, 0.0), Vector2d(0.0, 0.0),
                                Vector2d(-1.0, 0.0));
  peds_both[2] = MakePedestrian(Vector2d(-1.0, 0.0), Vector2d(0.0, 0.0),
                                Vector2d(1.0, 0.0));
  peds_both[0].f_i = Vector2d(0.0, 0.0);
  add_repulsive_effects_from_pedestrians(peds_both, &peds_both[0], 3);
  double f_both = peds_both[0].f_i.x;

  // Adding the rear beta (with weight c) pushes alpha forward by exactly
  // c * |contribution of one beta at distance 1|. The combined x-force should
  // therefore equal f_front * (1 - c).
  EXPECT_NEAR(f_both, f_front * (1.0 - c), kEpsLoose);
  // Sanity: with c < 1, the net push is still in -x but smaller in magnitude.
  EXPECT_LT(f_both, 0.0);
  EXPECT_GT(f_both, f_front);
}

// ===========================================================================
// add_repulsive_effects_from_borders
// ===========================================================================

TEST(BorderRepulsionTest, ForcePushesAwayFromNearestBorder) {
  // Empty inner-obstacle board so only the four walls act on the pedestrian.
  Board board(10.0, 10.0, 0, nullptr);
  Pedestrian p = MakePedestrian(Vector2d(0.5, 5.0), Vector2d(0.0, 0.0),
                                Vector2d(1.0, 0.0));
  p.f_i = Vector2d(0.0, 0.0);
  add_repulsive_effects_from_borders(board, &p);

  // Pedestrian is closest to the left wall, so the dominant component of the
  // border force should push in +x.
  EXPECT_GT(p.f_i.x, 0.0);
}

TEST(BorderRepulsionTest, SymmetryAtCenterCancelsOut) {
  // At the exact center of a square board, the four walls should produce
  // equal and opposite forces -> net force is ~zero.
  Board board(10.0, 10.0, 0, nullptr);
  Pedestrian p = MakePedestrian(Vector2d(5.0, 5.0), Vector2d(0.0, 0.0),
                                Vector2d(1.0, 0.0));
  p.f_i = Vector2d(0.0, 0.0);
  add_repulsive_effects_from_borders(board, &p);

  EXPECT_NEAR(p.f_i.x, 0.0, kEpsLoose);
  EXPECT_NEAR(p.f_i.y, 0.0, kEpsLoose);
}

TEST(BorderRepulsionTest, ForceDecaysWithDistance) {
  // Force on a pedestrian closer to the left wall should be larger than
  // the force on one farther from it.
  auto x_force_at = [](double x) {
    Board board(20.0, 10.0, 0, nullptr);
    Pedestrian p = MakePedestrian(Vector2d(x, 5.0), Vector2d(0.0, 0.0),
                                  Vector2d(1.0, 0.0));
    p.f_i = Vector2d(0.0, 0.0);
    add_repulsive_effects_from_borders(board, &p);
    return p.f_i.x;  // expect positive (pushed right)
  };

  // x = 0.3 is closer to the left wall than x = 1.0; the +x push should be
  // larger and clearly above what the right wall contributes back at x = 0.3.
  EXPECT_GT(x_force_at(0.3), x_force_at(1.0));
}

TEST(BorderRepulsionTest, InnerObstacleContributesForce) {
  // A wall placed right next to the pedestrian should produce a much larger
  // force than walls alone.
  auto force_norm = [](Obstacle* extra, int n_extra) {
    Board board(10.0, 10.0, n_extra, extra);
    Pedestrian p = MakePedestrian(Vector2d(5.0, 5.0), Vector2d(0.0, 0.0),
                                  Vector2d(1.0, 0.0));
    p.f_i = Vector2d(0.0, 0.0);
    add_repulsive_effects_from_borders(board, &p);
    return p.f_i.norm();
  };

  double baseline = force_norm(nullptr, 0);
  Obstacle near_wall[1] = {
      Obstacle(Vector2d(5.1, 4.0), Vector2d(5.1, 6.0))};
  double with_obstacle = force_norm(near_wall, 1);

  EXPECT_GT(with_obstacle, baseline + 1e-3);
}