Generalized code to easily allow mixed p-spins.

5 files changed, 129 insertions, 205 deletions

diff --git a/dynamics.hpp b/dynamics.hpp
index 7d04b1a..36b2dfa 100644
--- a/dynamics.hpp
+++ b/dynamics.hpp
@@ -6,22 +6,28 @@
 
 #include "p-spin.hpp"
 
-template <class Real, int p>
-Vector<Real> findMinimum(const Tensor<Real, p>& J, const Vector<Real>& z0, Real ε) {
+template <class Real, int ...ps>
+Vector<Real> findMinimum(const pSpinModel<Real, ps...>& M, const Vector<Real>& z0, Real ε) {
   Vector<Real> z = z0;
   Real λ = 10;
 
-  auto [H, dH, ddH] = hamGradHess(J, z);
+  Real H;
+  Vector<Real> dH;
+  Matrix<Real> ddH;
+  std::tie(H, dH, ddH, std::ignore) = M.hamGradHess(z);
 
   Vector<Real> g = dH - z.dot(dH) * z / (Real)z.size();
-  Matrix<Real> M = ddH - (dH * z.transpose() + z.dot(dH) * Matrix<Real>::Identity(z.size(), z.size()) + (ddH * z) * z.transpose()) / (Real)z.size() + 2.0 * z * z.transpose();
+  Matrix<Real> m = ddH - (dH * z.transpose() + z.dot(dH) * Matrix<Real>::Identity(z.size(), z.size()) + (ddH * z) * z.transpose()) / (Real)z.size() + 2.0 * z * z.transpose();
 
   while (g.norm() / z.size() > ε && λ < 1e8) {
-    Vector<Real> dz = (M + λ * (Matrix<Real>)abs(M.diagonal().array()).matrix().asDiagonal()).partialPivLu().solve(g);
+    Vector<Real> dz = (m + λ * (Matrix<Real>)abs(m.diagonal().array()).matrix().asDiagonal()).partialPivLu().solve(g);
     dz -= z.dot(dz) * z / (Real)z.size();
     Vector<Real> zNew = normalize(z - dz);
 
-    auto [HNew, dHNew, ddHNew] = hamGradHess(J, zNew);
+    Real HNew;
+    Vector<Real> dHNew;
+    Matrix<Real> ddHNew;
+    std::tie(HNew, dHNew, ddHNew, std::ignore) = M.hamGradHess(zNew);
 
     if (HNew * 1.0001 <= H) {
       z = zNew;
@@ -30,7 +36,7 @@ Vector<Real> findMinimum(const Tensor<Real, p>& J, const Vector<Real>& z0, Real
       ddH = ddHNew;
 
       g = dH - z.dot(dH) * z / (Real)z.size();
-      M = ddH - (dH * z.transpose() + z.dot(dH) * Matrix<Real>::Identity(z.size(), z.size()) + (ddH * z) * z.transpose()) / (Real)z.size() + 2.0 * z * z.transpose();
+      m = ddH - (dH * z.transpose() + z.dot(dH) * Matrix<Real>::Identity(z.size(), z.size()) + (ddH * z) * z.transpose()) / (Real)z.size() + 2.0 * z * z.transpose();
 
       λ /= 2;
     } else {
@@ -41,28 +47,28 @@ Vector<Real> findMinimum(const Tensor<Real, p>& J, const Vector<Real>& z0, Real
   return z;
 }
 
-template <class Real, class Scalar, int p>
-Vector<Scalar> findSaddle(const Tensor<Scalar, p>& J, const Vector<Scalar>& z0, Real ε) {
+template <class Real, class Scalar, int ...ps>
+Vector<Scalar> findSaddle(const pSpinModel<Scalar, ps...>& M, const Vector<Scalar>& z0, Real ε) {
   Vector<Scalar> z = z0;
 
   Vector<Scalar> dH;
   Matrix<Scalar> ddH;
-  std::tie(std::ignore, dH, ddH) = hamGradHess(J, z);
+  std::tie(std::ignore, dH, ddH, std::ignore) = M.hamGradHess(z);
 
   Scalar zz = z.transpose() * z;
   Vector<Scalar> g = dH - (z.transpose() * dH) * z / (Real)z.size() + z * (zz - (Real)z.size());
-  Matrix<Scalar> M = ddH - (dH * z.transpose() + (z.transpose() * dH) * Matrix<Scalar>::Identity(z.size(), z.size()) + (ddH * z) * z.transpose()) / (Real)z.size()  + Matrix<Scalar>::Identity(z.size(), z.size()) * (zz - (Real)z.size()) + 2.0 * z * z.transpose();
+  Matrix<Scalar> m = ddH - (dH * z.transpose() + (z.transpose() * dH) * Matrix<Scalar>::Identity(z.size(), z.size()) + (ddH * z) * z.transpose()) / (Real)z.size()  + Matrix<Scalar>::Identity(z.size(), z.size()) * (zz - (Real)z.size()) + 2.0 * z * z.transpose();
 
   while (g.norm() / z.size() > ε) {
-    Vector<Scalar> dz = M.partialPivLu().solve(g);
+    Vector<Scalar> dz = m.partialPivLu().solve(g);
     dz -= z.conjugate().dot(dz) / z.squaredNorm() * z.conjugate();
     z = normalize(z - dz);
 
-    std::tie(std::ignore, dH, ddH) = hamGradHess(J, z);
+    std::tie(std::ignore, dH, ddH, std::ignore) = M.hamGradHess(z);
 
     zz = z.transpose() * z;
     g = dH - (z.transpose() * dH) * z / (Real)z.size() + z * (zz - (Real)z.size());
-    M = ddH - (dH * z.transpose() + (z.transpose() * dH) * Matrix<Scalar>::Identity(z.size(), z.size()) + (ddH * z) * z.transpose()) / (Real)z.size()  + Matrix<Scalar>::Identity(z.size(), z.size()) * (zz - (Real)z.size()) + 2.0 * z * z.transpose();
+    m = ddH - (dH * z.transpose() + (z.transpose() * dH) * Matrix<Scalar>::Identity(z.size(), z.size()) + (ddH * z) * z.transpose()) / (Real)z.size()  + Matrix<Scalar>::Identity(z.size(), z.size()) * (zz - (Real)z.size()) + 2.0 * z * z.transpose();
   }
 
   return z;
@@ -79,16 +85,16 @@ Vector<Scalar> randomVector(unsigned N, Distribution d, Generator& r) {
   return z;
 }
 
-template <class Real, int p, class Distribution, class Generator>
-Vector<Real> randomMinimum(const Tensor<Real, p>& J, Distribution d, Generator& r, Real ε) {
+template <class Real, class Distribution, class Generator, int ...ps>
+Vector<Real> randomMinimum(const pSpinModel<Real, ps...>& M, Distribution d, Generator& r, Real ε) {
   Vector<Real> zSaddle;
   bool foundSaddle = false;
 
   while (!foundSaddle) {
-    Vector<Real> z0 = normalize(randomVector<Real>(J.dimension(0), d, r.engine()));
+    Vector<Real> z0 = normalize(randomVector<Real>(M.dimension(), d, r.engine()));
 
     try {
-      zSaddle = findMinimum(J, z0, ε);
+      zSaddle = findMinimum(M, z0, ε);
       foundSaddle = true;
     } catch (std::exception& e) {}
   }
@@ -96,16 +102,16 @@ Vector<Real> randomMinimum(const Tensor<Real, p>& J, Distribution d, Generator&
   return zSaddle;
 }
 
-template <class Real, class Scalar, int p, class Distribution, class Generator>
-Vector<Scalar> randomSaddle(const Tensor<Scalar, p>& J, Distribution d, Generator& r, Real ε) {
+template <class Real, class Scalar, class Distribution, class Generator, int ... ps>
+Vector<Scalar> randomSaddle(const pSpinModel<Real, ps...>& M, Distribution d, Generator& r, Real ε) {
   Vector<Scalar> zSaddle;
   bool foundSaddle = false;
 
   while (!foundSaddle) {
-    Vector<Scalar> z0 = normalize(randomVector<Scalar>(J.dimension(0), d, r.engine()));
+    Vector<Scalar> z0 = normalize(randomVector<Scalar>(M.dimension(), d, r.engine()));
 
     try {
-      zSaddle = findSaddle(J, z0, ε);
+      zSaddle = findSaddle(M, z0, ε);
       foundSaddle = true;
     } catch (std::exception& e) {}
   }
diff --git a/langevin.cpp b/langevin.cpp
index 86aeb9e..dc3d07d 100644
--- a/langevin.cpp
+++ b/langevin.cpp
@@ -27,63 +27,6 @@ using ComplexTensor = Tensor<Complex, p>;
 
 using Rng = randutils::random_generator<pcg32>;
 
-std::list<std::array<ComplexVector, 2>> saddlesCloserThan(const std::unordered_map<uint64_t, ComplexVector>& saddles, Real δ) {
-  std::list<std::array<ComplexVector, 2>> pairs;
-
-  for (auto it1 = saddles.begin(); it1 != saddles.end(); it1++) {
-    for (auto it2 = std::next(it1); it2 != saddles.end(); it2++) {
-      if ((it1->second - it2->second).norm() < δ) {
-        pairs.push_back({it1->second, it2->second});
-      }
-    }
-  }
-
-  return pairs;
-}
-
-template <class Generator>
-std::tuple<ComplexTensor, ComplexVector, ComplexVector> matchImaginaryEnergies(const ComplexTensor& J0, const ComplexVector& z10, const ComplexVector& z20, Real ε, Real Δ, Generator& r) {
-  Real σ = sqrt(factorial(p) / (Real(2) * pow(z10.size(), p - 1)));
-  complex_normal_distribution<Real> dJ(0, σ, 0);
-
-  ComplexTensor J = J0;
-  ComplexVector z1, z2;
-  Real ΔH = abs(imag(getHamiltonian(J, z10) - getHamiltonian(J, z20))) / z10.size();
-  Real γ = ΔH;
-
-  std::function<void(ComplexTensor&, std::array<unsigned, p>)> perturbJ =
-    [&γ, &dJ, &r] (ComplexTensor& JJ, std::array<unsigned, p> ind) {
-      Complex Ji = getJ<Complex, p>(JJ, ind);
-      setJ<Complex, p>(JJ, ind, Ji + γ * dJ(r.engine()));
-    };
-
-  while (ΔH > ε) {
-    ComplexTensor Jp = J;
-    iterateOver<Complex, p>(Jp, perturbJ);
-
-    try {
-      z1 = findSaddle(Jp, z10, Δ);
-      z2 = findSaddle(Jp, z20, Δ);
-
-      Real Δz = (z1 - z2).norm();
-
-      if (Δz > 1e-2) {
-        Real ΔHNew = abs(imag(getHamiltonian(Jp, z1) - getHamiltonian(Jp, z2))) / z1.size();
-
-        if (ΔHNew < ΔH) {
-          J = Jp;
-          ΔH = ΔHNew;
-          γ = ΔH;
-
-          std::cerr << "Match imaginary energies: Found couplings with ΔH = " << ΔH << std::endl;
-        }
-      }
-    } catch (std::exception& e) {}
-  }
-
-  return {J, z1, z2};
-}
-
 int main(int argc, char* argv[]) {
   // model parameters
   unsigned N = 10; // number of spins
@@ -143,24 +86,29 @@ int main(int argc, char* argv[]) {
 
   Rng r;
 
-  Tensor<Real, p> ReJ = generateCouplings<Real, p>(N, std::normal_distribution<Real>(0, σ), r.engine());
+  pSpinModel<Real, p> pSpin;
+
+  std::get<0>(pSpin.Js) = generateCouplings<Real, p>(N, std::normal_distribution<Real>(0, σ), r.engine());
 
   std::normal_distribution<Real> Red(0, 1);
 
-  RealVector zMin = randomMinimum(ReJ, Red, r, ε);
-  auto [Hr, dHr, ddHr] = hamGradHess(ReJ, zMin);
+  RealVector zMin = randomMinimum(pSpin, Red, r, ε);
+  Real Hr;
+  RealVector dHr;
+  RealMatrix ddHr;
+  std::tie(Hr, dHr, ddHr, std::ignore) = pSpin.hamGradHess(zMin);
   Eigen::EigenSolver<Matrix<Real>> eigenS(ddHr - (dHr * zMin.transpose() + zMin.dot(dHr) * Matrix<Real>::Identity(zMin.size(), zMin.size()) + (ddHr * zMin) * zMin.transpose()) / (Real)zMin.size() + 2.0 * zMin * zMin.transpose());
   std::cout << eigenS.eigenvalues().transpose() << std::endl;
   for (unsigned i = 0; i < N; i++) {
     RealVector zNew = normalize(zMin + 0.01 * eigenS.eigenvectors().col(i).real());
-    std::cout << getHamiltonian(ReJ, zNew) - Hr << " " << real(eigenS.eigenvectors().col(i).dot(zMin)) << std::endl;
+    std::cout << pSpin.getHamiltonian(zNew) - Hr << " " << real(eigenS.eigenvectors().col(i).dot(zMin)) << std::endl;
   }
   std::cout << std::endl;
   getchar();
 
   complex_normal_distribution<Real> d(0, 1, 0);
 
-  ComplexTensor J = ReJ.cast<Complex>();
+  pSpinModel<Complex, p> complexPSpin = pSpin.cast<Complex>();;
   ComplexVector zSaddle = zMin.cast<Complex>();
 
   ComplexVector zSaddleNext;
@@ -168,7 +116,7 @@ int main(int argc, char* argv[]) {
   while (!foundSaddle) {
     ComplexVector z0 = normalize(zSaddle + δ * randomVector<Complex>(N, d, r.engine()));
     try {
-      zSaddleNext = findSaddle(J, z0, ε);
+      zSaddleNext = findSaddle(complexPSpin, z0, ε);
       Real saddleDistance = (zSaddleNext - zSaddle).norm();
       if (saddleDistance / N > 1e-2) {
         std::cout << saddleDistance << std::endl;
@@ -177,27 +125,16 @@ int main(int argc, char* argv[]) {
     } catch (std::exception& e) {}
   }
 
-  auto [H1, dH1, ddH1] = hamGradHess(J, zSaddle);
-  auto [H2, dH2, ddH2] = hamGradHess(J, zSaddleNext);
-
-  Eigen::ComplexEigenSolver<ComplexMatrix> ces;
-  ces.compute(ddH1);
+  Complex H1 = complexPSpin.getHamiltonian(zSaddle);
+  Complex H2 = complexPSpin.getHamiltonian(zSaddleNext);
 
   Real φ = atan2( H2.imag() - H1.imag(), H1.real() - H2.real());
   std::cerr << (zSaddle - zSaddleNext).norm() / (Real)N << " " << φ << " " << H1 * exp(Complex(0, φ)) << " " << H2 * exp(Complex(0, φ)) << std::endl;
-  Real smallestNorm = std::numeric_limits<Real>::infinity();
-  for (Complex z : ces.eigenvalues()) {
-    if (norm(z) < smallestNorm) {
-      smallestNorm = norm(z);
-    }
-  }
-  std::cerr << smallestNorm << std::endl;
-  getchar();
 
-  J = exp(Complex(0, φ)) * J;
+  std::get<0>(complexPSpin.Js) = exp(Complex(0, φ)) * std::get<0>(complexPSpin.Js);
 
-  Cord test(J, zSaddle, zSaddleNext, 3);
-  test.relaxNewton(J, 10, 1, 1e4);
+  Cord test(complexPSpin, zSaddle, zSaddleNext, 3);
+  test.relaxNewton(10, 1, 1e4);
 
   std::cout << test.z0.transpose() << std::endl;
   std::cout << test.z1.transpose() << std::endl;
diff --git a/p-spin.hpp b/p-spin.hpp
index 6111b75..a5f6c9a 100644
--- a/p-spin.hpp
+++ b/p-spin.hpp
@@ -12,66 +12,93 @@ 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, int ... ps>
-std::tuple<Scalar, Vector<Scalar>, Matrix<Scalar>> hamGradHess(const Tensor<Scalar, p>& J, const Tensor<Scalar, ps>& ... Js, const Vector<Scalar>& z) {
-  Matrix<Scalar> Jz = contractDown(J, z); // Contracts J into p - 2 copies of z.
+template <class Scalar>
+std::tuple<Matrix<Scalar>, Tensor<Scalar, 3>> hamGradTensorHelper(const Vector<Scalar>& z, const Tensor<Scalar, 2>& J) {
+  Tensor<Scalar, 3> J3(z.size(), z.size(), z.size());;
+  J3.setZero();
+  Matrix<Scalar> Jz = Eigen::Map<const Matrix<Scalar>>(J.data(), z.size(), z.size());
+
+  return {Jz, J3};
+}
+
+template <class Scalar, int p>
+std::tuple<Matrix<Scalar>, Tensor<Scalar, 3>> hamGradTensorHelper(const Vector<Scalar>& z, const Tensor<Scalar, p>& J) {
+  Tensor<Scalar, 3> J3 = contractDown(J, z);
+  Tensor<Scalar, 1> zT = Eigen::TensorMap<Tensor<const Scalar, 1>>(z.data(), {z.size()});
+  Tensor<Scalar, 2> J3zT = J3.contract(zT, ip00);
+  Matrix<Scalar> Jz = Eigen::Map<const Matrix<Scalar>>(J3zT.data(), z.size(), z.size());
+
+  return {Jz, J3};
+}
+
+template <class Scalar, int p, int... ps>
+std::tuple<Scalar, Vector<Scalar>, Matrix<Scalar>, Tensor<Scalar, 3>> hamGradHessHelper(const Vector<Scalar>& z, const Tensor<Scalar, p>& J, const Tensor<Scalar, ps>& ...Js) {
+  auto [Jz, J3] = hamGradTensorHelper(z, J);
+  
   Vector<Scalar> Jzz = Jz * z;
   Scalar Jzzz = Jzz.transpose() * z;
 
   Real pBang = factorial(p);
 
+  Tensor<Scalar, 3> dddH = ((p - 2) * (p - 1) * p / pBang) * J3;
   Matrix<Scalar> ddH = ((p - 1) * p / pBang) * Jz;
   Vector<Scalar> dH = (p / pBang) * Jzz;
   Scalar H = Jzzz / pBang;
 
   if constexpr (sizeof...(Js) > 0) {
-    auto [Hs, dHs, ddHs] = hamGradHess(Js..., z);
+    auto [Hs, dHs, ddHs, dddHs] = hamGradHess(z, Js...);
     H += Hs;
     dH += dHs;
     ddH += ddHs;
+    dddH += dddHs;
   }
 
-  return {H, dH, ddH};
+  return {H, dH, ddH, dddH};
 }
 
-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 <typename Scalar, int... ps>
+class pSpinModel {
+public:
+  std::tuple<Tensor<Scalar, ps>...> Js;
 
-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;
-}
+  unsigned dimension() const {
+    return std::get<0>(Js).dimension(0);
+  }
 
-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 <typename NewScalar>
+  pSpinModel<NewScalar, ps...> cast() const {
+    pSpinModel<NewScalar, ps...> M;
+    M.Js = std::apply(
+        [] (const Tensor<Scalar, ps>& ...Ks) -> std::tuple<Tensor<NewScalar, ps>...> {
+          return std::make_tuple(Ks.template cast<NewScalar>()...);
+        }, Js
+        );
 
-template <class Scalar>
-Real getThresholdEnergyDensity(unsigned p, Scalar κ, Scalar ε, Real a) {
-  Real apm2 = pow(a, p - 2);
-  Scalar δ = κ / apm2;
-  Real θ = arg(κ) + 2 * arg(ε);
+    return M;
+  }
 
-  return (p - 1) * apm2 / (2 * p) * pow(1 - norm(δ), 2) / (1 + norm(δ) - 2 * abs(δ) * cos(θ));
-}
+  std::tuple<Scalar, Vector<Scalar>, Matrix<Scalar>, Tensor<Scalar, 3>> hamGradHess(const Vector<Scalar>& z) const {
+    return std::apply([&z](const Tensor<Scalar, ps>& ...Ks) -> std::tuple<Scalar, Vector<Scalar>, Matrix<Scalar>, Tensor<Scalar, 3>> { return hamGradHessHelper(z, Ks...); }, Js);
+  }
 
-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;
+  Scalar getHamiltonian(const Vector<Scalar>& z) const {
+    Scalar H;
+    std::tie(H, std::ignore, std::ignore, std::ignore) = hamGradHess(z);
+    return H;
+  }
 
-  return norm(ε) / getThresholdEnergyDensity(p, κ, ε, a);
-}
+  Vector<Scalar> getGradient(const Vector<Scalar>& z) const {
+    Vector<Scalar> dH;
+    std::tie(std::ignore, dH, std::ignore, std::ignore) = hamGradHess(z);
+    return dH;
+  }
+
+  Matrix<Scalar> getHessian(const Vector<Scalar>& z) const {
+    Matrix<Scalar> ddH;
+    std::tie(std::ignore, std::ignore, ddH, std::ignore) = hamGradHess(z);
+    return ddH;
+  }
+};
 
 template <class Scalar>
 Vector<Scalar> zDot(const Vector<Scalar>& z, const Vector<Scalar>& dH) {
diff --git a/stokes.hpp b/stokes.hpp
index b3ae90b..c2b0ec5 100644
--- a/stokes.hpp
+++ b/stokes.hpp
@@ -4,17 +4,18 @@
 #include "complex_normal.hpp"
 #include "dynamics.hpp"
 
-template <class Scalar>
+template <class Scalar, int ...ps>
 class Cord {
 public:
+  const pSpinModel<Scalar, ps...>& M;
   std::vector<Vector<Scalar>> gs;
   Vector<Scalar> z0;
   Vector<Scalar> z1;
+  double φ;
 
-  template <int p>
-  Cord(const Tensor<Scalar, p>& J, const Vector<Scalar>& z2, const Vector<Scalar>& z3, unsigned ng) : gs(ng, Vector<Scalar>::Zero(z2.size())) {
-    Scalar H2 = getHamiltonian(J, z2);
-    Scalar H3 = getHamiltonian(J, z3);
+  Cord(const pSpinModel<Scalar, ps...>& M, const Vector<Scalar>& z2, const Vector<Scalar>& z3, unsigned ng) : M(M), gs(ng, Vector<Scalar>::Zero(z2.size())) {
+    Scalar H2 = M.getHamiltonian(z2);
+    Scalar H3 = M.getHamiltonian(z3);
 
     if (real(H2) > real(H3)) {
       z0 = z2;
@@ -53,64 +54,31 @@ public:
     return z;
   }
 
-  template <int p>
-  Real cost(const Tensor<Scalar, p>& J, Real t) const {
+  Real cost(Real t) const {
     Vector<Scalar> z = f(t);
     Scalar H;
     Vector<Scalar> dH;
-    std::tie(H, dH, std::ignore) = hamGradHess(J, z);
+    std::tie(H, dH, std::ignore, std::ignore) = M.hamGradHess(z);
     Vector<Scalar> ż = zDot(z, dH);
     Vector<Scalar> dz = df(t);
 
     return 1 - real(ż.dot(dz)) / ż.norm() / dz.norm();
   }
 
-  template <int p>
-  Real totalCost(const Tensor<Scalar, p>& J, unsigned nt) const {
+  Real totalCost(unsigned nt) const {
     Real tc = 0;
 
     for (unsigned i = 0; i < nt; i++) {
       Real t = (i + 1.0) / (nt + 1.0);
-      tc += cost(J, t);
+      tc += cost(t);
     }
 
     return tc;
   }
 
-  template <int p>
-  std::vector<Vector<Scalar>> dgs(const Tensor<Scalar, p>& J, Real t) const {
+  std::pair<Vector<Scalar>, Matrix<Scalar>> gsGradHess(Real t) const {
     Vector<Scalar> z = f(t);
-    auto [H, dH, ddH] = hamGradHess(J, z);
-    Vector<Scalar> ż = zDot(z, dH);
-    Vector<Scalar> dz = df(t);
-    Matrix<Scalar> dż = dzDot(z, dH);
-    Matrix<Scalar> dżc = dzDotConjugate(z, dH, ddH);
-
-    std::vector<Vector<Scalar>> x;
-    x.reserve(gs.size());
-
-    for (unsigned i = 0; i < gs.size(); i++) {
-      Real fdg = gCoeff(i, t);
-      Real dfdg = dgCoeff(i, t);
-      Vector<Scalar> dC = - 0.5 / ż.norm() / dz.norm() * (
-          dfdg * ż.conjugate() + fdg * dżc * dz + fdg * dż * dz.conjugate()
-          - real(dz.dot(ż)) * (
-              dfdg * dz.conjugate() / dz.squaredNorm() +
-              fdg * (dżc * ż + dż * ż.conjugate()) / ż.squaredNorm()
-            )
-        );
-
-      x.push_back(dC.conjugate());
-    }
-
-    return x;
-  }
-
-  template <int p>
-  std::pair<Vector<Scalar>, Matrix<Scalar>> gsGradHess(const Tensor<Scalar, p>& J, Real t) const {
-    Vector<Scalar> z = f(t);
-    auto [H, dH, ddH] = hamGradHess(J, z);
-    Tensor<Scalar, 3> dddH = ((p - 2) * (p - 1) * p / (Real)factorial(p)) * J; // BUG IF P != 3
+    auto [H, dH, ddH, dddH] = M.hamGradHess(z);
     Vector<Scalar> ż = zDot(z, dH);
     Tensor<Scalar, 1> żT = Eigen::TensorMap<Tensor<const Scalar, 1>>(ż.data(), {z.size()});
     Vector<Scalar> dz = df(t);
@@ -241,14 +209,13 @@ public:
     return {x, M};
   }
 
-  template <int p>
-  std::pair<Real, Real> relaxStepNewton(const Tensor<Scalar, p>& J, unsigned nt, Real δ₀) {
+  std::pair<Real, Real> relaxStepNewton(unsigned nt, Real δ₀) {
     Vector<Scalar> dgsTot = Vector<Scalar>::Zero(2 * z0.size() * gs.size());
     Matrix<Scalar> HessTot = Matrix<Scalar>::Zero(2 * z0.size() * gs.size(), 2 * z0.size() * gs.size());
 
     for (unsigned i = 0; i < nt; i++) {
       Real t = (i + 1.0) / (nt + 1.0);
-      auto [dgsi, Hessi] = gsGradHess(J, t);
+      auto [dgsi, Hessi] = gsGradHess(t);
 
       dgsTot += dgsi / nt;
       HessTot += Hessi / nt;
@@ -259,7 +226,7 @@ public:
     Cord cNew(*this);
 
     Real δ = δ₀;
-    Real oldCost = totalCost(J, nt);
+    Real oldCost = totalCost(nt);
     Real newCost = std::numeric_limits<Real>::infinity();
 
     Matrix<Scalar> I = abs(HessTot.diagonal().array()).matrix().asDiagonal();
@@ -273,7 +240,7 @@ public:
         }
       }
 
-      newCost = cNew.totalCost(J, nt);
+      newCost = cNew.totalCost(nt);
 
       δ *= 2;
     }
@@ -283,13 +250,12 @@ public:
     return {δ / 2, stepSize};
   }
 
-  template <int p>
-  void relaxNewton(const Tensor<Scalar, p>& J, unsigned nt, Real δ₀, unsigned maxSteps) {
+  void relaxNewton(unsigned nt, Real δ₀, unsigned maxSteps) {
     Real δ = δ₀;
     Real stepSize = std::numeric_limits<Real>::infinity();
     unsigned steps = 0;
     while (stepSize > 1e-7 && steps < maxSteps) {
-      std::tie(δ, stepSize) = relaxStepNewton(J, nt, δ);
+      std::tie(δ, stepSize) = relaxStepNewton(nt, δ);
       δ /= 3;
       steps++;
     }
diff --git a/tensor.hpp b/tensor.hpp
index aa33069..fc99042 100644
--- a/tensor.hpp
+++ b/tensor.hpp
@@ -111,27 +111,15 @@ Tensor<Scalar, p> plantState(const Tensor<Scalar, p>& J, const Vector<Scalar>& z
 }
 
 template <class Scalar>
-Matrix<Scalar> contractDown(const Tensor<Scalar, 2>& J, const Vector<Scalar>& z) {
-  return Eigen::Map<const Matrix<Scalar>>(J.data(), z.size(), z.size());
+Tensor<Scalar, 3> contractDown(const Tensor<Scalar, 3>& J, const Vector<Scalar>& z) {
+  return J;
 }
 
 const std::array<Eigen::IndexPair<int>, 1> ip00 = {Eigen::IndexPair<int>(0, 0)};
 
 template <class Scalar, int r>
-Matrix<Scalar> contractDown(const Tensor<Scalar, r>& J, const Vector<Scalar>& z) {
+Tensor<Scalar, 3> contractDown(const Tensor<Scalar, r>& J, const Vector<Scalar>& z) {
   Tensor<Scalar, 1> zT = Eigen::TensorMap<Tensor<const Scalar, 1>>(z.data(), {z.size()});
   Tensor<Scalar, r - 1> Jz = J.contract(zT, ip00);
   return contractDown(Jz, z);
 }
-
-template <int f, class Scalar>
-Tensor<Scalar, f> contractDownTo(const Tensor<Scalar, f>& J, const Vector<Scalar>& z) {
-  return J;
-}
-
-template <int f, class Scalar, int r>
-Tensor<Scalar, f> contractDownTo(const Tensor<Scalar, r>& J, const Vector<Scalar>& z) {
-  Tensor<Scalar, 1> zT = Eigen::TensorMap<Tensor<const Scalar, 1>>(z.data(), {z.size()});
-  Tensor<Scalar, r - 1> Jz = J.contract(zT, ip00);
-  return contractDownTo<f>(Jz, z);
-}