#pragma once

#include <eigen3/Eigen/Cholesky>

#include "p-spin.hpp"

template <class Scalar>
Vector<Scalar> stereographicToEuclidean(const Vector<Scalar>& ζ) {
  unsigned N = ζ.size() + 1;
  Vector<Scalar> z(N);

  Scalar a = ζ.transpose() * ζ;
  Scalar b = 2 * sqrt(N) / (1.0 + a);

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

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

  return b * z;
}

template <class Scalar>
Vector<Scalar> euclideanToStereographic(const Vector<Scalar>& z) {
  unsigned N = z.size();
  Vector<Scalar> ζ(N - 1);

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

  return ζ / (sqrt(N) - z(N - 1));
}

template <class Scalar>
Matrix<Scalar> stereographicJacobian(const Vector<Scalar>& ζ) {
  unsigned N = ζ.size();
  Matrix<Scalar> J(N, N + 1);

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

  for (unsigned i = 0; i < N; i++) {
    for (unsigned j = 0; j < N; j++) {
      J(i, j) = - ζ(i) * ζ(j);

      if (i == j) {
        J(i, j) += 0.5 * b;
      }
    }

    J(i, N) = ζ(i);
  }

  return 4 * sqrt(N + 1) * J / pow(b, 2);
}

template <class Scalar, int p>
std::tuple<Scalar, Vector<Scalar>, Matrix<Scalar>> stereographicHamGradHess(const Tensor<Scalar, p>& J, const Vector<Scalar>& ζ, const Vector<Scalar>& z) {
  auto [hamiltonian, gradZ, hessZ] = hamGradHess(J, z);
  Matrix<Scalar> jacobian = stereographicJacobian(ζ);

  Matrix<Scalar> metric = jacobian * jacobian.adjoint();

  // The metric is Hermitian and positive definite, so a Cholesky decomposition can be used.
  Vector<Scalar> grad = metric.llt().solve(jacobian) * gradZ;
  Matrix<Scalar> hess = jacobian * hessZ * jacobian.transpose();

  return {hamiltonian, grad, hess};
}