#include "pch.h"

#include "YCurve.hpp"

#include <iostream>
#include <cassert>

YCurve::YCurve(const float s, const float b, const float m, const float w, const float h, const float maxChannelY)
{
  max = maxChannelY;
  Eigen::RowVectorXf knots(10);
  knots << 0.0f, 0.0f, 0.0f, 0.2f, 0.4f, 0.6f, 0.8f, 1.0f, 1.0f, 1.0f;

  Eigen::MatrixXf ctrls(7, 2);
  ctrls << 0, 0,
    10, s,
    30, b,
    50, m,
    70, w,
    90, h,
    max, 100;
  ctrls.transposeInPlace();

  spline = Eigen::Spline<float, 2, 2>(knots, ctrls);
}


Eigen::MatrixXf* YCurve::evalpts(unsigned int nb)
{
  Eigen::MatrixXf* pts = new Eigen::MatrixXf(nb, 2);

  float delta = 1.0f / (nb - 1.0f);
  unsigned int i;
  float step;

  for (i = 0, step = 0; i < nb; i++, step += delta)
  {
    //Peut mieux faire en récupérant spline(step) dans une variable pour éviter de relancer le calcul
    Eigen::Spline<float, 2, 2>::PointType estim = spline(i * delta);
    (*pts)(i, 0) = estim[0];
    (*pts)(i, 1) = estim[1];
  }

  return pts;
}

