#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();

  // Our time unit (how long a timestep goes)
  double dt = 1;

  // 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$
      // Intermediate velocity: current_velocity + force impulse (Force * time) (Equation1)
      Vector2d w_i = p.curr_vel + p.F_i * dt;

      // TODO: Update $\vec{v}_i^{t+1}$
      //
      // $\vec{v}_i^{t+1} += \vectorarrow{w}_ig(\frac{v^{max}_i}{\Arrowvert \vectorarrow{w}_i \Arrowvert}) $
      // Limit velocity to vMax
      double w_norm = w_i.norm();
      double g = (w_norm < 1e-6) ? 1.0 : fmin(1.0, p.vMax / w_norm);
      p.next_vel = w_i * g;

      // TODO: Update $\vec{r}_i^{t+1}$
      // $\vec{r}_i^{t+1} += \vectorarrow{r}^t_i + v^t_idt$
      p.next_pos = p.curr_pos + p.curr_vel * dt;
    }

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

  return 0;
}