#pragma once
#include "vector2d.hpp"

struct Obstacle {
  Vector2d start;
  Vector2d end;

  Obstacle() : start(0, 0), end(0, 0) {}
  Obstacle(Vector2d s, Vector2d e) : start(s), end(e) {}

  // Find the point of the obstacle that is nearest to p
  // # 15 flops
  Vector2d find_nearest_obstacle_point(Vector2d p) {

    // # 2 ADDs
    Vector2d obstacle_v = end - start;

    // # 9 flops =  2 ADDs, 2 MULTs, 1 ADD, 1 DIV, 2 MULTs, 1 ADD
    double projection = (p - start).dot(obstacle_v) / obstacle_v.dot(obstacle_v);

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

    // # 4 flops = 2 ADDs, 2 MULTs
    return start + obstacle_v * projection;

  }

};

struct Board {
  double length, width;
  Obstacle *obstacles; // array of obstacles (represented as lines) that are present on the board
  int n_obstacles;     // number of obstacles

  Board(double l, double w, int n, Obstacle *o) : length(l), width(w), n_obstacles(n + 4) {
    obstacles = new Obstacle[n + 4];
    for (int i = 0; i < n; i++) {
      obstacles[i] = o[i];
    }

    // borders of the board: bottom, right, top, left
    obstacles[n] = Obstacle(Vector2d(0, 0), Vector2d(l, 0));
    obstacles[n + 1] = Obstacle(Vector2d(l, 0), Vector2d(l, w));
    obstacles[n + 2] = Obstacle(Vector2d(l, w), Vector2d(0, w));
    obstacles[n + 3] = Obstacle(Vector2d(0, w), Vector2d(0, 0));
  }

  ~Board() { delete[] obstacles; }
};