#include <vector>
#include <queue>
#include <tuple>
#include <cassert>
#include <iostream>
#include <cmath>

std::tuple< std::vector<double>, std::vector<double>, std::vector<double> >
  adaptive_fehlberg( double f(double, double),
                     double t0, double y0, double tf,
                     double h, double h_min,
                     double eps_abs, std::size_t max_iterations ) {
    std::size_t const DIM{6};
    double step[DIM - 1]{1.0/4.0, 3.0/8.0, 12.0/13.0, 1.0, 1.0/2.0};

    double tableau[DIM - 1][DIM - 1]{
        {  1.0},
        {  1.0/4.0,      3.0/4.0},
        {161.0/169.0, -600.0/169.0, 608.0/169.0},
        {439.0/216.0,   -8.0,      3680.0/513.0,  -845.0/4104.0},
        {-16.0/27.0,     4.0,     -7088.0/2565.0, 1859.0/2052.0, -11.0/20.0}	
    };

    double y_coeff[DIM]{
          25.0/216.0,
           0.0,
        1408.0/2565.0,
        2197.0/4104.0,
          -1.0/5.0,
           0.0
    };

    double z_coeff[DIM]{
           16.0/135.0,
            0.0,
         6656.0/12825.0,
        28561.0/56430.0,
           -9.0/50.0,
            2.0/55.0
    };

    assert( h > 0 );

    std::size_t k{0};

    // We will put the data onto queues,
    // ensuring that each operation is O(1)
    std::queue<double> q_t{};
    std::queue<double> q_y{};
    std::queue<double> q_d{};

    q_t.push( t0 );
    q_y.push( y0 );

    for ( std::size_t iterations{0};
         (iterations < max_iterations) && (t0 < tf);
          ++iterations ) {
        double s[DIM]{f( t0, q_y.back() )};
        q_d.push( s[0] );
        double t1, y1, a;
        bool using_h_min;

        do {
            // If 'h' is smaller than 'h_min', then use 'h_min'
            // and terminate this loop
            using_h_min = (h <= h_min);

            if ( using_h_min ) {
                h = h_min;
            }

            t1 = t0 + h;

            if ( t1 > tf ) {
                t1 = tf;
                h = tf - t0;
            }

            for ( std::size_t i{0}; i < DIM - 1; ++i ) {
                double slope{0.0};

                for ( std::size_t j{0}; j <= i; ++j ) {
                    slope += tableau[i][j]*s[j];
                }

                s[i + 1] = f( t0 + h*step[i], q_y.back() + h*step[i]*slope );
            }

            double slope_y{0.0};
            double slope_z{0.0};

            for ( std::size_t i{0}; i < DIM; ++i ) {
                slope_y += y_coeff[i]*s[i];
                slope_z += z_coeff[i]*s[i];
            }

            y1 = q_y.back() + h*slope_y;
            double z1 = q_y.back() + h*slope_z;
            a = 0.9*std::pow( eps_abs*h/(2.0*std::abs( z1 - y1 )), 0.25 );

            if ( a >= 2.0 ) {
                h = 2.0*h;
            } else if ( a <= 0.5 ) {
                h = 0.5*h;
            } else {
                h = a*h;
            }
	} while ( (a < 0.9) && !using_h_min );

        q_t.push( t1 );
        // We cannot use the higher-order approximation, as the higher order
        // approximation may be worse due to the magnitude of the coefficients
        //  - Use Dormand-Prince if you want to use
        //    local extrapolation using z1 instead of y1
        q_y.push( y1 );

        t0 = t1;
    }

    if ( t0 != tf ) {
        throw std::runtime_error( "Too many iterations" );
    }

    // Calculate the slope at the last point
    q_d.push( f( t0, q_y.back() ) );

    assert( (q_t.size() == q_y.size()) && (q_t.size() == q_d.size()) );

    // Create three vectors of the appropriate capacity and copy over
    // the entries from the queues to the corresponding vectors
    std::size_t n{q_t.size()};

    std::vector<double> t(n);
    std::vector<double> y(n);
    std::vector<double> d(n);

    for ( std::size_t k{0}; k < n; ++k ) {
        t[k] = q_t.front();
        y[k] = q_y.front();
        d[k] = q_d.front();
        q_t.pop();
        q_y.pop();
        q_d.pop();
    }

    return std::make_tuple( t, y, d );
}