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

#include "randutils/randutils.hpp"

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

template <class T, int D> using Matrix = Eigen::Matrix<T, D, D>;

/** brief diff - periodic subtraction of vectors
 */
template <class T, int D> Vector<T, D> diff(T L, Vector<T, D> v1, Vector<T, D> v2) {
  Vector<T, 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 T, int D, class S> class Spin {
public:
  Vector<T, D> x;
  S s;
};

template <class T, int D> class Euclidean {
private:
  T L;

public:
  Vector<T, D> t;
  Matrix<T, D> r;

  /** brief Euclidean - default constructor, constructs the identity
   */
  Euclidean(T 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(T L, Vector<T, D> t0, Matrix<T, D> r0) : L(L) {
    t = t0;
    r = r0;
  }

  template <class S> Spin<T, D, S> act(const Spin<T, D, S>& s) const {
    Spin<T, D, S> 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<T, D> tnew = r * x.t + t;
    Matrix<T, 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<T, D> tnew = -r.transpose() * t;
    Matrix<T, D> rnew = r.transpose();

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

    return pnew;
  }
};

template <class T, int D, class S> class Octree {
private:
  T L;
  unsigned N;
  std::vector<std::set<Spin<T, D, S>*>> data;

public:
  Octree(T L, unsigned depth) : L(L), N(pow(2, depth)), data(pow(N, D)){};

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

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

    assert(pos_ind < data.size());

    return pos_ind;
  }

  void insert(Spin<T, D, S>* s) { data[ind(s->x)].insert(s); };

  void remove(Spin<T, D, S>* s) { data[ind(s->x)].erase(s); };

  std::set<Spin<T, D, S>*> neighbors(const Vector<T, D>& x, unsigned depth) const {
    std::set<Spin<T, D, S>*> ns;
    std::set<unsigned> seen;
    nearest_neighbors_of(ind(x), depth, ns, seen);
    return ns;
  };

  void nearest_neighbors_of(unsigned ind, unsigned depth, std::set<Spin<T, D, S>*>& ns,
                            std::set<unsigned>& seen) const {
    ns.insert(data[ind].begin(), data[ind].end());
    seen.insert(ind);

    if (depth > 0) {
      for (unsigned i = 0; i < D; i++) {
        for (signed j : {-1, 1}) {
          unsigned nind = pow(N, i + 1) * (ind / ((unsigned)pow(N, i + 1))) +
                          fmod(pow(N, i + 1) + ind + j * pow(N, i), pow(N, i + 1));

          if (!seen.contains(nind)) {
            seen.insert(nind);
            nearest_neighbors_of(nind, depth - 1, ns, seen);
          }
        }
      }
    }
  }
};

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, int D, class R, class S> class Model;

template <class U, int D, class R, class S> class measurement {
public:
  virtual void pre_cluster(const Model<U, D, R, S>&, unsigned, const R&){};
  virtual void plain_bond_visited(const Model<U, D, R, S>&, const Spin<U, D, S>*, const Spin<U, D, S>*,
                                  const Spin<U, D, S>&, double){};
  virtual void plain_site_transformed(const Model<U, D, R, S>&, const Spin<U, D, S>*,
                                      const Spin<U, D, S>&){};

  virtual void ghost_bond_visited(const Model<U, D, R, S>&, const Spin<U, D, S>&, const Spin<U, D, S>&,
                                  double){};
  virtual void ghost_site_transformed(const Model<U, D, R, S>&, const R&){};

  virtual void post_cluster(const Model<U, D, R, S>&){};
};

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

template <class U, int D>
std::vector<Vector<U, D>> torus_vecs(U L) {
  std::vector<Vector<U, D>> vecs;
  std::array<double, D> ini_sequence;
  ini_sequence.fill(1);
  std::list<std::array<double, D>> sequences;
  sequences.push_back(ini_sequence);
  sequences.pop_front(); // don't want the identity matrix!
 
  for (std::array<double, D> sequence : sequences) {
    Vector<U, D> v;
    for (unsigned i = 0; i < D; i++) {
      if (sequence[i] == 1) {
        v(i) = 0;
       } else {
        v(i) = L / 2;
       }
     }

     vecs.push_back(v);
   }

  return vecs;
}

template <class U, int D>
std::vector<Matrix<U, D>> torus_mats() {
  std::vector<Matrix<U, 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(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);
  }

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

  return mats;
}

template <class U, int D, class R, class S> class Model {
public:
  U L;
  R s0;
  std::vector<Spin<U, D, S>> s;
  Octree<U, D, S> dict;
  unsigned dDepth;
  unsigned nDepth;
  std::function<double(const Spin<U, D, S>&, const Spin<U, D, S>&)> Z;
  std::function<double(const Spin<U, D, S>&)> B;
  std::function<R(const Model<U, D, R, S>&, randutils::mt19937_rng&)> g;
  randutils::mt19937_rng rng;
  measurement<U, D, R, S>& A;


  Model(U L, std::function<double(const Spin<U, D, S>&, const Spin<U, D, S>&)> Z,
        std::function<double(const Spin<U, D, S>&)> B, std::function<R(const Model<U, D, R, S>&, randutils::mt19937_rng&)> g, unsigned dDepth, unsigned nDepth,
        measurement<U, D, R, S>& A)
      : L(L), s0(L), dict(L, dDepth), Z(Z), B(B), g(g), dDepth(dDepth), nDepth(nDepth), rng(), A(A) {}

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

    std::queue<Spin<U, D, S>*> queue;
    queue.push(&(s[ind]));

    std::set<Spin<U, D, S>*> visited;

    while (!queue.empty()) {
      Spin<U, D, S>* si = queue.front();
      queue.pop();

      if (!visited.contains(si)) {
        visited.insert(si);

        if (si == NULL) {
          Euclidean<U, D> s0_new = r.act(s0);
          for (Spin<U, D, S>& ss : s) {
            Spin<U, D, S> s0s_old = s0.inverse().act(ss);
            Spin<U, D, S> s0s_new = s0_new.inverse().act(ss);
            double p = 1.0 - exp(-(B(s0s_new) - B(s0s_old)) / T);
            A.ghost_bond_visited(*this, s0s_old, s0s_new, p);
            if (dist(rng) < p) {
              queue.push(&ss);
            }
          }
          A.ghost_site_transformed(*this, s0_new);
          s0 = s0_new;
        } else {
          Spin<U, D, S> si_new = r.act(*si);
          std::set<Spin<U, D, S>*> all_neighbors;
          std::set<Spin<U, D, S>*> current_neighbors = dict.neighbors(si->x, nDepth);
          std::set<Spin<U, D, S>*> new_neighbors = dict.neighbors(si_new.x, nDepth);

          all_neighbors.insert(current_neighbors.begin(), current_neighbors.end());
          all_neighbors.insert(new_neighbors.begin(), new_neighbors.end());
          all_neighbors.insert(NULL);
          for (Spin<U, D, S>* sj : all_neighbors) {
            if (sj != si) {
              double p;
              if (sj == NULL) {
                Spin<U, D, S> s0s_old = s0.inverse().act(*si);
                Spin<U, D, S> s0s_new = s0.inverse().act(si_new);
                p = 1.0 - exp(-(B(s0s_new) - B(s0s_old)) / T);
                A.ghost_bond_visited(*this, s0s_old, s0s_new, p);
              } else {
                p = 1.0 - exp(-(Z(*si, *sj) - Z(si_new, *sj)) / T);
                A.plain_bond_visited(*this, si, sj, si_new, p);
              }
              if (dist(rng) < p) {
                queue.push(sj);
              }
            }
          }
          A.plain_site_transformed(*this, si, si_new);
          dict.remove(si);
          *si = si_new;
          dict.insert(si);
          cluster_size++;
        }
      }
    }
  }

  void wolff(double T, unsigned N) {
    std::uniform_int_distribution<unsigned> ind_dist(0, s.size() - 1);

    for (unsigned i = 0; i < N; i++) {
      R g = g(*this, rng);
      unsigned ind = ind_dist(rng);

      A.pre_cluster(*this, ind, g);

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

      A.post_cluster(*this);
    }
  }
};