#include <list>

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/src/Core/CwiseNullaryOp.h>

#include "pcg-cpp/include/pcg_random.hpp"
#include "randutils/randutils.hpp"

template <int D> using Vector = Eigen::Matrix<int, D, 1>;
template <int D> using Matrix = Eigen::Matrix<int, D, D>;

using Rng = randutils::random_generator<pcg32>;

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

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

  Transformation<D> inverse() const {
    return Transformation<D>(L, m.transpose(), -m.transpose() * v);
  }
};

template <typename T> concept State = requires(T v) {
  { v.empty() } -> std::same_as<bool>;
  {v.remove()};
};

template <unsigned D, State S> class HalfEdge;

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

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

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

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