#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 - (zGrad * projGradConj + (z.transpose() * projGradConj) * (gradient + hessian * z)) / N;
  */

  Vector dH;
  Matrix ddH;
  std::tie(std::ignore, dH, ddH) = hamGradHess(J, z);

  double N = z.size();
  Scalar dHz = (Scalar)(dH.transpose() * z) / N;

  Vector pdH = dH - dHz * z;
  Vector pdHc = pdH.conjugate();

  Scalar pdHcz = pdH.dot(z) / N;

  double W = pdH.squaredNorm();
  Vector dW = ddH * (pdHc - pdHcz * z) - (dHz * pdHc + pdHcz * dH);

  return {W, dW};
}