// A bit of code from some camera-path stabilization code

void Path::computeSteps(FloatFunction velocity, float totalLength, float stepTime) {
  float curTime = 0.0f;
  float curLength = 0.0f;
  steps.push_back(Vector2(fx(curTime), fy(curTime)));

  while(curLength < totalLength) {
    float vel = velocity(curTime);
    float s = vel * stepTime;

    std::cout << "S: " << s << std::endl;
    std::cout << "CurTime: " << curTime << std::endl;

    // Oh God Why
    float nextTime = findZero([&](float x){
    return romberg([&](float y){
        return sqrt(pow(derivative([&](float z){
            return fx(z);}, y), 2) + pow(derivative([&](float z) {
              return fy(z);}, y), 2));}
        , curTime, x, 3) - s;}
    , curTime + 2 * curTime + 0.01);

    curTime = nextTime;
    std::cout << "NextTime: " << nextTime << std::endl;
    curLength += s;
    steps.push_back(Vector2(fx(curTime), fy(curTime)));
  }
  if(forward)
    position = steps.begin();
  else
    position = steps.end();
}