#include <eigen3/Eigen/Cholesky>

#include "Eigen/src/Core/util/Meta.h"
#include "p-spin.hpp"
#include "unsupported/Eigen/CXX11/src/Tensor/TensorMeta.h"

Vector stereographicToEuclidean(const Vector& ζ) {
  unsigned N = ζ.size() + 1;
  Vector z(N);
  Scalar a = ζ.transpose() * ζ;
  Scalar b = 2 * sqrt(N) / (1.0 + a);

  for (unsigned i = 0; i < N - 1; i++) {
    z(i) = b * ζ(i);
  }

  z(N - 1) = b * (a - 1.0) / 2.0;

  return z;
}

Vector euclideanToStereographic(const Vector& z) {
  unsigned N = z.size();
  Vector ζ(N - 1);

  for (unsigned i = 0; i < N - 1; i++) {
    ζ(i) = z(i) / (sqrt(N) - z(N - 1));
  }

  return ζ;
}

Matrix stereographicJacobian(const Vector& ζ) {
  unsigned N = ζ.size() + 1;
  Matrix J(N - 1, N);

  Scalar b = ζ.transpose() * ζ;

  for (unsigned i = 0; i < N - 1; i++) {
    for (unsigned j = 0; j < N - 1; j++) {
      J(i, j) = - 4 * sqrt(N) * ζ(i) * ζ(j) / pow(1.0 + b, 2);
      if (i == j) {
        J(i, j) += 2 * sqrt(N) * (1.0 + b) / pow(1.0 + b, 2);
      }
    }

    J(i, N - 1) = 4.0 * sqrt(N) * ζ(i) / pow(1.0 + b, 2);
  }

  return J;
}

Matrix stereographicMetric(const Matrix& J) {
  return J * J.adjoint();
}

// Gives the Christoffel symbol, with Γ_(i1, i2)^(i3).
Eigen::Tensor<Scalar, 3> stereographicChristoffel(const Vector& ζ, const Matrix& gInvJacConj) {
  unsigned N = ζ.size() + 1;
  Eigen::Tensor<Scalar, 3> dJ(N - 1, N - 1, N);

  Scalar b = 1.0 + (Scalar)(ζ.transpose() * ζ);

  for (unsigned i = 0; i < N - 1; i++) {
    for (unsigned j = 0; j < N - 1; j++) {
      for (unsigned k = 0; k < N - 1; k++) {
        dJ(i, j, k) = 16 * sqrt(N) * ζ(i) * ζ(j) * ζ(k) / pow(b, 3);
        if (i == j) {
          dJ(i, j, k) -= 4 * sqrt(N) * ζ(k) / pow(b, 2);
        }
        if (i == k) {
          dJ(i, j, k) -= 4 * sqrt(N) * ζ(j) / pow(b, 2);
        }
        if (j == k) {
          dJ(i, j, k) -= 4 * sqrt(N) * ζ(i) / pow(b, 2);
        }
      }
      dJ(i, j, N - 1) = - 16 * sqrt(N) * ζ(i) * ζ(j) / pow(b, 3);
      if (i == j) {
        dJ(i, j, N - 1) += 4 * sqrt(N) * ζ(i) / pow(b, 2);
      }
    }
  }

  std::array<Eigen::IndexPair<int>, 1> ip = {Eigen::IndexPair<int>(2, 1)};

  return dJ.contract(Eigen::TensorMap<Eigen::Tensor<const Scalar, 2>>(gInvJacConj.data(), N - 1, N), ip);
}

std::tuple<Scalar, Vector, Matrix> stereographicHamGradHess(const Tensor& J, const Vector& ζ) {
  Vector grad;
  Matrix hess;

  Matrix jacobian = stereographicJacobian(ζ);
  Vector z = stereographicToEuclidean(ζ);

  std::complex<double> hamiltonian;
  Vector gradZ;
  Matrix hessZ;
  std::tie(hamiltonian, gradZ, hessZ) = hamGradHess(J, z);

  Matrix metric = stereographicMetric(jacobian);

  grad = metric.llt().solve(jacobian) * gradZ;

  /* This is much slower to calculate than the marginal speedup it offers...
  Eigen::Tensor<Scalar, 3> Γ = stereographicChristoffel(ζ, gInvJac.conjugate());
  Vector df = jacobian * gradZ;
  Eigen::Tensor<Scalar, 1> dfT = Eigen::TensorMap<Eigen::Tensor<Scalar, 1>>(df.data(), {df.size()});
  std::array<Eigen::IndexPair<int>, 1> ip = {Eigen::IndexPair<int>(2, 0)};
  Eigen::Tensor<Scalar, 2> H2 = Γ.contract(dfT, ip);
  */

  hess = jacobian * hessZ * jacobian.transpose(); // - Eigen::Map<Matrix>(H2.data(), ζ.size(), ζ.size());

  return {hamiltonian, grad, hess};
}