#include "board.hpp"
#include "forces.hpp"
#include "math.h"
#include "pedestrian.hpp"
#include "tsc_x86.h"
#include "vector2d.hpp"
#include <fstream>
#include <iostream>
#include <tuple>
#include <vector>

#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::mt19937 gen(42);                                      // use fixed seed for reproducibility
  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++) {
    double y = y_gen(gen);
    Vector2d start_pos = Vector2d(x_gen(gen), y);
    Vector2d destination = Vector2d(BOARD_LENGTH + 300, BOARD_WIDTH / 2);
    // double desired_v0 = velocity_gen(gen);
    double desired_v0;
    do {
      desired_v0 = velocity_gen(gen);
    } while (desired_v0 <= 0.1); // to avoid small values (Way below the social force -> that would make the walk backwards) (we discovered some weird stuff in the simulation)
    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++) {
    double y = y_gen(gen);
    Vector2d start_pos = Vector2d(x_gen(gen), y);
    Vector2d destination = Vector2d(-300, BOARD_WIDTH / 2);
    // double desired_v0 = velocity_gen(gen);
    double desired_v0;
    do {
      desired_v0 = velocity_gen(gen);
    } while (desired_v0 <= 0.1);
    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]);

  // Logic to get/dump the simulation data
  const int dump_interval = 5; // dump every 5 iterations -> 20 fps (since dt = 0.01)
  const char *dump_path = (argc >= 4) ? argv[3] : nullptr;

  std::vector<std::tuple<int, int, double, double, int>> samples; // t, id, x, y, color
  if (dump_path)
    samples.reserve((size_t)n_iter / dump_interval * n_p);

  Pedestrian *pedestrians = init_pedestrians(n_p);

  // # Start here or right after function signature
  uint64_t start = start_tsc();

  // # (8 + 55N - 55 + 192 + 4 + 8 + 4 + 10)*n_p*n_iter
  for (int t = 0; t < n_iter; t++) {
    // Pass1: conmpute all forces from the current state
    for (int i = 0; i < n_p; i++) {

      Pedestrian &p = pedestrians[i];

      p.f_i.reset();

      // # 8 flops
      compute_acceleration_term(&p);

      // # 55N - 55 flops
      compute_repulsive_effects_from_pedestrians(pedestrians, &p, n_p);

      // # 192 flops
      compute_repulsive_effects_from_borders(board, &p);
    }
    // Pass2: integrate the forces and update the current state
    for (int i = 0; i < n_p; i++) {
      Pedestrian &p = pedestrians[i];
      // # 4 flops = 2 ADDs, 2 MULTs
      // integrate preferred velocity:  dw/dt = F           (Eq. 10)
      p.w = p.w + p.f_i * dt;
      // Vector2d w_alpha = p.curr_vel + p.f_i * dt; // commented since this was using p.curr_vel instead of p.w (different from paper)

      // # 8 flops = 2 MULTs, 2 DIVs, 1 SQRT, 2 MULTs, 1 ADD
      p.curr_vel = p.w * g(p.v_max, p.w.norm());

      // # 4 flops = 2 ADDs, 2 MULTs
      p.curr_pos = p.curr_pos + p.curr_vel * dt;

      // how to handle if a pedestrian reached the end of our simulation/end of the hallway
      // The option we do here is to just "teleport" the person back to the beginning
      // the good thing is we will have a constant flow of pedestrians and in the simulation we should see how lines build up
      if (p.color == WHITE && p.curr_pos.x > (BOARD_LENGTH - 1.5)) {
        // basically just keep the y position and reset the x
        p.curr_pos.x = 1.0;
        // p.destination.y = p.curr_pos.y;
      }
      if (p.color == BLACK && p.curr_pos.x < 1.5) {
        p.curr_pos.x = BOARD_LENGTH - 1.0;
        // p.destination.y = p.curr_pos.y;
      }

      // # 10 flops = 5 ADDs, 2 DIVs, 1 SQRT, 2 MULTs
      p.e = (p.destination - p.curr_pos) / (p.destination - p.curr_pos).norm();

      // if we have a dump path/file we wanna save the data for visualisation
      if (dump_path && (t % dump_interval == 0)) {
        samples.emplace_back(t, i, p.curr_pos.x, p.curr_pos.y, p.color);
      }
    }
  }
  long long W = (long long)(8 + 55 * n_p - 55 + 192 + 4 + 8 + 4 + 10) * n_p * n_iter;
  std::cout << "W(n): ";
  std::cout << W;
  std::cout << " flops\n";

  double T = stop_tsc(start);
  std::cout << "T(n): ";
  std::cout << T;
  std::cout << " cycles\n";

  double P = W / T;

  std::cout << "P(n): ";
  std::cout << P;
  std::cout << " flops/cycle\n";

  // now we wanna save the data to a csv
  if (dump_path) {
    std::ofstream out(dump_path);
    out << "time,id,x,y,color\n";
    for (auto &[t, id, x, y, c] : samples) {
      out << t << ',' << id << ',' << x << ',' << y << ',' << c << '\n';
    }
  }

  delete[] pedestrians;
  return 0;
}