#include <cfloat>
#include <iostream>
#include <math.h>
#include <vector2d.h>
#include <pedestrian.h>
#include <sidewalk.h>
#include <forces.h>

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() {
  Pedestrian* P = new Pedestrian[n_p];
  for (int i = 0; i < n_p; i++) {
    P[i] = Pedestrian();
  }
  return P;
}

Sidewalk init_sidewalk(int width, int length) {
  
  Vector2d obs[] = {Vector2d(2.0, 1.0), Vector2d(4.0, 1.0), Vector2d(6.0, 2.0), Vector2d(6.0, 4.0)};
  return Sidewalk(width, length, 4, obs);
}



int main() {

  // ! DONT TOUCH

  // $\vec{v}^0_i$
  // Velocity at iteration $t$ for pedestrian $i$
  double v[n_iter][n_p];

  // $\vec{r}^0_i$
  // Intended destination at iteration $t$ for pedestrian $i$
  Vector2d r[n_iter][n_p];

  // $\vec{e}_i$
  // Desired direction to the next way point at iteration $t$ for pedestrian $i$
  Vector2d e[n_iter][n_p];

  // $v_i^{max}$
  // Maximum allowed speed for pedestrian $i$
  double v_max[n_p];

  // $v_i^{0}$
  // Desired speed for pedestrian $i$
  double v_0[n_p];

  // $\vec{F}_i$
  // Total social force for pedestrian $i$
  Vector2d F[n_p][n_p];

  // $\vec{r}_0$
  // Desired destination of pedestrian $n_p$
  Vector2d r_0[n_p];

  // create sidewalk
  Sidewalk S = init_sidewalk(10, 15);


  // store Array of Structs, store all pedestrians
  Pedestrian* P = init_pedestrians();

  // for each time frame
  for (int t = 0; t < n_iter; t++) {
    // for each pedestrian
    for (int i = 0; i < n_p; i++) {
      // update...
      Pedestrian& p = P[i];
      
      // reset total social force
      p.F_i.reset();

      //
      // $\vectorarrow{F}_\alpha(\vectorarrow{v}_\alpha, v^0_\alpha \vectorarrow{e}_\alpha) \vectorarrow{e}_\alpha(t) = \frac{1}{\tau_\alpha}(v^0_\alpha \vectorarrow{e}_\alpha - \vectorarrow{v}_\alpha)$
      //
      compute_acceleration_term(&p);


      // $\vectorarrow{f}_{\alpha \beta}(\vectorarrow{r}_{\alpha \beta}) = -\gradient_{\vectorarrow{r}_{\alpha \beta}} V_{\alpha \beta}[b(\vectorarrow{r}_{\alpha \beta})]$
      compute_repulsive_effects_from_pedestrians(P, &p, n_p);


      // TODO: finalize this method
      //
      // $\vectorarrow{F}_{\alpha B}(\vectorarrow{r}_{\alpha B}) = \frac{U^0_{\alpha B}}{R} e^{- \Arrowvert \vectorarrow{r}_{\alpha B} \Arrowvert / R} \frac{\vectorarrow{r}_{\alpha B}}{\Arrowvert \vectorarrow{r}_{\alpha B} \Arrowvert}$
      //
      compute_repulsive_effects_from_borders(S, &p);


      // TODO: Update $\vec{w}_i$
      // $\vectorarrow{w}_i += \vectorarrow{v}_i + \vectorarrow{F}_idt$


      // TODO: Update $\vec{v}_i^{t+1}$
      //
      // $\vec{v}_i^{t+1} += \vectorarrow{w}_ig(\frac{v^{max}_i}{\Arrowvert \vectorarrow{w}_i \Arrowvert}) $
      //


      // TODO: Update $\vec{r}_i^{t+1}$
      // $\vec{r}_i^{t+1} += \vectorarrow{r}^t_i + v^t_idt$


    }

    for (int i = 0; i < n_p; i++) {
      // move each pedestrian to their next position
      P[i].update();
    }

  }

  return 0;
}