#include <getopt.h>
#include <iomanip>
#include <random>

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

#include "eigen/Eigen/Dense"
#include "eigen/unsupported/Eigen/CXX11/Tensor"
#include "eigen/unsupported/Eigen/CXX11/TensorSymmetry"

using Rng = randutils::random_generator<pcg32>;

using Real = double;
using Vector = Eigen::Matrix<Real, Eigen::Dynamic, 1>;
using Matrix = Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic>;

class Tensor : public Eigen::Tensor<Real, 3> {
  using Eigen::Tensor<Real, 3>::Tensor;

public:
  Matrix operator*(const Vector& x) const {
    std::array<Eigen::IndexPair<int>, 1> ip20 = {Eigen::IndexPair<int>(2, 0)};
    const Eigen::Tensor<Real, 1> xT = Eigen::TensorMap<const Eigen::Tensor<Real, 1>>(x.data(), x.size());
    const Eigen::Tensor<Real, 2> JxT = contract(xT, ip20);
    return Eigen::Map<const Matrix>(JxT.data(), dimension(0), dimension(1));
  }
};

Matrix operator*(const Eigen::Matrix<Real, 1, Eigen::Dynamic>& x, const Tensor& J) {
  std::array<Eigen::IndexPair<int>, 1> ip00 = {Eigen::IndexPair<int>(0, 0)};
  const Eigen::Tensor<Real, 1> xT = Eigen::TensorMap<const Eigen::Tensor<Real, 1>>(x.data(), x.size());
  const Eigen::Tensor<Real, 2> JxT = J.contract(xT, ip00);
  return Eigen::Map<const Matrix>(JxT.data(), J.dimension(1), J.dimension(2));
}

Vector normalize(const Vector& x) {
  return x * sqrt(x.size() / x.squaredNorm());
}

Vector ∂HFromV∂V(const Vector& V, const Matrix& ∂V) {
  return V.transpose() * ∂V;
}

Vector VFromABJx(const Vector& b, const Matrix& A, const Matrix& Jx, const Vector& x) {
  return b + (A + 0.5 * Jx) * x;
}

class Spherical3Spin {
private:
  Tensor J;

public:
  unsigned N;

  Spherical3Spin(unsigned N, Rng& r) : J(N, N, N), N(N) {
    Eigen::StaticSGroup<Eigen::Symmetry<0,1>, Eigen::Symmetry<1,2>> sym123;

    for (unsigned i = 0; i < N; i++) {
      for (unsigned j = i; j < N; j++) {
        for (unsigned k = j; k < N; k++) {
          sym123(J, i, j, k) = r.variate<Real, std::normal_distribution>(0, sqrt(2) / N);
        }
      }
    }
  }

  std::tuple<Real, Vector, Matrix> H_∂H_∂∂H(const Vector& x) const {
    Matrix ∂∂H = J * x / (6 * N);
    Vector ∂H = ∂∂H * x;
    Real   H = ∂H.dot(x);
    return {H, ∂H, ∂∂H};
  }
};

class ConstrainedHeight {
  private:
    Vector x0;
    Real E;
    Spherical3Spin S;

  public:
    unsigned N;

  ConstrainedHeight(unsigned N, Real E, Rng& r) : x0(N), E(E), S(N, r), N(N) {
    for (Real& x0ᵢ : x0) {
      x0ᵢ = r.variate<Real, std::normal_distribution>();
    }

    x0 = normalize(x0);
  }

  Real overlap(const Vector& v) const {
    return v.head(N).dot(x0) / N;
  }

  std::tuple<Vector, Matrix> ∂L_∂∂L(const Vector& v) const {
    Vector x = v.head(N);
    Real ω₀ = v(N);
    Real ω₁ = v(N + 1);

    Real H;
    Vector ∂H∂x;
    Matrix ∂²H∂²x;
    std::tie(H, ∂H∂x, ∂²H∂²x) = S.H_∂H_∂∂H(x);

    Vector ∂L∂x = x0 + ω₀ * x + ω₁ * ∂H∂x;
    Real ∂L∂ω₀ = 0.5 * (x.squaredNorm() - N);
    Real ∂L∂ω₁ = H - E;

    Vector ∂L(N + 2);
    ∂L << ∂L∂x, ∂L∂ω₀, ∂L∂ω₁;

    Matrix ∂L²∂x² = ω₀ * Matrix::Identity(N, N) + ω₁ * ∂²H∂²x;
    Vector ∂²L∂x∂ω₀ = x;
    Vector ∂²L∂x∂ω₁ = ∂H∂x;

    Matrix ∂∂L(N + 2, N + 2);
    ∂∂L << ∂L²∂x², ∂²L∂x∂ω₀, ∂²L∂x∂ω₁, ∂²L∂x∂ω₀.transpose(), 0, 0, ∂²L∂x∂ω₁.transpose(), 0, 0;

    return {∂L, ∂∂L};
  }

  Vector newtonMethod(const Vector& v0, Real γ = 1) {
    Vector v = v0;

    Vector ∂L;
    Matrix ∂∂L;

    while (std::tie(∂L, ∂∂L) = ∂L_∂∂L(v), ∂L.squaredNorm() > 1e-10) {
      v -= γ * ∂∂L.partialPivLu().solve(∂L);
    }

    return v;
  }
};


int main(int argc, char* argv[]) {
  unsigned N = 10;
  double E = 0;
  unsigned samples = 10;

  int opt;

  while ((opt = getopt(argc, argv, "N:E:n:")) != -1) {
    switch (opt) {
    case 'N':
      N = (unsigned)atof(optarg);
      break;
    case 'E':
      E = atof(optarg);
      break;
    case 'n':
      samples = atoi(optarg);
      break;
    default:
      exit(1);
    }
  }

  Rng r;

  ConstrainedHeight M(N, E, r);

  Vector x(N);
  for (Real& xᵢ : x) {
    xᵢ = r.variate<Real, std::normal_distribution>();
  }
  x = normalize(x);

  Vector v0(N + 2);
  v0.head(N) = x;
  v0(N) = r.variate<Real, std::normal_distribution>();
  v0(N+1) = r.variate<Real, std::normal_distribution>();

  std::cout << std::setprecision(15);

  for (unsigned sample = 0; sample < samples; sample++) {
    ConstrainedHeight M(N, E, r);
    Vector v = M.newtonMethod(v0, 0.05);
    std::cout << M.overlap(v) << std::endl;
  }

  return 0;
}