#pragma once

#include <eigen3/Eigen/Dense>
#include <iterator>

#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, int p>
Scalar getHamiltonian(const Tensor<Scalar, p>& J, const Vector<Scalar>& z) {
  Scalar H;
  std::tie(H, std::ignore, std::ignore) = hamGradHess(J, z);
  return H;
}

template <class Scalar, int p>
Vector<Scalar> getGradient(const Tensor<Scalar, p>& J, const Vector<Scalar>& z) {
  Vector<Scalar> dH;
  std::tie(std::ignore, dH, std::ignore) = hamGradHess(J, z);
  return dH;
}

template <class Scalar, int p>
Matrix<Scalar> getHessian(const Tensor<Scalar, p>& J, const Vector<Scalar>& z) {
  Matrix<Scalar> ddH;
  std::tie(std::ignore, std::ignore, ddH) = hamGradHess(J, z);
  return ddH;
}

template <class Scalar>
Real getThresholdEnergyDensity(unsigned p, Scalar κ, Scalar ε, Real a) {
  Real apm2 = pow(a, p - 2);
  Scalar δ = κ / apm2;
  Real θ = arg(κ) + 2 * arg(ε);

  return (p - 1) * apm2 / (2 * p) * pow(1 - norm(δ), 2) / (1 + norm(δ) - 2 * abs(δ) * cos(θ));
}

template <class Scalar, int p>
Real getProportionOfThreshold(Scalar κ, const Tensor<Scalar, p>& J, const Vector<Scalar>& z) {
  Real N = z.size();
  Scalar ε = getHamiltonian(J, z) / N;
  Real a = z.squaredNorm() / N;

  return norm(ε) / getThresholdEnergyDensity(p, κ, ε, a);
}

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

template <class Scalar>
Matrix<Scalar> dzDot(const Vector<Scalar>& z, const Vector<Scalar>& dH) {
  Real z² = z.squaredNorm();
  return (dH.conjugate() - (dH.dot(z) / z²) * z.conjugate()) * z.adjoint() / z²;
}

template <class Scalar>
Matrix<Scalar> dzDotConjugate(const Vector<Scalar>& z, const Vector<Scalar>& dH, const Matrix<Scalar>& ddH) {
  Real z² = z.squaredNorm();
  return -ddH + (ddH * z.conjugate()) * z.transpose() / z²
          + (z.dot(dH) / z²) * (
              Matrix<Scalar>::Identity(ddH.rows(), ddH.cols()) - z.conjugate() * z.transpose() / z²
            );
}

template <class Scalar>
Tensor<Scalar, 3> ddzDot(const Vector<Scalar>& z, const Vector<Scalar>& dH) {
  Tensor<Scalar, 1> zT = Eigen::TensorMap<Tensor<const Scalar, 1>>(z.data(), {z.size()});
  Tensor<Scalar, 1> dHT = Eigen::TensorMap<Tensor<const Scalar, 1>>(dH.data(), {dH.size()});

  Eigen::array<Eigen::IndexPair<int>, 0> ei = {};

  Scalar z² = z.squaredNorm();

  return - zT.conjugate().contract(dHT.conjugate(), ei).contract(zT.conjugate(), ei) / pow(z², 2)
         - dHT.conjugate().contract(zT.conjugate(), ei).contract(zT.conjugate(), ei) / pow(z², 2)
         + zT.conjugate().contract(zT.conjugate(), ei).contract(zT.conjugate(), ei) * (2.0 * dH.dot(z) / pow(z², 3));
}

template <class Scalar>
Tensor<Scalar, 3> ddzDotConjugate(const Vector<Scalar>& z, const Vector<Scalar>& dH, const Matrix<Scalar>& ddH, const Tensor<Scalar, 3>& dddH) {
  Tensor<Scalar, 1> zT = Eigen::TensorMap<Tensor<const Scalar, 1>>(z.data(), {z.size()});
  Tensor<Scalar, 1> dHT = Eigen::TensorMap<Tensor<const Scalar, 1>>(dH.data(), {dH.size()});
  Tensor<Scalar, 2> ddHT = Eigen::TensorMap<Tensor<const Scalar, 2>>(ddH.data(), {z.size(), z.size()});

  Matrix<Scalar> I = Matrix<Real>::Identity(z.size(), z.size());
  Tensor<Scalar, 2> IT = Eigen::TensorMap<Tensor<const Scalar, 2>>(I.data(), {z.size(), z.size()});

  Eigen::array<Eigen::IndexPair<int>, 0> ei = {};
  Eigen::array<Eigen::IndexPair<int>, 1> ip00 = {Eigen::IndexPair<int>(0, 0)};

  Scalar z² = z.squaredNorm();

  return - dddH + dddH.contract(zT.conjugate(), ip00).contract(zT, ei) / z²
         + IT.contract(ddHT.contract(zT.conjugate(), ip00), ei).shuffle((std::array<int, 3>){0, 2, 1}) / z²
         + ddHT.contract(zT.conjugate(), ip00).contract(IT, ei) / z²
         - zT.conjugate().contract(ddHT.contract(zT.conjugate(), ip00), ei).contract(zT, ei) / pow(z², 2)
         - zT.conjugate().contract(IT, ei) * (z.dot(dH) / pow(z², 2))
         - IT.contract(zT.conjugate(), ei).shuffle((std::array<int, 3>){0, 2, 1}) * (z.dot(dH) / pow(z², 2))
         - ddHT.contract(zT.conjugate(), ip00).contract(zT.conjugate(), ei).contract(zT, ei) / pow(z², 2)
         + zT.conjugate().contract(zT.conjugate(), ei).contract(zT, ei) * (2.0 * z.dot(dH) / pow(z², 3));
}