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

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

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

  Vector dzdt = project(z, dH.conjugate());

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

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

  return {W, dW};
}