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

#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::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]--;
      return true;
    } else {
      return false;
    }
  }

  bool remove(Vertex<D>& v) {
    if (v.empty()) {
      return false;
    } else {
      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);
  }

  void sweepGrandCanonical(double z, Rng& r) {
    for (unsigned i = 0; i < size(); i++) {
      if (0.5 < r.uniform(0.0, 1.0)) {
        double pIns = size() * z / (occupancy() + 1);

        if (pIns > r.uniform(0.0, 1.0)) {
          while (true) {
            Vertex<D>& v = r.pick(vertices);
            if (v.empty()) {
              insert(v, r.pick({1, 2, 2, 2, 2, 2, 3, 3, 3, 3}));
              break;
            }
          }
        }
      } else {

        double pDel = density() / z;

        if (pDel > r.uniform(0.0, 1.0)) {
          remove(r.pick(vertices));
        }
      }

      randomLocalExchange(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);
    }
  }

  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 : v0.neighbors) {
        if ((!vn.marked && !vn.visited) && !vn.empty()) {
          q.push(vn);
          vn.visited = true;
        }
      }
      for (Vertex<D>& vn : v0New.neighbors) {
        if ((!vn.marked && !vn.visited) && !vn.empty()) {
          q.push(vn);
          vn.visited = true;
        }
      }

      unsigned last_unsatisfied = 0;

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

        if (!v.marked) {
          if (!overlaps(v).empty()) {
            last_unsatisfied = 0;
            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 : v.neighbors) {
                if ((!vn.marked && !vn.visited) && !vn.empty()) {
                  q.push(vn);
                  vn.visited = true;
                }
              }
              for (Vertex<D>& vn : vNew.neighbors) {
                if ((!vn.marked && !vn.visited) && !vn.empty()) {
                  q.push(vn);
                  vn.visited = true;
                }
              }

              n += 2;
            } else {
            }
          } else {
            q.push(v);
            last_unsatisfied++;
          }
          n += 1;
        }
      }
    } else {
      n = 1;
    }

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

    for (Vertex<D>& v : vertices) {
      v.marked = false;
      v.visited = 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 Tmin = 0.04;
  double Tmax = 0.2;
  double δT = 0.02;

  System<D> s(L, 3);

  Rng r;

  double z = exp(1 / 0.15);

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

  while (s.density() < 0.56) {
    s.sweepGrandCanonical(z, r);
  }

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

  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;
  while (nC / s.size() < 1e5) {
    if (n < 20 * log(i + 1)) {
      n++;
      std::cout << nC / s.size() << " " << 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;
}