#pragma once

#include <eigen3/Eigen/Dense>

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

template <typename Derived>
Vector<typename Derived::Scalar> normalize(const Eigen::MatrixBase<Derived>& z) {
  return z * sqrt((Real)z.size() / (typename Derived::Scalar)(z.transpose() * z));
}

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;

  Real 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> zDot(const Vector<Scalar>& z, const Vector<Scalar>& dH) {
  return -dH.conjugate() + (dH.dot(z) / z.squaredNorm()) * z.conjugate();
}

template <class Scalar, int p>
std::tuple<Real, 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 = zDot(z, dH);

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

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