#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 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 S> class Model;

/*
template <class U, int D, class S>
  class measurement {
    public:
      virtual void pre_cluster(const Model<U, D, S>&, unsigned, const Euclidean<U, D>&) {};
      virtual void plain_bond_visited(const Model<U, D, S>&, const X_t&, double) {};
      virtual void plain_site_transformed(const system<R_t, X_t, G_t>&, const typename G_t::vertex&
v, const X_t&) {};

      virtual void ghost_bond_visited(const system<R_t, X_t, G_t>&, const typename G_t::vertex& v,
const X_t&, const X_t&, double) {}; virtual void ghost_site_transformed(const system<R_t, X_t,
G_t>&, const R_t&) {};

      virtual void post_cluster(unsigned, unsigned, const system<R_t, X_t, G_t>&) {};
  };
  */

template <class U, int D, class S> class Model {
public:
  U L;
  Euclidean<U, D> 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::vector<Matrix<U, D>> mats;
  std::vector<Vector<U, D>> steps;
  double E;

  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, std::function<double(const Spin<U, D, S>&, const Spin<U, D, S>&)> Z,
        std::function<double(const Spin<U, D, S>&)> B, unsigned dDepth, unsigned nDepth)
      : L(L), s0(L), dict(L, dDepth), Z(Z), B(B), dDepth(dDepth), nDepth(nDepth) {
    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<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);
            if (dist(rng) < p) {
              queue.push(&ss);
            }
            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);
              } else {
                p = 1.0 - exp(-(Z(si_new, *sj) - Z(*si, *sj)) / T);
              }
              if (dist(rng) < p) {
                queue.push(sj);
              }
            }
          }
          dict.remove(si);
          *si = si_new;
          dict.insert(si);
          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();
    }
  }
};