#include "stereographic.hpp"

template <class Scalar>
class Rope {
  private:
    std::vector<Vector<Scalar>> z;

    template <int p>
    Vector<Scalar> δz(const Tensor<Scalar, p>& J) const {
      Vector<Scalar> dz(z.size());

      for (unsigned i = 1; i < z.size() - 1; i++) {
        Vector<Scalar> z12 = (z[i] + z[i - 1]) / 2.0;
        Vector<Scalar> g;
        std::tie(std::ignore, g, std::ignore) = stereographicHamGradHessHess(J, z12);

        Vector<Scalar> Δ = z[i] - z[i - 1];

        g.normalize();
        Δ.normalize();

        if (abs(arg(g.dot(Δ))) < M_PI / 2) {
          dz[i] = g - Δ;
        } else {
          dz[i] = -g - Δ;
        }
      }

      return dz;
    }

    template <int p>
    void perturb(const Tensor<Scalar, p>& J, double δ) {
      z += δ * this->δz(J);
    }

    void spread() {
      std::vector<double> d(z.size() - 1);
      double l = 0;

      for (unsigned i= 0; i < z.size() - 1; i++) {
        double Δ = (z[i + 1] - z[i]).norm();
        d[i] = Δ;
        l += Δ;
      }

      double a = 0;
      unsigned pos = 0;

      for (unsigned i = 1; i < z.size() - 1; i++) {
        double b = i * l / (z.size() - 1);
        while (b >= a) {
          a += d[pos];
          pos++;
        } 

        z[i] = z[pos] - (a - b) * (z[pos] - z[pos - 1]).normalized();
      }
    }

  public:
    Rope(unsigned N, const Vector<Scalar>& z1, const Vector<Scalar>& z2) : z(N + 2) {
      for (unsigned i = 0; i < N + 2; i++) {
        z[i] = z1 + (z2 - z1) * ((double)i / (N + 1.0));
      }
    }

    template <int p>
    void step(const Tensor<Scalar, p>& J, double δ) {
      this->perturb(J, δ);
      this->spread();
    }

    template <int p>
    double cost(const Tensor<Scalar, p>& J) const {
      return this->δz(J).norm();
    }

    template <int p>
    void relax(const Tensor<Scalar, p>& J, unsigned N, double δ) {
      for (unsigned i = 0; i < N; i++) {
        this->step(J, δ);
      }
    }

    Rope<Scalar> interpolate() const {
      Rope<Scalar> r(2 * z.size() - 1);

      for (unsigned i = 0; i < z.size(); i++) {
        r.z[2 * i] = z[i];
      }

      for (unsigned i = 0; i < z.size() - 1; i++) {
        r.z[2 * i + 1] = (z[i] + z[i + 1]) / 2.0;
      }

      return r;
    }
};