#include <cfloat>
#include <iostream>
#include <math.h>

using namespace std;

struct Vector2d {
  double x, y;
  Vector2d(double a, double b) : x(a), y(b) {}
  Vector2d() : x(.0), y(.0) {}
  Vector2d operator+(Vector2d v) const { return Vector2d(v.x + x, v.y + y); }
  Vector2d operator-(Vector2d v) const { return Vector2d(x - v.x, y - v.y); }
  Vector2d operator-(double a) const { return Vector2d(x - a, y - a); }
  Vector2d operator+(double a) const { return Vector2d(x + a, y + a); }
  Vector2d operator/(double a) const { return Vector2d(x / a, y / a); }
  Vector2d operator*(double a) const { return Vector2d(x * a, y * a); }
  Vector2d operator+=(const Vector2d &v) {
    *this = *this + v;
    return *this;
  }
  double norm() { return sqrt(x * x + y * y); }
  double dot(Vector2d v) { return x * v.x + y * v.y; }
  void reset() {
    x = 0;
    y = 0;
  }
};


struct Pedestrian {
  Pedestrian() : curr_pos(), next_pos(), curr_vel(), next_vel(), destination(), desired_next_node(), actual_next_node(), F_i(), desired_v0(1.34), actual_v0(1.34), vMax(1.34 * 1.3) {}
  Pedestrian(Vector2d c_p, Vector2d n_p, Vector2d c_v, Vector2d n_v, Vector2d d, Vector2d d_n_n, Vector2d a_n_n, Vector2d f) : curr_pos(c_p), next_pos(n_p), curr_vel(c_v), next_vel(n_v), destination(d), desired_next_node(d_n_n), actual_next_node(a_n_n), F_i(f) {}
  Vector2d curr_pos;  // $\vectorarrow{r}_\alpha(t)$
  Vector2d next_pos;  // $\vectorarrow{r}_\alpha(t+1)$ position, curr_pos is needed because when calculating the force on pedestrian j, we need the old pos, not the updated
  Vector2d curr_vel, next_vel;  // velocity
  Vector2d destination;  // $\vectorarrow{r}^0_\alpha$
  Vector2d desired_next_node; // $\vectorarrow{r}^k_\alpha$
  Vector2d actual_next_node; // $\vectorarrow{r}^k_\alpha$
  Vector2d F_i;  // social force
  double desired_v0;  // $v_\alpha^0$ desired speed in m/s e.g. 1.34
  double actual_v0;
  double vMax; // vMax = v0 * 1.3
  // direction Vector2d e = (dest - curr.position).norm() --> calculate on the fly or part of struct?
  void update() { curr_pos = next_pos; }
};


// initialize map
struct Sidewalk{
  int row, col;
  double length, width;
  Sidewalk(int r, int c, double l, double w) : row(r), col(c), length(l), width(w) {}
  /* X = obstacle
  |     X      |
  |   X        |
  |            |
  |         X  |
  |X           |
  */
};

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

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

// TODO: Change this
// $\tau$
const double tau = 0.12345;

// F_i is the current position resp. direction vector of pedestrian p in time i?
//
// $\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)$
//
void compute_acceleration_term(Pedestrian *P) {

  Vector2d desired_velocity = get_desired_direction(*P) * P->desired_v0;

  // TODO: How to get this number
  // Probably needs to lookup the map and see if the desired position is reachable
  Vector2d actual_velocity = get_actual_direction(*P) * P->actual_v0;

  double tau_inv = 1 / tau;

  P->F_i = (desired_velocity - actual_velocity) * tau_inv;

}

Vector2d get_next_node(Pedestrian p, Vector2d map[]) {
  // dummy element
  return Vector2d(p.curr_pos.x + 1.0, p.curr_pos.y + 1.0);
}

// return the direction between curr position and destination
// $\vectorarrow{e}_\alpha(t)$
Vector2d get_desired_direction(Pedestrian p) {
  Vector2d diff = p.desired_next_node - p.curr_pos;
  double length = diff.norm();
  return (p.desired_next_node - p.curr_pos) / length;
}

// return the direction between curr position and destination
// $\vectorarrow{e}_\alpha(t)$
Vector2d get_actual_direction(Pedestrian p) {
  Vector2d diff = p.actual_next_node - p.curr_pos;
  double length = diff.norm();
  return (p.actual_next_node - p.curr_pos) / length;
}


void compute_repulsive_effects_from_pedestrians(Vector2d *F_i) {

}

void compute_repulsive_effects_from_borders(Vector2d *F_i) {

}

// Return an array of doubles
double *w(Vector2d e, Vector2d f) {

  // TODO:
  // $cos(60\degree)$
  // if (e * f > f.norm() * (0.5)) {
  // }
  // return 0;
}

Pedestrian* init_pedestrians() {
  Pedestrian* P = new Pedestrian[n_p];
  for (int i = 0; i < n_p; i++) {
    P[i] = Pedestrian();
  }
  return P;
}

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];


  // 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);




      compute_repulsive_effects_from_pedestrians(F[i]);




      compute_repulsive_effects_from_borders(F[i]);


      // TODO: Update $\vec{w}_i$

      // TODO: Update $\vec{v}_i^{t+1}$

      // TODO: Update $\vec{r}_i^{t+1}$
    }
  }

  return 0;
}