#pragma once

#include <assert.h>
#include <set>
#include <vector>
#include <queue>

#include <eigen3/Eigen/Dense>

#include "pcg-cpp/include/pcg_random.hpp"
#include "randutils/randutils.hpp"

using Rng = randutils::random_generator<pcg32>;

template <int D> using Vector = Eigen::Matrix<double, D, 1>;
template <int D> using Matrix = Eigen::Matrix<double, D, D>;

template <int D> class Position : public Vector<D> {
private:

  void recenter() {
    for (unsigned i = 0; i < D; i++) {
      double& vi = Vector<D>::operator()(i);
      signed n = floor(vi);
      vi -= n;
      if (i == D - 2) {
        Vector<D>::operator()(D - 1) += γ * n;
      }
    }
  }

public:
  const double& γ;
  Position(const double& γ) : γ(γ) {}

  Position(const Position<D>& p) : Vector<D>(p), γ(p.γ) {}
  Position(const Vector<D>& v, const double& γ) : Vector<D>(v), γ(γ) {}

  Position<D>& operator=(const Position<D>& p) {
    Vector<D>::operator=(p);
    return *this;
  }

  Position<D>& operator+=(const Vector<D>& u) {
    Vector<D>::operator+=(u);
    recenter();
    return *this;
  }

  friend Position<D> operator+(Position<D> v, const Vector<D>& u) {
    v += u;
    return v;
  }

  Position<D>& operator-=(const Vector<D>& u) {
    Vector<D>::operator-=(u);
    recenter();
    return *this;
  }

  friend Vector<D> operator-(const Position<D>& v, const Position<D>& u) {
    std::vector<Vector<2>> ds;
    ds.reserve(9);

    unsigned iCur  = 0;
    unsigned iMin;
    double dMin = std::numeric_limits<double>::infinity();

    Position<2> v2(v.γ);
    v2(0) = v(D - 2);
    v2(1) = v(D - 1);

    for (signed i : {-1, 0, 1}) {
      for (signed j : {-1, 0, 1}) {
        Vector<2> u2;
        u2(0) = u(0) + i;
        u2(1) = u(1) + j + v.γ * i;

        ds.push_back(v2 - u2);

        double d = ds.back().squaredNorm();

        if (d < dMin) {
          iMin = iCur;
          dMin = d;
        }

        iCur++;
      }
    }

    Vector<D> w = v - (Vector<D>)u;

    for (unsigned i = 0; i < D - 2; i++) {
      double& wi = w(i);
      if (wi > 0.5) {
        wi = 1 - wi;
      }
    }

    w(D - 2) = ds[iMin](0); 
    w(D - 1) = ds[iMin](1); 

    return (Vector<D>)w;
  }
};

template <int D> class Euclidean {
public:
  Vector<D> t;
  Matrix<D> r;

  Euclidean() {
    t.setZero();
    r.setIdentity();
  }

  Euclidean(const Vector<D>& t0, const Matrix<D>& r0) : t(t0), r(r0) {}

  Euclidean inverse() const { return Euclidean(-r.transpose() * t, r.transpose()); }

  Vector<D> act(const Vector<D>& x) const { return r * x + t; }
  Position<D> act(const Position<D>& x) const { return Position<D>(r * x, x.γ) + t; }
  Euclidean<D> act(const Euclidean<D>& R) const { return {act(R.t), r * R.r}; }
};

template <typename T, int D> concept Particle = requires(T v, const Position<D>& x, double r) {
  { v.x } -> std::same_as<Position<D>>;
  { v.r } -> std::same_as<double>;
  { v.m } -> std::same_as<bool>;
  { v.U(x, r) } -> std::same_as<double>;
};

template <int D, Particle<D> T> class NeighborLists {
private:
  unsigned n;
  const double& γ;
  std::vector<std::set<T*>> lists;

  unsigned mod(signed a, unsigned b) const {
    if (a >= 0) {
      return a % b;
    } else {
      return (a + b * ((b - a) / b)) % b;
    }
  }

  int iPow(int x, unsigned p) const {
    if (p == 0)
      return 1;
    if (p == 1)
      return x;

    int tmp = iPow(x, p / 2);
    if (p % 2 == 0) {
      return tmp * tmp;
    } else {
      return x * tmp * tmp;
    }
  }

  unsigned index(Position<D> x) const {
    unsigned ind = 0;
    for (unsigned i = 0; i < D; i++) {
      ind += pow(n, i) * std::floor(n * x(i));
    }
    assert(ind < lists.size());
    return ind;
  }

  std::pair<typename std::set<T*>::iterator, bool> insert(T& p, unsigned ind) {
    return lists[ind].insert(&p);
  }

  size_t erase(T& p, unsigned ind) {
    return lists[ind].erase(&p);
  }

  void neighborsOfHelper(signed i0, unsigned d, std::set<T*>& ns, signed overEdge = 0) const {
    if (d == 0) {
      const std::set<T*>& a = lists[i0];
      ns.insert(a.begin(), a.end());
    } else {
      for (signed j : {-1, 0, 1}) {
        signed i1 = iPow(n, d) * (i0 / iPow(n, d)) + mod(i0 + (signed)round(overEdge * n * γ) + j * iPow(n, d-1), iPow(n, d));
        signed newEdge = 0;
        if (d == 2) {
          if (i0 - i1 == n)
            newEdge = -1;
          if (i1 - i0 == n) {
            newEdge = 1;
          }
        }
        neighborsOfHelper(i1, d - 1, ns, newEdge);
      }
    }
  }

public:
  NeighborLists(const double& γ, unsigned m) : n(m), γ(γ), lists(pow(n, D)) {}
  NeighborLists(const double& γ, double l) : NeighborLists(γ, (unsigned)floor(1 / l)) {}

  std::pair<typename std::set<T*>::iterator, bool> insert(T& p, const Position<D>& x) {
    return insert(p, index(x));
  }

  std::pair<typename std::set<T*>::iterator, bool> insert(T& p) {
    return insert(p, p.x);
  }

  size_t erase(T& p, const Position<D>& x) {
    return erase(p, index(x));
  }

  size_t erase(T& p) {
    return erase(p, p.x);
  }

  void move(T& p, const Position<D>& xNew) {
    unsigned iOld = index(p.x);
    unsigned iNew = index(xNew);
    if (iNew != iOld) {
      erase(p, iOld);
      insert(p, iNew);
    }
  }

  std::set<T*> neighborsOf(const Position<D>& x) const {
    std::set<T*> neighbors;
    neighborsOfHelper(index(x), D, neighbors);
    return neighbors;
  }
};

template <int D, Particle<D> T> class Model {
private:
  bool metropolisAccept(double β, double ΔE, Rng& r) const {
    return ΔE < 0 || exp(-β * ΔE) > r.uniform(0.0, 1.0);
  }

  bool swapAccept(const T& p1, const T& p2) const {
    double rat = p1.r / p2.r;
    return (rat < 1.2 && rat > 0.83);
  }

public:
  double γ;
  NeighborLists<D, T> nl;
  std::vector<T> particles;
  Euclidean<D> orientation;

  Model(unsigned N, double l, std::function<double(Rng& r)> g, Rng& r) : γ(0), nl(γ, l) {
    particles.reserve(N);
    for (unsigned i = 0; i < N; i++) {
      Position<D> x(γ);
      for (double& xi : x) {
        xi = r.uniform(0.0, 1.0);
      }
      particles.push_back(T(x, g(r)));
      nl.insert(particles.back());
    }
  };

  void shear(double Δγ) {
    γ += Δγ;
  }

  void move(T& p, const Position<D>& x) {
    nl.move(p, x);
    p.x = x;
  }

  bool move(double β, T& p, const Vector<D>& Δx, Rng& r) {
    Position<D> xNew = p.x + Δx;
    double ΔE = 0;

    for (const T* neighbor : nl.neighborsOf(p.x)) {
      if (neighbor != &p) {
        ΔE -= neighbor->U(p.x, p.r);
      }
    }

    for (const T* neighbor : nl.neighborsOf(xNew)) {
      if (neighbor != &p) {
        ΔE += neighbor->U(xNew, p.r);
      }
    }

    if (metropolisAccept(β, ΔE, r)) {
      move(p, xNew);
      return true;
    } else {
      return false;
    }
  }

  bool swap(double β, T& p1, T& p2, Rng& r) {
    if (!swapAccept(p1, p2))
      return false;

    double ΔE = 0;

    for (const T* neighbor : nl.neighborsOf(p1.x)) {
      if (neighbor != &p1 && neighbor != &p2) {
        ΔE += neighbor->U(p1.x, p2.r) - neighbor->U(p1.x, p1.r);
      }
    }

    for (const T* neighbor : nl.neighborsOf(p2.x)) {
      if (neighbor != &p1 && neighbor != &p2) {
        ΔE += neighbor->U(p2.x, p1.r) - neighbor->U(p2.x, p2.r);
      }
    }

    if (metropolisAccept(β, ΔE, r)) {
      std::swap(p1.r, p2.r);
      return true;
    } else {
      return false;
    }
  }

  bool randomSwap(double β, Rng& r) {
    return swap(β, r.pick(particles), r.pick(particles), r);
  }

  bool randomMove(double β, double δ, Rng& r) {
    Vector<D> Δx;
    for (double& Δxi : Δx) {
      Δxi = r.variate<double, std::normal_distribution>(0, δ);
    }
    return move(β, r.pick(particles), Δx, r);
  }

  unsigned clusterFlip(double β, const Euclidean<D>& R, T& p0, Rng& r) {
    unsigned n = 0;
    std::queue<T*> q;
    q.push(&p0);

    while (!q.empty()) {
      T* p = q.front();
      q.pop();

      if (!p->m) {
        p->m = true;
        Position<D> xNew = R.act(p->x);

        for (T* neighbor : nl.neighborsOf(xNew)) {
          if (!neighbor->m) {
            double ΔE = neighbor->U(xNew, p->r) - neighbor->U(p->x, p->r);
            if (1 - exp(-β * ΔE) > r.uniform(0.0, 1.0)) {
              q.push(neighbor);
            }
          }
        }

        move(*p, xNew);
        n++;
      }
    }

    for (T& p : particles) {
      p.m = false;
    }

    if (n > particles.size() / 2) {
      orientation = R.act(orientation);
    }

    return n;
  }

  unsigned clusterSwap(double β, T& s1, T& s2, Rng& r) {
    if (!swapAccept(s1, s2))
      return 0;

    Vector<2> t1 = s1.x;
    Vector<2> t2 = s2.x;
    Vector<2> t = (t1 + t2) / 2;

    Matrix<2> mat;

    mat(0, 0) = -1;
    mat(1, 1) = -1;
    mat(0, 1) = 0;
    mat(1, 0) = 0;

    Euclidean<2> g(t - mat * t, mat);

    return clusterFlip(β, g, s1, r);
  }

  unsigned randomClusterSwap(double β, Rng& r) {
    return clusterSwap(β, r.pick(particles), r.pick(particles), r);
  }
};

template <int D> class Sphere {
public:
  Position<D> x;
  bool m;
  double r;

  Sphere() : m(false) {};

  Sphere(const Position<D>& x, double r) : x(x), m(false), r(r) {};

  double regularizedDistanceSquared(const Position<D>& y, double ry) const {
    return (x - y).squaredNorm() / pow(r + ry, 2);
  } 

  bool intersectsBoundary() const {
    for (double xi : x) {
      if (xi < r || 1 - xi < r) {
        return true;
      }
    }
    return false;
  }
};

std::function<double(Rng&)> gContinuousPolydisperse(double Rmax) {
  return [Rmax] (Rng& r) -> double {
    return pow(r.uniform(pow(2.219, -2), 1.0), -0.5) * Rmax / 2.219;
  };
}