#include <iostream>
#include "tinysplinecpp.h"
#include <cmath>

int main(int argc, char **argv)
{
    std::vector<tinyspline::real> points;

    points.push_back(1.2);
    points.push_back(0.1);
    //angle
    points.push_back(0);
    //minSpeed
    points.push_back(0);
    //maxSpeed
    points.push_back(0.01);

    points.push_back(1.19);
    points.push_back(0.1);
    //angle
    points.push_back(0);
    //minSpeed
    points.push_back(0);
    //maxSpeed
    points.push_back(1.1);

    points.push_back(0.21);
    points.push_back(0.1);
    //angle
    points.push_back(M_PI / 2.0);
    //minSpeed
    points.push_back(0);
    //maxSpeed
    points.push_back(1.1);

    points.push_back(0.2);
    points.push_back(0.1);
    //angle
    points.push_back(M_PI / 2.0);
    //minSpeed
    points.push_back(0);
    //maxSpeed
    points.push_back(0.01);

    tinyspline::BSpline straightDown = tinyspline::Utils::interpolateCubic(&points, 5);

    const int nrSamples = 1000;
    for(int i = 0; i < nrSamples; i++)
    {
        std::vector<tinyspline::real> result = straightDown.eval(i * 1.0 / static_cast<double>(nrSamples - 1)).result();
        std::cout << result[0] << " " << result[1] << " " << result[2] << " " << result[3] << " " << result[4] << std::endl;

    }
    return 0;
}