#include "randutils/randutils.hpp"
#include <cmath>
#include <eigen3/Eigen/Dense>
#include <fstream>
#include <functional>
#include <iostream>
#include <list>
#include <queue>
#include <random>
#include <set>
#include <vector>

const std::array<std::array<unsigned, 16>, 16> smiley = {
    {{{0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0}},
     {{0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0}},
     {{0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0}},
     {{0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0}},
     {{1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1}},
     {{1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1}},
     {{1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1}},
     {{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1}},
     {{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1}},
     {{1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1}},
     {{1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1}},
     {{1, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 1}},
     {{0, 1, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 1, 0}},
     {{0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0}},
     {{0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0}},
     {{0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0}}}};

template <class U, unsigned D> using Vector = Eigen::Matrix<U, D, 1>;

template <class U, unsigned D> using Matrix = Eigen::Matrix<U, D, D>;

template <class U, unsigned D>
Vector<U, D> diff(U L, Vector<U, D> v1, Vector<U, D> v2) {
  Vector<U, D> v;

  for (unsigned i = 0; i < D; i++) {
    v(i) = std::abs(v1(i) - v2(i));
    if (v(i) > L / 2) {
      v(i) = L - v(i);
    }
  }

  return v;
}

template <class U, unsigned D, class state> class Spin {
public:
  Vector<U, D> x;
  state s;
};

template <class U, unsigned D> class Euclidean {
private:
  U L;

public:
  Vector<U, D> t;
  Matrix<U, D> r;
  Euclidean(U L) : L(L) {
    for (unsigned i = 0; i < D; i++) {
      t(i) = 0;
      r(i, i) = 1;
      for (unsigned j = 1; j < D; j++) {
        r(i, (i + j) % D) = 0;
      }
    }
  }

  Euclidean(U L, Vector<U, D> t0, Matrix<U, D> r0) : L(L) {
    t = t0;
    r = r0;
  }

  template <class state>
  Spin<U, D, state> act(const Spin<U, D, state> &s) const {
    Spin<U, D, state> s_new;

    s_new.x = t + r * s.x;
    s_new.s = s.s;

    for (unsigned i = 0; i < D; i++) {
      s_new.x(i) = fmod(L + s_new.x(i), L);
    }

    return s_new;
  }

  Euclidean act(const Euclidean &x) const {
    Vector<U, D> tnew = r * x.t + t;
    Matrix<U, D> rnew = r * x.r;

    for (unsigned i = 0; i < D; i++) {
      tnew(i) = fmod(L + tnew(i), L);
    }

    Euclidean pnew(this->L, tnew, rnew);

    return pnew;
  }

  Euclidean inverse() const {
    Vector<U, D> tnew = -r.transpose() * t;
    Matrix<U, D> rnew = r.transpose();

    Euclidean pnew(this->L, tnew, rnew);

    return pnew;
  }
};

template <class T, unsigned D> class Dictionary {
private:
  unsigned N;
  T L;
  std::vector<std::set<unsigned>> d;

public:
  Dictionary(unsigned Ni, double Li) : N(Ni), L(Li), d(pow(Ni, D)){};

  unsigned dictionary_index(Vector<T, D> x) const {
    unsigned pos_ind = 0;

    for (unsigned i = 0; i < D; i++) {
      pos_ind += pow(N, i) * (unsigned)std::floor(x(i) * N / L);
    };

    return pos_ind;
  }

  void record(Vector<T, D> x, unsigned ind) {
    d[this->dictionary_index(x)].insert(ind);
  };

  void remove(Vector<T, D> x, unsigned ind) {
    d[this->dictionary_index(x)].erase(ind);
  };

  std::set<unsigned> neighbors(Vector<T, D> x, unsigned depth) const {
    return nearest_neighbors_of(this->dictionary_index(x), depth, {});
  };

  std::set<unsigned> nearest_neighbors_of(unsigned ind, unsigned depth,
                                          std::list<unsigned> ignores) const {
    std::set<unsigned> ns = d[ind];

    if (depth > 0) {
      for (unsigned i = 0; i < D; i++) {
        if (std::none_of(ignores.begin(), ignores.end(), [i](unsigned k) { return i == k; })) {
          std::list<unsigned> ignores_new = ignores;
          ignores_new.push_back(i);
          for (signed j : {-1, 1}) {
            unsigned ni = pow(L, i + 1) * (ind / ((unsigned)pow(L, i + 1))) +
                          fmod(pow(L, i + 1) + ind + j * pow(L, i), pow(L, i + 1));

            std::set<unsigned> nns = nearest_neighbors_of(ni, depth - 1, ignores_new);

            for (unsigned guy : nns) {
              ns.insert(guy);
            }
          }
        }
      }
    }

    return ns;
  };
};

class Quantity {
private:
  double total;
  double total2;
  std::vector<double> C;
  unsigned wait;

public:
  unsigned n;
  std::list<double> hist;

  Quantity(unsigned lag, unsigned wait) : C(lag), wait(wait) {
    n = 0;
    total = 0;
    total2 = 0;
  }

  void add(double x) {
    if (n > wait) {
      hist.push_front(x);
      if (hist.size() > C.size()) {
        hist.pop_back();
        unsigned t = 0;
        for (double a : hist) {
          C[t] += a * x;
          t++;
        }
        total += x;
        total2 += pow(x, 2);
      }
    }
    n++;
  }

  double avg() const { return total / (n - wait); }

  double avg2() const { return total2 / (n - wait); }

  std::vector<double> ρ() const {
    double C0 = C.front() / (n - wait);
    double avg2 = pow(total / (n - wait), 2);

    std::vector<double> ρtmp;

    for (double Ct : C) {
      ρtmp.push_back((Ct / (n - wait) - avg2) / (C0 - avg2));
    }

    return ρtmp;
  }

  std::array<double, 2> τ() const {
    double τtmp = 0.5;
    unsigned M = 1;
    double c = 8.0;

    std::vector<double> ρ_tmp = this->ρ();

    while (c * τtmp > M && M < C.size()) {
      τtmp += ρ_tmp[M];
      M++;
    }

    return {τtmp, 2.0 * (2.0 * M + 1) * pow(τtmp, 2) / (n - wait)};
  }

  double σ() const {
    return 2.0 / (n - wait) * this->τ()[0] * (C[0] / (n - wait) - pow(this->avg(), 2));
  }

  double serr() const { return sqrt(this->σ()); }

  unsigned num_added() const { return n - wait; }
};

template <class U, unsigned D, class state> class Model {
public:
  U L;
  Euclidean<U, D> s0;
  std::vector<Spin<U, D, state>> s;
  Dictionary<U, D> dict;
  std::function<std::set<unsigned>(Model<U, D, state> &, unsigned,
                                   Spin<U, D, state>)>
      neighbors;
  std::function<double(Spin<U, D, state>, Spin<U, D, state>)> Z;
  std::function<double(Spin<U, D, state>)> B;
  std::vector<Matrix<U, D>> mats;
  std::vector<Vector<U, D>> steps;
  long double E;
  Quantity Eq;
  Quantity Cq;

  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(sequences, new_level);
    }
  }

  Model(U L, unsigned N,
        std::function<double(Spin<U, D, state>, Spin<U, D, state>)> Z,
        std::function<double(Spin<U, D, state>)> B,
        std::function<std::set<unsigned>(Model<U, D, state> &, unsigned,
                                         Spin<U, D, state>)>
            ns)
      : L(L), s0(L), dict(N, L), neighbors(ns), Z(Z), B(B), Eq(1000, 1000),
        Cq(1000, 1000) {
    std::array<double, D> ini_sequence;
    ini_sequence.fill(1);
    std::list<std::array<double, D>> sequences;
    sequences.push_back(ini_sequence);

    one_sequences(sequences, D);

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

    for (std::array<double, D> sequence : sequences) {
      Matrix<U, 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);

      Vector<U, D> v;
      for (unsigned i = 0; i < D; i++) {
        if (sequence[i] == 1) {
          v(i) = 0;
        } else {
          v(i) = L / 2;
        }
      }

      steps.push_back(v);
    }

    for (unsigned i = 0; i < D; i++) {
      for (unsigned j = 0; j < D; j++) {
        if (i != j) {
          Matrix<U, 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 {
                m(k, l) = 0;
              }
            }
          }
          mats.push_back(m);
        }
      }
    }
  }

  void update_energy() {
    E = 0;
    for (unsigned i = 0; i < s.size(); i++) {
      for (unsigned j = 0; j < i; j++) {
         E -= Z(s[i], s[j]);
      }

      E -= B(s0.inverse().act(s[i]));
    }
  }

  void step(double T, unsigned ind, Euclidean<U, D> r, std::mt19937 &rng) {
    unsigned cluster_size = 0;
    std::uniform_real_distribution<double> dist(0.0, 1.0);

    std::queue<unsigned> queue;
    queue.push(ind);

    std::vector<bool> visited(s.size() + 1, false);

    while (!queue.empty()) {
      unsigned i = queue.front();
      queue.pop();

      if (!visited[i]) {
        visited[i] = true;

        bool we_are_ghost = i == s.size();

        Spin<U, D, state> si_new;
        Euclidean<U, D> s0_new(L);

        if (we_are_ghost) {
          s0_new = r.act(s0);
        } else {
          si_new = r.act(s[i]);
        }

        for (unsigned j : neighbors(*this, i, si_new)) {
          if (j != i) {
            double p;
            bool neighbor_is_ghost = j == s.size();

            if (we_are_ghost || neighbor_is_ghost) {
              Spin<U, D, state> s0s_old, s0s_new;
              unsigned non_ghost;

              if (neighbor_is_ghost) {
                non_ghost = i;
                s0s_old = s0.inverse().act(s[i]);
                s0s_new = s0.inverse().act(si_new);
              } else {
                non_ghost = j;
                s0s_old = s0.inverse().act(s[j]);
                s0s_new = s0_new.inverse().act(s[j]);
              }

              p = 1.0 - exp(-(B(s0s_new) - B(s0s_old)) / T);
            } else {
              p = 1.0 - exp(-(Z(si_new, s[j]) - Z(s[i], s[j])) / T);
            }

            if (dist(rng) < p) {
              queue.push(j);
            }
          }
        }

        if (we_are_ghost) {
          s0 = s0_new;
        } else {
          dict.remove(s[i].x, i);
          s[i] = si_new;
          dict.record(s[i].x, i);
          cluster_size++;
        }
      }
    }

    Cq.add(cluster_size);
  }

  void wolff(double T, unsigned N, std::mt19937& rng) {
    std::uniform_real_distribution<double> t_dist(0, L);
    std::uniform_int_distribution<unsigned> r_dist(0, D - 1);
    std::uniform_int_distribution<unsigned> ind_dist(0, s.size() - 1);
    std::uniform_int_distribution<unsigned> coin(0, mats.size() + steps.size() - 1);

    for (unsigned i = 0; i < N; i++) {
      Vector<U, D> t;
      Matrix<U, D> m;
      unsigned flip = coin(rng);
      if (flip < mats.size()) {
        for (unsigned j = 0; j < D; j++) {
          t(j) = (U)t_dist(rng);
        }
        m = mats[flip];
      } else {
        for (unsigned j = 0; j < D; j++) {
          for (unsigned k = 0; k < D; k++) {
            if (j == k) {
              m(j, k) = 1;
            } else {
              m(j, k) = 0;
            }
          }
        }

        t = steps[flip - mats.size()];
      }

      Euclidean<U, D> g(L, t, m);

      this->step(T, ind_dist(rng), g, rng);

      this->update_energy();
      Eq.add(E);
    }
  }
};