#pragma once

#include <eigen3/Eigen/Dense>

#include "tensor.hpp"
#include "factorial.hpp"

template <class Scalar, int p>
std::tuple<Scalar, Vector<Scalar>, Matrix<Scalar>> hamGradHess(const Tensor<Scalar, p>& J, const Vector<Scalar>& z) {
  Matrix<Scalar> Jz = contractDown(J, z); // Contracts J into p - 2 copies of z.
  Vector<Scalar> Jzz = Jz * z;
  Scalar Jzzz = Jzz.transpose() * z;

  double pBang = factorial(p);

  Matrix<Scalar> hessian = ((p - 1) * p / pBang) * Jz;
  Vector<Scalar> gradient = (p / pBang) * Jzz;
  Scalar hamiltonian = Jzzz / pBang;

  return {hamiltonian, gradient, hessian};
}

template <class Scalar>
Vector<Scalar> normalize(const Vector<Scalar>& z) {
  return z * sqrt((double)z.size() / (Scalar)(z.transpose() * z));
}

template <class Scalar>
Vector<Scalar> project(const Vector<Scalar>& z, const Vector<Scalar>& x) {
  Scalar xz = x.transpose() * z;
  return x - (xz / z.squaredNorm()) * z.conjugate();
}

template <class Scalar, int p>
std::tuple<double, Vector<Scalar>> WdW(const Tensor<Scalar, p>& J, const Vector<Scalar>& z) {
  Vector<Scalar> dH;
  Matrix<Scalar> ddH;
  std::tie(std::ignore, dH, ddH) = hamGradHess(J, z);

  Vector<Scalar> dzdt = project(z, dH.conjugate().eval());

  double a = z.squaredNorm();
  Scalar A = (Scalar)(z.transpose() * dzdt) / a;
  Scalar B = dH.dot(z) / a;

  double W = dzdt.squaredNorm();
  Vector<Scalar> dW = ddH * (dzdt - A * z.conjugate())
    + 2 * (conj(A) * B * z).real()
    - conj(B) * dzdt - conj(A) * dH.conjugate();

  return {W, dW};
}