#include <fstream>
#include <iostream>
#include <limits>
#include <list>
#include <queue>
#include <vector>
#include <chrono>
#include <unordered_set>

#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<int, D, 1>;
template <int D> using Matrix = Eigen::Matrix<int, D, D>;

const unsigned Empty = std::numeric_limits<unsigned>::max();

int iPow(int x, unsigned p) {
  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 mod(signed a, unsigned b) { return ((a < 0) ? (a + (1 - a / (signed)b) * b) : a) % b; }

template <int D, typename Derived> Vector<D> mod(const Eigen::MatrixBase<Derived>& v, unsigned b) {
  Vector<D> u;
  for (unsigned i = 0; i < D; i++) {
    u(i) = mod(v(i), b);
  }
  return u;
}

template <int D> void one_sequences(std::list<std::array<double, D>>& sequences, unsigned level) {
  if (level > 0) {
    unsigned new_level = level - 1;
    for (std::array<double, D>& sequence : sequences) {
      std::array<double, D> new_sequence = sequence;
      new_sequence[new_level] = -1;
      sequences.push_front(new_sequence);
    }
    one_sequences<D>(sequences, new_level);
  }
}

template <int D> std::vector<Matrix<D>> generateTorusMatrices() {
  std::vector<Matrix<D>> mats;

  std::array<double, D> ini_sequence;
  ini_sequence.fill(1);
  std::list<std::array<double, D>> sequences;
  sequences.push_back(ini_sequence);

  one_sequences<D>(sequences, D);

  sequences.pop_back(); // don't want the identity matrix!

  for (std::array<double, D> sequence : sequences) {
    Matrix<D> m;
    for (unsigned i = 0; i < D; i++) {
      for (unsigned j = 0; j < D; j++) {
        if (i == j) {
          m(i, j) = sequence[i];
        } else {
          m(i, j) = 0;
        }
      }
    }

    mats.push_back(m);
  }

  for (unsigned i = 0; i < D; i++) {
    for (unsigned j = 0; j < D; j++) {
      if (i != j) {
        Matrix<D> m;
        for (unsigned k = 0; k < D; k++) {
          for (unsigned l = 0; l < D; l++) {
            if ((k == i && l == j) || (k == j && l == i)) {
              if (i < j) {
                m(k, l) = 1;
              } else {
                m(k, l) = -1;
              }
            } else if (k == l && (k != i && k != j)) {
              m(k, l) = 1;
            } else {
              m(k, l) = 0;
            }
          }
        }
        mats.push_back(m);
      }
    }
  }

  return mats;
}

template <int D> class Transformation {
public:
  unsigned L;
  Matrix<D> m;
  Vector<D> v;

  Transformation(unsigned L) : L(L) {
    m.setIdentity();
    v.setZero();
  }

  Transformation(unsigned L, const Matrix<D>& m, const Vector<D>& v) : L(L), m(m), v(v) {}

  Transformation(unsigned L, const std::vector<Matrix<D>>& ms, Rng& r) : m(r.pick(ms)), L(L) {
    for (unsigned i = 0; i < D; i++) {
      v[i] = r.uniform((unsigned)0, L - 1);
    }

    v = v - m * v;
  }

  Transformation<D> inverse() const {
    return Transformation<D>(L, m.transpose(), -m.transpose() * v);
  }

  Vector<D> apply(const Vector<D>& x) const { return mod<D>(v + m * x, L); }

  Transformation<D> apply(const Transformation<D>& t) const {
    Transformation<D> tNew(L);

    tNew.m = m * t.m;
    tNew.v = apply(t.v);

    return tNew;
  }
};

template <int D> class Vertex {
public:
  Vector<D> initialPosition;
  Vector<D> position;
  std::vector<std::reference_wrapper<Vertex<D>>> neighbors;
  unsigned occupiedNeighbors;
  unsigned maximumNeighbors;
  bool marked;
  bool visited;

  Vertex() {
    occupiedNeighbors = 0;
    maximumNeighbors = Empty;
    marked = false;
  }

  bool empty() const { return maximumNeighbors == Empty; }
  bool frustrated() const { return occupiedNeighbors > maximumNeighbors; }
  bool valid() const {
    unsigned o = 0;

    for (const Vertex<D>& v : neighbors) {
      if (!v.empty()) {
        o++;
      }
    }

    return o == occupiedNeighbors;
  }
  void setInitial() {
    initialPosition = position;
  }
};

template <int D> class System {
public:
  const unsigned L;
  std::vector<unsigned> N;
  std::unordered_set<Vertex<D>*> occupants;
  std::vector<Vertex<D>> vertices;
  Transformation<D> orientation;

  unsigned size() const { return vertices.size(); }

  unsigned types() const { return N.size() - 1; }

  unsigned occupancy() const {
    return size() - N[0];
  }

  double density() const { return (double)occupancy() / size(); }

  bool compatible() const {
    for (const Vertex<D>& v : vertices) {
      if (v.frustrated()) {
        return false;
      }
    }

    return true;
  }

  bool valid() const {
    for (const Vertex<D>& v : vertices) {
      if (!v.valid()) {
        return false;
      }
    }

    return true;
  }

  unsigned vectorToIndex(const Vector<D>& x) const {
    unsigned i = 0;
    for (unsigned d = 0; d < D; d++) {
      i += x[d] * iPow(L, d);
    }
    return i;
  }

  Vector<D> indexToVector(unsigned i) const {
    Vector<D> x;
    for (unsigned d = 0; d < D; d++) {
      x[d] = (i / iPow(L, d)) % L;
    }
    return x;
  }

  System(unsigned L, unsigned T) : L(L), N(T + 1, 0), vertices(iPow(L, D)), orientation(L) {
    N[0] = size();

    for (unsigned i = 0; i < size(); i++) {
      vertices[i].position = indexToVector(i);
      vertices[i].neighbors.reserve(2 * D);
    }

    for (unsigned d = 0; d < D; d++) {
      for (signed i = 0; i < size(); i++) {
        unsigned j = iPow(L, d + 1) * (i / iPow(L, d + 1)) + mod(i + iPow(L, d), pow(L, d + 1));
        vertices[i].neighbors.push_back(vertices[j]);
        vertices[j].neighbors.push_back(vertices[i]);
      }
    }
  }

  std::list<std::reference_wrapper<Vertex<D>>> overlaps(Vertex<D>& v) {
    std::list<std::reference_wrapper<Vertex<D>>> o;

    if (v.empty()) { // an empty site cannot be frustrating anyone
      return o;
    }

    bool selfFrustrated = v.frustrated();
    bool anyNeighborFrustrated = false;

    for (Vertex<D>& vn : v.neighbors) {
      if (!vn.empty()) {
        bool thisNeighborFrustrated = vn.frustrated();

        if (thisNeighborFrustrated) {
          anyNeighborFrustrated = true;
        }

        if (selfFrustrated || thisNeighborFrustrated) {
          o.push_back(vn);
        }
      }
    }

    if (selfFrustrated || anyNeighborFrustrated) {
      o.push_back(v);
    }

    return o;
  }

  std::list<std::reference_wrapper<Vertex<D>>> overlaps(const std::list<std::reference_wrapper<Vertex<D>>>& vs) {
    std::list<std::reference_wrapper<Vertex<D>>> o;

    for (Vertex<D>& v : vs) {
      o.splice(o.begin(), overlaps(v));
    }

    return o;
  }

  bool canReplaceWith(const Vertex<D>& v, unsigned t) const {
    if (t == Empty) {
      return true;
    }

    if (t < v.occupiedNeighbors) {
      return false;
    }

    if (v.empty()) {
      for (const Vertex<D>& vn : v.neighbors) {
        if (vn.maximumNeighbors < vn.occupiedNeighbors + 1) {
          return false;
        }
      }
    }

    return true;
  }

  void setInitial() {
    for (Vertex<D>& v : vertices) {
      v.setInitial();
    }
  }

  bool insert(Vertex<D>& v, unsigned t, bool force = false) {
    if (force || (v.empty() && canReplaceWith(v, t))) {
      v.maximumNeighbors = t;
      for (Vertex<D>& vn : v.neighbors) {
        vn.occupiedNeighbors++;
      }
      N[t]++;
      N[0]--;
      if (t > 0) {
        occupants.insert(&v);
      }
      return true;
    } else {
      return false;
    }
  }

  bool remove(Vertex<D>& v) {
    if (v.empty()) {
      return false;
    } else {
      if (v.maximumNeighbors > 0) {
        occupants.erase(&v);
      }
      N[v.maximumNeighbors]--;
      N[0]++;
      v.maximumNeighbors = Empty;
      for (Vertex<D>& vn : v.neighbors) {
        vn.occupiedNeighbors--;
      }
      return true;
    }
  }

  bool randomLocalExchange(Rng& r) {
    Vertex<D>& v = r.pick(vertices);

    return exchange(v, r.pick(v.neighbors));
  }

  void swap(Vertex<D>& v1, Vertex<D>& v2) {
    if (v1.maximumNeighbors != v2.maximumNeighbors) {
      if (!v1.empty() && !v2.empty()) {
        std::swap(v1.maximumNeighbors, v2.maximumNeighbors);
      } else if (v1.empty()) {
        unsigned t = v2.maximumNeighbors;
        remove(v2);
        insert(v1, t, true);
      } else {
        unsigned t = v1.maximumNeighbors;
        remove(v1);
        insert(v2, t, true);
      }
      std::swap(v1.initialPosition, v2.initialPosition);
    }
  }

  bool exchange(Vertex<D>& v1, Vertex<D>& v2) {
    if (canReplaceWith(v1, v2.maximumNeighbors) && canReplaceWith(v2, v1.maximumNeighbors)) {
      swap(v1, v2);
      return true;
    } else {
      return false;
    }
  }

  std::complex<double> selfIntermediateScattering(unsigned k, unsigned t = 2) const {
    std::complex<double> F = 0;

    using namespace std::complex_literals;

    for (const Vertex<D>& v : vertices) {
      if (v.maximumNeighbors == t) {
        for (unsigned d = 0; d < D; d++) {
          F += exp(1i * 2.0 * M_PI * (double)k * (double)(v.position(d) - v.initialPosition(d)) / (double)L);
        }
      }
    }

    F /= D * N[t];

    return F;
  }

  double overlap(const System<D>& s, unsigned t = 2) const {
    int o = 0;

    for (const Vertex<D>& v1 : vertices) {
      const Vertex<D>& v2 = s.vertices[vectorToIndex(orientation.inverse().apply(v1.position))];
      if (v1.maximumNeighbors == v2.maximumNeighbors) {
        o++;
      }
    }

    unsigned nn = 0;

    for (unsigned n : N) {
      nn += iPow(n, 2);
    }

    return ((double)o / (double)size() - (double)nn / pow((double)size(), 2)) / (1 - (double)nn / pow((double)size(), 2));
  }

  bool randomExchange(Rng& r) {
    Vertex<D>& v1 = r.pick(vertices);
    Vertex<D>& v2 = r.pick(vertices);

    return exchange(v1, v2);
  }

  const std::array<double, 3> ratios = {0.1, 0.5, 0.4};

  void stepGrandCanonical(double z, Rng& r) {
    if (1.0 / 11.0 < r.uniform(0.0, 1.0)) {
      unsigned t = r.pick({1, 2, 2, 2, 2, 2, 3, 3, 3, 3});
      double pIns = size() * z / (occupancy() + 1);

      if (pIns > r.uniform(0.0, 1.0)) {
        Vertex<D>& v = r.pick(vertices);
        insert(v, t);
      }
    } else {
      double pDel = density() / z;

      if (pDel > r.uniform(0.0, 1.0)) {
        Vertex<D>* v = r.pick(occupants);
        remove(*v);
      }
    }
  }

  void sweepGrandCanonical(double z, Rng& r) {
    for (unsigned i = 0; i < size(); i++) {
      stepGrandCanonical(z, r);

      randomExchange(r);
    }
  }

  void sweepLocal(Rng& r) {
    for (unsigned i = 0; i < size() / 2; i++) {
      randomLocalExchange(r);
    }
  }

  void sweepSwap(Rng& r) {
    for (unsigned i = 0; i < size() / 2; i++) {
      randomExchange(r);
    }
  }

  Vertex<D>& apply(const Transformation<D>& R, const Vertex<D>& v) {
    return vertices[vectorToIndex(R.apply(v.position))];
  }

  bool eventChain(const Transformation<D>& R, Vertex<D>& v0) {
    unsigned n = 0;

    if (!v0.empty()) {
      Vertex<D>& v0New = apply(R, v0);;
      unsigned t0 = v0.maximumNeighbors;

      std::queue<std::pair<std::reference_wrapper<Vertex<D>>, unsigned>> q;

      q.push({v0New, t0});
      remove(v0);

      while (!q.empty()) {
        auto [vr, t] = q.front();
        Vertex<D>& v = vr;
        q.pop();

        if (!v.empty()) {
          q.push({apply(R, v), v.maximumNeighbors});
          remove(v);
        }

        insert(v, t, true);
        v.marked = true;

        for (Vertex<D>& vn : overlaps({v})) {
          if (!vn.marked) {
            q.push({apply(R, vn), vn.maximumNeighbors});
            remove(vn);
            vn.marked = true;
          } else {
            /* If a neighbor has already been moved but is still
             * frustrated, it may be due to one of its neighbors!
             */
            if (&vn != &v) {
              for (Vertex<D>& vnn : overlaps(vn)) {
                if (!vnn.marked) {
                  q.push({apply(R, vnn), vnn.maximumNeighbors});
                  remove(vnn);
                  vnn.marked = true;
                }
              }
            }
          }
        }
      }
    }

    for (Vertex<D>& v : vertices) {
      v.marked = false;
    }

    return true;
  }

  bool randomEventChain(Rng& r) {
    Transformation<D> R(L);
    R.v[r.uniform((unsigned)0, (unsigned)D-1)] = r.pick({-1, 1});
    return eventChain(R, r.pick(vertices));
  }

  unsigned flipCluster(const Transformation<D>& R, Vertex<D>& v0) {
    unsigned n = 0;

    Vertex<D>& v0New = vertices[vectorToIndex(R.apply(v0.position))];

    if (&v0New != &v0) {
      std::queue<std::reference_wrapper<Vertex<D>>> q;

      v0.marked = true;
      v0New.marked = true;

      swap(v0, v0New);

      for (Vertex<D>& vn : overlaps({v0, v0New})) {
        if (!vn.marked) {
          q.push(vn);
        }
      }

      while (!q.empty()) {
        Vertex<D>& v = q.front();
        q.pop();

        if (!v.marked && !overlaps(v).empty()) {
          v.marked = true;
          Vertex<D>& vNew = vertices[vectorToIndex(R.apply(v.position))];

          if (&vNew != &v) {
            vNew.marked = true;

            swap(v, vNew);

            for (Vertex<D>& vn : overlaps({v, vNew})) {
              if (!vn.marked) {
                q.push(vn);
              } else {
                /* If a neighbor has already been moved but is still
                 * frustrated, it may be due to one of its neighbors!
                 */
                if (&vn != &v && &vn != &vNew) {
                  for (Vertex<D>& vnn : overlaps(vn)) {
                    if (!vnn.marked) {
                      q.push(vnn);
                    }
                  }
                }
              }
            }
            n += 2;
          } else {
            n += 1;
          }
        }
      }
    } else {
      n = 1;
    }

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

    for (Vertex<D>& v : vertices) {
      v.marked = false;
    }

    return n;
  }
};

void print(const System<2>& s) {
  for (const Vertex<2>& v : s.vertices) {
    if (!v.empty()) {
      std::cerr << v.maximumNeighbors;
    } else {
      std::cerr << " ";
    }

    if (v.position(0) == s.L - 1) {
      std::cerr << std::endl;
    }
  }
}

int main() {
  const unsigned D = 3;
  unsigned L = 15;
  unsigned Nmin = 2e2;
  unsigned Nmax = 2e5;
  double μmin = -3;
  double μmax = 10;
  double dμ = 0.05;

  System<D> s(L, 3);

  Rng r;

  for (double μ = μmin; μ <= μmax; μ += dμ) {
    double z = exp(μ);
    for (unsigned i = 0; i < 1e4; i++) {
      for (unsigned j = 0; j < s.size(); j++) {
        s.stepGrandCanonical(z, r);
//        s.randomEventChain(r);
        s.randomExchange(r);
      }

      if (!s.compatible()) {
        std::cerr << "Not compatible!" << std::endl;
        return 1;
      }

    }

    std::cout << μ << " " << s.density() << std::endl;
  }


  /*
  std::cerr << "Found state with appropriate density." << std::endl;

  System<D> s0 = s;

  std::vector<Matrix<D>> ms = generateTorusMatrices<D>();

  std::vector<unsigned> clusterDist(s.size() + 1);

  s.setInitial();

  unsigned n = 1;
  unsigned i = 0;
  double nC = 0;
  auto start = std::chrono::high_resolution_clock::now();
  while (nC / s.size() < 1e5) {
    if (n < 20 * log(i + 1)) {
      auto stop = std::chrono::high_resolution_clock::now();
      auto duration = duration_cast<std::chrono::microseconds>(stop - start);
      n++;
      std::cout << duration.count() << " " << s.overlap(s0) << std::endl;
    }
//    unsigned nn = s.flipCluster(Transformation<D>(L, ms, r), r.pick(s.vertices));
//    nC += nn;
//    clusterDist[nn]++;
    //s.sweepLocal(r);
    nC += s.size();
    s.sweepSwap(r);
    i++;
  }

  if (!s.compatible()) {
    std::cerr << "Not compatible!" << std::endl;
    return 1;
  }

  if (!s.valid()) {
    std::cerr << "Not valid!" << std::endl;
    return 1;
  }

  std::ofstream file("dist.dat");
  for (unsigned i : clusterDist) {
    file << i << " ";
  }
  file.close();
  */

  return 0;
}