// =============================================================================
//  Tests for forces.cpp
// =============================================================================

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

#include <algorithm>
#include <cmath>

namespace {

// Build a pedestrian with default testing values so each test only has to override what it actually cares about.
Pedestrian MakePed(Vector2d pos = Vector2d(0.0, 0.0), Vector2d vel = Vector2d(1.0, 0.0), Vector2d e = Vector2d(1.0, 0.0), double v_max = 1.5, double desired_v0 = 1.15, Vector2d dest = Vector2d(100.0, 0.0)) { return Pedestrian(WHITE, vel, pos, e, v_max, desired_v0, dest); }

// One Euler step, mirroring social_force_baseline.cpp:
//   w_new   = w + total_force_dt
//   v_new   = w_new * min(1, v_max / ||w_new||)
//   pos_new = pos + v_new * dt
struct StepResult {
  Vector2d w, v, pos;
};

StepResult IntegrateOneStep(const Pedestrian &p, Vector2d total_force_dt) {
  Vector2d w_new = p.w + total_force_dt;
  double w_norm = w_new.norm();
  double cap = std::min(1.0, p.v_max / (w_norm > EPS ? w_norm : EPS));
  Vector2d v_new = w_new * cap;
  Vector2d pos_new = p.curr_pos + v_new * dt;
  return {w_new, v_new, pos_new};
}

} // namespace

// =============================================================================
//  Tests for w(e, f)
// =============================================================================

TEST(WeightFn, ForceAlignedWithDirectionIsInView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 0.0), Vector2d(1.0, 0.0)), dt_neg); }

TEST(WeightFn, ForceOppositeToDirectionIsOutOfView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 0.0), Vector2d(-1.0, 0.0)), c_neg_times_dt); }

TEST(WeightFn, PerpendicularForceIsInView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 1.0), Vector2d(-1.0, 1.0)), dt_neg); }

TEST(WeightFn, ZeroForceIsInView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 0.0), Vector2d(0.0, 0.0)), dt_neg); }

TEST(WeightFn, AngleSlightlyInsidePhiIsInView) {
  Vector2d e(1.0, 0.0);
  const double angle = 99.0 / 180.0 * M_PI;
  Vector2d f(std::cos(angle), std::sin(angle));
  EXPECT_DOUBLE_EQ(w(e, f), dt_neg);
}

TEST(WeightFn, AngleSlightlyOutsidePhiIsOutOfView) {
  Vector2d e(1.0, 0.0);
  const double angle = 101.0 / 180.0 * M_PI;
  Vector2d f(std::cos(angle), std::sin(angle));
  EXPECT_DOUBLE_EQ(w(e, f), c_neg_times_dt);
}

TEST(WeightFn, ScalingForceMagnitudeDoesNotChangeView) {
  Vector2d e(1.0, 0.0);
  Vector2d f(0.7, 0.7);
  EXPECT_DOUBLE_EQ(w(e, f), w(e, f * 1000.0));
  EXPECT_DOUBLE_EQ(w(e, f), w(e, f * 0.001));
}

// =============================================================================
//  Tests for acceleration_term_dt(alpha)
// =============================================================================

TEST(AccelerationTerm, AtDesiredVelocityProducesNoForce) {
  Vector2d e(1.0, 0.0);
  double v0 = 1.5;
  Pedestrian p = MakePed(Vector2d(), e * v0, e, 2.0, v0);
  EXPECT_VEC2_NEAR_TOL(acceleration_term_dt(p), Vector2d(0.0, 0.0), 0.0);
}

TEST(AccelerationTerm, BelowDesiredVelocityAccelerates) {
  Vector2d e(1.0, 0.0);
  double v0 = 1.5;
  Pedestrian p = MakePed(Vector2d(), Vector2d(0.5, 0.0), e, 2.0, v0);
  Vector2d expected = Vector2d(2.0, 0.0) * dt;
  EXPECT_VEC2_NEAR(acceleration_term_dt(p), expected);
  EXPECT_GT(acceleration_term_dt(p).x, 0.0);
}

TEST(AccelerationTerm, AboveDesiredVelocityDecelerates) {
  Vector2d e(1.0, 0.0);
  double v0 = 1.5;
  Pedestrian p = MakePed(Vector2d(), Vector2d(1.75, 0.0), e, 2.0, v0);
  Vector2d expected = Vector2d(-0.5, 0.0) * dt;
  EXPECT_VEC2_NEAR(acceleration_term_dt(p), expected);
  EXPECT_LT(acceleration_term_dt(p).x, 0.0);
}

TEST(AccelerationTerm, MovingBackwardProducesLargerForwardForceThanMovingForward) {
  Vector2d e(1.0, 0.0);
  Pedestrian forward = MakePed(Vector2d(3.0, 0.0), Vector2d(0.5, 0.0), e, 2.0, 1.5);
  Pedestrian backward = MakePed(Vector2d(3.0, 0.0), Vector2d(-1.0, 0.0), e, 2.0, 1.5);
  EXPECT_GT(acceleration_term_dt(backward).x, acceleration_term_dt(forward).x);
}

TEST(AccelerationTerm, DiagonalWithVelocity) {
  Pedestrian p = MakePed(Vector2d(), Vector2d(0.5, -0.25), Vector2d(1.0, 1.0), 5.0, 2.0);
  EXPECT_VEC2_NEAR(acceleration_term_dt(p), Vector2d(3.0, 4.5) * dt);
}

TEST(AccelerationTerm, ZeroDesiredSpeed) {
  Pedestrian p = MakePed(Vector2d(), Vector2d(0.7, -0.3), Vector2d(1.0, 0.0), 2.0, 0.0);
  EXPECT_VEC2_NEAR(acceleration_term_dt(p), Vector2d(-1.4, 0.6) * dt);
}

TEST(AccelerationTerm, SidewaysVelocityProducesPerpendicularCorrection) {
  Pedestrian p = MakePed(Vector2d(), Vector2d(1.0, 0.5), Vector2d(1, 0), 1.3, 1.0);
  EXPECT_DOUBLE_EQ(acceleration_term_dt(p).x, 0.0);
  EXPECT_VEC2_NEAR(acceleration_term_dt(p), Vector2d(0.0, -1.0 * dt));
}

// =============================================================================
//  Tests for pedestrian_repulsion_dt(alpha, beta)
// =============================================================================

TEST(PedRepulsion, DistantPedestriansProduceNegligibleForce) {
  Pedestrian a = MakePed(Vector2d(0.0, 0.0), Vector2d(1.0, 0.0), Vector2d(1.0, 0.0));
  Pedestrian b = MakePed(Vector2d(100.0, 0.0), Vector2d(-1.2, 0.0), Vector2d(-1.0, 0.0));
  Vector2d f = pedestrian_repulsion_dt(a, b);
  EXPECT_LT(f.norm(), 1e-30);
}

TEST(PedRepulsion, CloseIsStrongerThanFar) {
  Pedestrian a = MakePed(Vector2d(0.0, 0.0), Vector2d(1.0, 0.0), Vector2d(1.0, 0.0));
  Pedestrian close = MakePed(Vector2d(2.0, 0.0), Vector2d(-1.0, 0.0), Vector2d(-1.0, 0.0));
  Pedestrian far = MakePed(Vector2d(8.0, 0.0), Vector2d(-1.0, 0.0), Vector2d(-1.0, 0.0));
  EXPECT_GT(pedestrian_repulsion_dt(a, close).norm(), pedestrian_repulsion_dt(a, far).norm());
}

TEST(PedRepulsion, StationaryBetaInFront) {
  Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
  Pedestrian b = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(-100, 0));
  Vector2d expected(-0.24971795 * dt, 0.0);
  EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), expected);
}

TEST(PedRepulsion, StationaryBetaToTheSide) {
  Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
  Pedestrian b = MakePed(Vector2d(0, 1), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(0, -100));
  Vector2d expected(0.0, -0.24971795 * dt);
  EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), expected);
}

TEST(PedRepulsion, StationaryBetaDiagonal) {
  Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
  Pedestrian b = MakePed(Vector2d(1, 1), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(-100, -100));
  Vector2d expected(-0.04439144 * dt, -0.04439144 * dt);
  EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), expected);
}

TEST(PedRepulsion, HeadOnWithMovingBeta) {
  Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(1, 0), Vector2d(1, 0), 1.3, 1.0);
  Pedestrian b = MakePed(Vector2d(3, 0), Vector2d(-1, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(-100, 0));
  EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), Vector2d((-0.02512853) * dt, 0.0));
}

TEST(PedRepulsion, OutOfViewAttenuatedByFactorC) {
  Pedestrian a_front = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
  Pedestrian b_front = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0);
  Pedestrian a_back = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
  Pedestrian b_back = MakePed(Vector2d(-1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0);

  Vector2d f_in = pedestrian_repulsion_dt(a_front, b_front);
  Vector2d f_out = pedestrian_repulsion_dt(a_back, b_back);

  EXPECT_NEAR(f_in.norm() / f_out.norm(), 1.0 / -c_neg, tolerance);
}

TEST(PedRepulsion, ForceOnAlphaPointsAwayFromBeta) {
  Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 1), 1.3, 1.0, Vector2d(100, 100));
  Pedestrian b = MakePed(Vector2d(1, 1), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0);
  Vector2d f = pedestrian_repulsion_dt(a, b);
  Vector2d to_b(1.0, 1.0);
  EXPECT_LT(f.dot(to_b), 0.0);
}

TEST(PedRepulsion, OverlappingPedestriansProduceFiniteOutput) {
  Pedestrian a = MakePed(Vector2d(2, 2), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
  Pedestrian b = MakePed(Vector2d(2, 2), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(0, 0));
  Vector2d f = pedestrian_repulsion_dt(a, b);
  EXPECT_TRUE(std::isfinite(f.x));
  EXPECT_TRUE(std::isfinite(f.y));
}

// =============================================================================
//  Tests for obstacle_repulsion_dt(alpha, B)
// =============================================================================

TEST(ObstRepulsion, DistantObstacleProducesNegligibleForce) {
  Pedestrian p = MakePed(Vector2d(0.0, 0.0), Vector2d(1.0, 0.0), Vector2d(1.0, 0.0));
  Obstacle o(Vector2d(100.0, -50.0), Vector2d(100.0, 50.0));
  EXPECT_LT(obstacle_repulsion_dt(p, o).norm(), 1e-30);
}

TEST(ObstRepulsion, PerpendicularWallBelow) {
  Pedestrian p = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
  Obstacle wall(Vector2d(0, -1), Vector2d(10, -1));
  Vector2d expected(0.0, 0.33689735 * dt);
  EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
}

TEST(ObstRepulsion, ClampedToEndpoint) {
  Pedestrian p = MakePed(Vector2d(12, 0), Vector2d(0, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(0, 0));
  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
  Vector2d expected(0.0022699965 * dt, 0.0);
  EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
}

TEST(ObstRepulsion, ClampedToStart) {
  Pedestrian p = MakePed(Vector2d(-3, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
  Vector2d expected(-0.0000152951 * dt, 0.0);
  EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
}

TEST(ObstRepulsion, OutOfViewAttenuatedByFactorC) {
  Pedestrian p = MakePed(Vector2d(5, 1), Vector2d(0, 0), Vector2d(0, 1), 1.3, 1.0, Vector2d(5, 100));
  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
  Vector2d expected(0.0, 0.33689735 * dt * -c_neg);
  EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
}

TEST(ObstRepulsion, SinglePointObstacle) {
  Pedestrian p = MakePed(Vector2d(2, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
  Obstacle pt(Vector2d(5, 0), Vector2d(5, 0));
  Vector2d expected(-0.0000152951 * dt, 0.0);
  EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, pt), expected);
}

TEST(ObstRepulsion, ForceOnAlphaPointsAwayFromObstacle) {
  Pedestrian p = MakePed(Vector2d(5, 2), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 2));
  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
  Vector2d f = obstacle_repulsion_dt(p, wall);
  EXPECT_GT(f.y, 0.0);
  EXPECT_NEAR(f.x, 0.0, 1e-12);
}

TEST(ObstRepulsion, AlphaOnObstacleProducesFiniteOutput) {
  Pedestrian p = MakePed(Vector2d(5, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
  Vector2d f = obstacle_repulsion_dt(p, wall);
  EXPECT_TRUE(std::isfinite(f.x));
  EXPECT_TRUE(std::isfinite(f.y));
  EXPECT_VEC2_NEAR(f, Vector2d(0.0, 0.0));
}

// =============================================================================
//  Integration tests
// =============================================================================

TEST(Integration, OnePedNoInteractions_AccelerationOnly) {
  Pedestrian alpha = MakePed(Vector2d(2, 3), Vector2d(0.5, 0), Vector2d(1, 0), 1.3, 1.0);

  Vector2d total = acceleration_term_dt(alpha);

  Vector2d expected_total(dt, 0.0);
  EXPECT_VEC2_NEAR(total, expected_total);

  StepResult step = IntegrateOneStep(alpha, total);
  EXPECT_VEC2_NEAR(step.w, Vector2d(0.52, 0.0));
  EXPECT_VEC2_NEAR(step.v, Vector2d(0.52, 0.0));
  EXPECT_VEC2_NEAR(step.pos, Vector2d(2.0 + 0.52 * dt, 3.0));
}

TEST(Integration, OnePedOneStationaryBeta) {
  Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
  Pedestrian beta = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(-100, 0));

  Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, beta);

  double Fa0_x = dt_times_tau_alpha_inv;
  double Fab_x = -(V0_AB / SIGMA) * std::exp(-1.0 / SIGMA) * dt;
  Vector2d expected_total(Fa0_x + Fab_x, 0.0);
  EXPECT_VEC2_NEAR(total, expected_total);

  StepResult step = IntegrateOneStep(alpha, total);
  EXPECT_VEC2_NEAR(step.w, expected_total);
  EXPECT_LT(step.w.norm(), alpha.v_max);
  EXPECT_VEC2_NEAR(step.v, expected_total);
  EXPECT_VEC2_NEAR(step.pos, expected_total * dt);
}

TEST(Integration, OnePedOneWall) {
  Pedestrian alpha = MakePed(Vector2d(0, 0.5), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));

  Vector2d total = acceleration_term_dt(alpha) + obstacle_repulsion_dt(alpha, wall);

  double Fa0_x = dt_times_tau_alpha_inv;
  double FaB_y = (U0_AB / R) * std::exp(-0.5 / R) * dt;
  Vector2d expected_total(Fa0_x, FaB_y);
  EXPECT_VEC2_NEAR(total, expected_total);

  StepResult step = IntegrateOneStep(alpha, total);
  EXPECT_VEC2_NEAR(step.w, expected_total);
  EXPECT_LT(step.w.norm(), alpha.v_max);
  EXPECT_VEC2_NEAR(step.v, expected_total);
  EXPECT_VEC2_NEAR(step.pos, Vector2d(0, 0.5) + expected_total * dt);
}

TEST(Integration, AllThreeForcesCombined) {
  Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
  Pedestrian beta = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(-100, 0));
  Obstacle wall(Vector2d(0, -1), Vector2d(10, -1));

  Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, beta) + obstacle_repulsion_dt(alpha, wall);

  double Fa0_x = dt_times_tau_alpha_inv;
  double Fab_x = -(V0_AB / SIGMA) * std::exp(-1.0 / SIGMA) * dt;
  double FaB_y = (U0_AB / R) * std::exp(-1.0 / R) * dt;
  Vector2d expected_total(Fa0_x + Fab_x, FaB_y);
  EXPECT_VEC2_NEAR(total, expected_total);

  StepResult step = IntegrateOneStep(alpha, total);
  EXPECT_VEC2_NEAR(step.w, expected_total);
  EXPECT_LT(step.w.norm(), alpha.v_max);
  EXPECT_VEC2_NEAR(step.v, expected_total);
  EXPECT_VEC2_NEAR(step.pos, expected_total * dt);
}

TEST(Integration, SymmetricStationaryBetas_VerticalForcesCancel) {
  Pedestrian alpha = MakePed(Vector2d(5, 5), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 5));
  Pedestrian b_north = MakePed(Vector2d(5, 6), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(5, 100));
  Pedestrian b_south = MakePed(Vector2d(5, 4), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(5, -100));

  Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, b_north) + pedestrian_repulsion_dt(alpha, b_south);

  Vector2d expected_total(dt_times_tau_alpha_inv, 0.0);
  EXPECT_VEC2_NEAR(total, expected_total);

  StepResult step = IntegrateOneStep(alpha, total);
  EXPECT_VEC2_NEAR(step.w, expected_total);
  EXPECT_VEC2_NEAR(step.v, expected_total);
  EXPECT_VEC2_NEAR(step.pos, Vector2d(5, 5) + expected_total * dt);
}

TEST(Integration, PedBetweenTwoParallelWalls_VerticalForcesCancel) {
  Pedestrian alpha = MakePed(Vector2d(5, 1), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 1));
  Obstacle bottom(Vector2d(0, 0), Vector2d(10, 0));
  Obstacle top(Vector2d(0, 2), Vector2d(10, 2));

  Vector2d total = acceleration_term_dt(alpha) + obstacle_repulsion_dt(alpha, bottom) + obstacle_repulsion_dt(alpha, top);

  Vector2d expected_total(dt_times_tau_alpha_inv, 0.0);
  EXPECT_VEC2_NEAR(total, expected_total);

  StepResult step = IntegrateOneStep(alpha, total);
  EXPECT_VEC2_NEAR(step.w, expected_total);
  EXPECT_VEC2_NEAR(step.v, expected_total);
  EXPECT_VEC2_NEAR(step.pos, Vector2d(5, 1) + expected_total * dt);
}

TEST(Integration, HeadOnEncounter) {
  Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(1, 0), Vector2d(1, 0), 1.3, 1.0);
  Pedestrian beta = MakePed(Vector2d(3, 0), Vector2d(-1, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(-100, 0));

  Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, beta);

  double sqrt3 = std::sqrt(3.0);
  double Fab_x = -(2.0 * V0_AB / SIGMA / sqrt3) * std::exp(-sqrt3 / SIGMA) * dt;
  Vector2d expected_total(Fab_x, 0.0);
  EXPECT_VEC2_NEAR(total, expected_total);

  StepResult step = IntegrateOneStep(alpha, total);
  Vector2d expected_w(1.0 + Fab_x, 0.0);
  EXPECT_VEC2_NEAR(step.w, expected_w);
  EXPECT_LT(step.w.norm(), alpha.v_max);
  EXPECT_VEC2_NEAR(step.v, expected_w);
  EXPECT_VEC2_NEAR(step.pos, Vector2d(0.0, 0.0) + expected_w * dt);
}

TEST(Integration, SpeedCapAppliesWhenPreferredExceedsVMax) {
  Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 100.0, Vector2d(1000, 0));

  Vector2d total = acceleration_term_dt(alpha);
  EXPECT_VEC2_NEAR(total, Vector2d(4.0, 0.0));

  StepResult step = IntegrateOneStep(alpha, total);
  EXPECT_VEC2_NEAR(step.w, Vector2d(4.0, 0.0));
  EXPECT_VEC2_NEAR(step.v, Vector2d(alpha.v_max, 0.0));
  EXPECT_VEC2_NEAR(step.pos, Vector2d(alpha.v_max * dt, 0.0));
}

TEST(Integration, FourPedestriansNoWalls_AsymmetricCrowd) {

  Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
  Pedestrian b_front = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
  Pedestrian b_behind = MakePed(Vector2d(-1, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
  Pedestrian b_north = MakePed(Vector2d(0, 1), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 1));

  Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, b_front) + pedestrian_repulsion_dt(alpha, b_behind) + pedestrian_repulsion_dt(alpha, b_north);
  const double A = (V0_AB / SIGMA) * std::exp(-1.0 / SIGMA) * dt;
  Vector2d expected_total(dt_times_tau_alpha_inv - A + -c_neg * A, -A);

  EXPECT_VEC2_NEAR(total, expected_total);

  StepResult step = IntegrateOneStep(alpha, total);

  Vector2d expected_w = expected_total;
  Vector2d expected_v = expected_w;
  Vector2d expected_pos = alpha.curr_pos + expected_v * dt;

  EXPECT_LT(expected_w.norm(), alpha.v_max);
  EXPECT_VEC2_NEAR(step.w, expected_w);
  EXPECT_VEC2_NEAR(step.v, expected_v);
  EXPECT_VEC2_NEAR(step.pos, expected_pos);
}