#pragma once

#include <eigen3/Eigen/Dense>

#include "tensor.hpp"

#define PSPIN_P 3
const unsigned p = PSPIN_P; // polynomial degree of Hamiltonian

using Scalar = std::complex<double>;
using Vector = Eigen::VectorXcd;
using Matrix = Eigen::MatrixXcd;
using Tensor = Eigen::Tensor<Scalar, PSPIN_P>;

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

  double pBang = factorial(p);

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

  return {hamiltonian, gradient, hessian};
}

std::tuple<double, Vector> WdW(const Tensor& J, const Vector& z) {
  Vector gradient;
  Matrix hessian;
  std::tie(std::ignore, gradient, hessian) = hamGradHess(J, z);

  Scalar zGrad = gradient.transpose() * z;
  double N = z.size();

  Vector projGrad = gradient - (zGrad / N) * z;
  Vector projGradConj = projGrad.conjugate();

  Scalar zProjGrad = z.transpose() * projGradConj;

  double W = projGrad.norm();
  Vector dW = hessian * (projGradConj - (zProjGrad / N) * z) - (zGrad * projGradConj + zProjGrad * gradient) / N;

  return {W, dW};
}