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

#define BOARD_WIDTH 10.0
#define BOARD_LENGTH 50.0

using namespace std;

// Numer of pedestrians
// $n_p$
// const int n_p = 100;

// Number of iterations
// $n_{iter}$
// const int n_iter = 100;

Pedestrian *init_pedestrians(int n_p) {
  Pedestrian *P = new Pedestrian[n_p];
  std::random_device rd;                                     // use for truly random simulation
  std::mt19937 gen(rd());                                    // use rd instead of 42 as seed for real randomness
  std::normal_distribution<double> velocity_gen(1.34, 0.26); // 1.34 and 0.26 are the mean and stdrd deviation used in paper
  std::uniform_real_distribution<double> x_gen(2, BOARD_LENGTH - 2);
  std::uniform_real_distribution<double> y_gen(2, BOARD_WIDTH - 2);
  for (int i = 0; i < n_p / 2; i++) {
    Vector2d start_pos = Vector2d(1, y_gen(gen));
    Vector2d destination = Vector2d(BOARD_LENGTH - 1, y_gen(gen));
    double desired_v0 = velocity_gen(gen);
    double v_max = 1.3 * desired_v0;
    P[i] = Pedestrian(WHITE, (destination - start_pos) / (destination - start_pos).norm() * desired_v0, start_pos, (destination - start_pos) / (destination - start_pos).norm(), v_max, desired_v0, destination, Vector2d());
  }

  for (int i = n_p / 2; i < n_p; i++) {
    Vector2d start_pos = Vector2d(BOARD_LENGTH - 1, y_gen(gen));
    Vector2d destination = Vector2d(1, y_gen(gen));
    double desired_v0 = velocity_gen(gen);
    double v_max = 1.3 * desired_v0;
    P[i] = Pedestrian(BLACK, (destination - start_pos) / (destination - start_pos).norm() * desired_v0, start_pos, (destination - start_pos) / (destination - start_pos).norm(), v_max, desired_v0, destination, Vector2d(0, 0));
  }
  return P;
}

int main(int argc, const char *argv[]) {

  Obstacle obs[] = {};
  Board board = Board(BOARD_LENGTH, BOARD_WIDTH, 0, obs);

  int n_p = atoi(argv[1]);
  int n_iter = atoi(argv[2]);
  Pedestrian *pedestrians = init_pedestrians(n_p);
  for (int t = 0; t < n_iter; t++) {

    for (int i = 0; i < n_p; i++) {

      Pedestrian &p = pedestrians[i];

      p.f_i.reset();

      compute_acceleration_term(&p);

      compute_repulsive_effects_from_pedestrians(pedestrians, &p, n_p);

      compute_repulsive_effects_from_borders(board, &p);

      Vector2d w_alpha = p.curr_vel + p.f_i * dt;

      p.curr_vel = w_alpha * g(p.v_max, w_alpha.norm());

      p.curr_pos = p.curr_pos + p.curr_vel * dt;

      p.e = (p.destination - p.curr_pos) / (p.destination - p.curr_pos).norm();
    }
  }

  return 0;
}