#pragma once

#include <eigen3/Eigen/Core>
#include <eigen3/unsupported/Eigen/CXX11/Tensor>

#include "pcg-cpp/include/pcg_random.hpp"
#include "randutils/randutils.hpp"

#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;

  double f = factorial(p);

  Matrix hessian = ((p - 1) * p / f) * Jz;
  Vector gradient = (p / f) * Jzz;
  Scalar hamiltonian = (1 / f) * Jzz.transpose() * z;

  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);

  Vector projectedGradient = (gradient - ((gradient.transpose() * z) * z / (double)z.size())).conjugate();

  double W = projectedGradient.cwiseAbs2().sum();
  Vector dW = hessian * projectedGradient - ((z.transpose() * gradient) * projectedGradient + (z.transpose() * projectedGradient) * (gradient + hessian * z)) / (double)z.size();

  return {W, dW};
}