#pragma once
#include "vector2d.hpp"

struct Obstacle {
  Vector2d start;       // First endpoint of the obstacle segment.
  Vector2d end;         // Second endpoint of the obstacle segment.
  Vector2d obstacle_v;  // Direction vector of the segment: end - start.
  double length_sq_inv; // Inverse squared length of the segment, used for projection.

  // Default constructor: creates a degenerate obstacle at the origin.
  // This is mainly used for array allocation before assigning real obstacles.
  Obstacle() : start(0, 0), end(0, 0), obstacle_v(0, 0), length_sq_inv(0.0) {}

  // Constructs an obstacle as a finite line segment from s to e.
  //
  // The segment direction and inverse squared length are precomputed and cached because
  // they are reused when projecting pedestrian positions onto the obstacle.
  //
  // @param s The first endpoint of the obstacle segment.
  // @param e The second endpoint of the obstacle segment.
  //
  Obstacle(Vector2d s, Vector2d e) : start(s), end(e) {
    obstacle_v = end - start;
    double length_sq = obstacle_v.dot(obstacle_v);
    length_sq_inv = length_sq != 0.0 ? 1.0 / length_sq : 0.0;
  }

  // Flop count: 5 ADDs, 5 MULTs
  //
  // Finds the point on this finite obstacle segment that is closest to p.
  //
  // The method projects the vector (p - start) onto the obstacle direction
  // vector, then clamps the projection factor to [0, 1] so that the returned
  // point lies on the finite segment.
  //
  // Note : If the obstacle is a single point, meaning start == end, length_sq_inv is zero
  // and the method returns start.
  //
  // @param p The point for which the closest obstacle point is computed.
  // @return The closest point on the obstacle segment to p.
  //
  Vector2d find_nearest_obstacle_point(const Vector2d &p) const {

    // # 3 ADDs, 3 MULTs
    double projection = (p - start).dot(obstacle_v) * length_sq_inv;

    if (projection < 0.0)
      projection = 0.0;
    else if (projection > 1.0)
      projection = 1.0;

    // # 2 ADDs, 2 MULTs
    return start + obstacle_v * projection;
  }
};

struct Board {
  double length;       // Board length.
  double width;        // Board width.
  Obstacle *obstacles; // Array of obstacles contained in the board.
  int n_obstacles;     // Total number of obstacles

  // Constructs a rectangular board of size length x width.
  //
  // The input array o contains n user-defined obstacles. The constructor copies
  // those obstacles and then appends the two horizontal borders of the board (bottom and top) as additional
  // obstacle segments:
  //
  //   bottom: (0, 0) -> (length, 0)
  //   top:    (0, width) -> (length, width)
  //
  // Note: only the horizontal borders are modeled as obstacles. The vertical
  // borders are left open so pedestrians can enter and leave the walkway without
  // being affected by repulsive forces from those borders. This is consistent
  // with Fig. 2 in the paper, where the horizontal walkway boundaries are
  // emphasized but the vertical ends are not shown as walls.
  //
  // @param l The board length.
  // @param w The board width.
  // @param n The number of user-defined obstacles in o.
  // @param o Array of user-defined obstacles to copy into the board obstacles array.
  //
  Board(double l, double w, int n, Obstacle *o) : length(l), width(w), n_obstacles(n + 2) {
    obstacles = new Obstacle[n + 2];

    for (int i = 0; i < n; i++) {
      obstacles[i] = o[i];
    }

    // Board borders: bottom, top.
    obstacles[n] = Obstacle(Vector2d(0, 0), Vector2d(l, 0));
    obstacles[n + 1] = Obstacle(Vector2d(0, w), Vector2d(l, w));
  }

  // Releases the dynamically allocated obstacle array.
  ~Board() { delete[] obstacles; }
};