#include <eigen3/Eigen/Dense>
#include <iostream>
#include <list>
#include <vector>
#include <queue>

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

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;
    unsigned old_length = sequences.size();
    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 <unsigned 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 <unsigned D> class State : public Vector<D> {
public:
  State() : Vector<D>(Vector<D>::Zero()) {}

  State(const Vector<D>& v) : Vector<D>(v) {}

  State(unsigned a, signed b) : Vector<D>(Vector<D>::Zero()) { this->operator()(a) = b; }

  State(Rng& r) : State(r.uniform((unsigned)0, D - 1), r.pick({-1, 1})) {}

  bool isEmpty() const { return this->squaredNorm() == 0; }

  void remove() { this->setZero(); }
};

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

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

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

    State<D> apply(const State<D>& x) const {
      return State<D>(m * x);
    }
};

template <unsigned D> class HalfEdge;

template <unsigned D> class Vertex {
public:
  Vector<D> position;
  State<D> state;
  std::vector<HalfEdge<D>> adjacentEdges;
  bool marked;

  bool isEmpty() const { return state.isEmpty(); }
};

template <unsigned D> class HalfEdge {
public:
  Vertex<D>& neighbor;
  Vector<D> Δx;

  HalfEdge(Vertex<D>& n, const Vector<D>& d) : neighbor(n), Δx(d) {}
};

template <unsigned D> class System {
public:
  const unsigned L;
  unsigned N;
  std::vector<Vertex<D>> vertices;

  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) : L(L), N(0), vertices(iPow(L, D)) {
    for (unsigned i = 0; i < iPow(L, D); i++) {
      vertices[i].position = indexToVector(i);
      vertices[i].adjacentEdges.reserve(2 * D);
      vertices[i].marked = false;
    }

    for (unsigned d = 0; d < D; d++) {
      Vector<D> Δx = Vector<D>::Zero();
      Δx[d] = 1;
      for (signed i = 0; i < iPow(L, D); i++) {
        unsigned j = iPow(L, d + 1) * (i / iPow(L, d + 1)) + mod(i + iPow(L, d), pow(L, d + 1));
        vertices[i].adjacentEdges.push_back(HalfEdge<D>(vertices[j], Δx));
        vertices[j].adjacentEdges.push_back(HalfEdge<D>(vertices[i], -Δx));
      }
    }
  }

  std::list<std::reference_wrapper<Vertex<D>>> overlaps(Vertex<D>& v, const State<D>& s,
                                                        bool excludeSelf = false) {
    std::list<std::reference_wrapper<Vertex<D>>> o;

    if (s.isEmpty()) {
      return o;
    }

    if (!v.isEmpty() && !excludeSelf) {
      o.push_back(v);
    }

    for (const HalfEdge<D>& e : v.adjacentEdges) {
      if (!e.neighbor.isEmpty()) {
        if (s.dot(e.Δx) == 1 || e.neighbor.state.dot(e.Δx) == -1) {
          o.push_back(e.neighbor);
        }
      }
    }

    return o;
  }

  bool insert(Vertex<D>& v, const State<D>& s) {
    if (overlaps(v, s).empty()) {
      v.state = s;
      N++;
      return true;
    } else {
      return false;
    }
  }

  bool tryDeletion(Vertex<D>& v) {
    if (v.isEmpty()) {
      return false;
    } else {
      v.state.remove();
      N--;
      return true;
    }
  }

  bool tryRandomMove(Rng& r) {
    Vertex<D>& v = r.pick(vertices);
    State<D> oldState = v.state;
    if (!tryDeletion(v)) {
      return false;
    }

    if (0.2 > r.uniform(0.0, 1.0)) {
      if (insert(v, State<D>(r))) {
        return true;
      }
    } else {
      if (insert(r.pick(v.adjacentEdges).neighbor, State<D>(r))) {
        return true;
      }
    }
    v.state = oldState;
    N++;
    return false;
  }

  bool trySwap(Vertex<D>& v1, Vertex<D>& v2) {
    if (overlaps(v1, v2.state, true).size() == 0 && overlaps(v2, v1.state, true).size() == 0) {
      std::swap(v1.state, v2.state);
      return true;
    } else {
      return false;
    }
  }

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

    return trySwap(v1, v2);
  }

  void setGroundState() {
    N = 0;

    for (Vertex<D>& v : vertices) {
      unsigned a = 0;
      for (unsigned d = 0; d < D; d++) {
        a += (d + 1) * v.position(d);
      }
      a %= 2 * D + 1;

      v.state.setZero() = Vector<D>::Zero();

      if (0 < a && a <= D) {
        v.state(a - 1) = -1;
        N++;
      } else if (D < a) {
        v.state(2 * D - a) = 1;
        N++;
      }
    }
  }

  bool compatible() {
    for (Vertex<D>& v : vertices) {
      if (overlaps(v, v.state, true).size() > 0) {
        return false;
      }
    }

    return true;
  }

  double density() const { return N / pow(L, D); }

  unsigned maxOccupation() const {
    //      return (2 * D * iPow(L, D)) / (2 * D + 1);
    return iPow(L, D);
  }

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

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

        double pDel = N / (z * maxOccupation());

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

      tryRandomMove(r);
      tryRandomSwap(r);
    }
  }

  void sweepLocal(double z, Rng& r) {
    for (unsigned i = 0; i < iPow(L, D); i++) {
      tryRandomMove(r);
    }
  }

  void sweepSwap(double z, Rng& r) {
    for (unsigned i = 0; i < iPow(L, D); i++) {
      tryRandomSwap(r);
    }
  }

  unsigned flipCluster(const Transformation<D>& R, Vertex<D>& v0, Rng& r, bool dry = false) {
    std::queue<std::reference_wrapper<Vertex<D>>> q;
    q.push(v0);

    unsigned n = 0;

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

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

        v.marked = true;
        vNew.marked = true;

        State<D> s = R.apply(v.state);
        State<D> sNew = R.apply(vNew.state);

        std::list<std::reference_wrapper<Vertex<D>>> overlaps1 = overlaps(vNew, s, true);
        std::list<std::reference_wrapper<Vertex<D>>> overlaps2 = overlaps(v, sNew, true);
        overlaps1.splice(overlaps1.begin(), overlaps2);

        for (Vertex<D>& vn : overlaps1) {
          if (!vn.marked) {
            q.push(vn);
          }
        }

        if (!dry) {
          v.state = sNew;
          vNew.state = s;
        }

        n += 1;
      }
    }

    return n;
  }

  void swendsenWang(const Transformation<D>& R, Rng& r) {
    for (Vertex<D>& v : vertices) {
      if (!v.marked) {
        unsigned n = flipCluster(R, v, r, 0.5 < r.uniform(0.0, 1.0));
        std::cerr << n << " ";
      }
    }

    std::cerr << std::endl;

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

void print(const System<2>& s) {
  for (const Vertex<2>& v : s.vertices) {
    if (v.state(0) == 1 && v.state(1) == 0) {
      std::cerr << "▶";
    } else if (v.state(0) == -1 && v.state(1) == 0) {
      std::cerr << "◀";
    } else if (v.state(0) == 0 && v.state(1) == -1) {
      std::cerr << "▲";
    } else if (v.state(0) == 0 && v.state(1) == 1) {
      std::cerr << "▼";
    } else if (v.state(0) == 0 && v.state(1) == 0) {
      std::cerr << " ";
    } else {
      std::cerr << "X";
    }

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

template <unsigned D> Vector<D> randomVector(unsigned L, Rng& r) {
  Vector<D> x;
  for (unsigned i = 0; i < D; i++) {
    x[i] = r.uniform((unsigned)0, L - 1);
  }
  return x;
}

int main() {
  const unsigned D = 3;
  unsigned L = 28;
  unsigned Nmin = 2e2;
  unsigned Nmax = 2e5;
  double Tmin = 0.04;
  double Tmax = 0.2;
  double δT = 0.02;

  System<D> s(L);

  Rng r;

  double z = exp(1 / 0.15);

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

  for (unsigned n = 0; n < 1e3; n++) {
    s.sweepGrandCanonical(z, r);
  }

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


  for (unsigned n = 0; n < 10; n++) {
    s.swendsenWang(Transformation<D>(L, ms, r), r);
  }

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

  /*

  for (unsigned N = Nmin; N <= Nmax; N *= 10) {
    s.setGroundState();
    for (double T = Tmin; T < Tmax; T *= 1 + δT) {
      double z = exp(1 / T);

      for (unsigned n = 0; n < N; n++) {
        s.sweepGrandCanonical(z, r);
      }

      std::cout << L << " " << T << " " << N << " " << δT << " " << 1 / s.density() << std::endl;
    }

    for (double T = Tmax; T > Tmin; T /= 1 + δT) {
      double z = exp(1 / T);

      for (unsigned n = 0; n < N; n++) {
        s.sweepGrandCanonical(z, r);
      }

      std::cout << L << " " << T << " " << N << " " << δT << " " << 1 / s.density() << std::endl;
    }
  }
  */

  return 0;
}