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

template <typename T>
std::tuple< std::vector<double>, std::vector<T>, std::vector<T> >
  adaptive_dormand_prince( T f(double, T),
                           double t0, T y0, double tf,
                           double h, double min_h,
                           double eps_abs, std::size_t max_iterations ) {
    std::size_t const DIM{7};
    double step[DIM - 1]{1.0/5.0, 3.0/10.0, 4.0/5.0, 8.0/9.0, 1.0, 1.0};

    double tableau[DIM - 1][DIM - 1]{
        {   1.0},
        {   1.0/4.0,        3.0/4},
        {  11.0/9.0,      -14.0/3.0,     40.0/9.0},
        {4843.0/1458.0, -3170.0/243.0, 8056.0/729.0,  -53.0/162.0},
        {9017.0/3168.0,  -355.0/33.0, 46732.0/5247.0,  49.0/176.0, -5103.0/18656.0},
        {  35.0/384.0,      0.0,        500.0/1113.0, 125.0/192.0, -2187.0/6784.0, 11.0/84.0}
    };

    double y_coeff[DIM]{
          5179.0/57600.0,
             0.0,
          7571.0/16695.0,
           393.0/640.0,
        -92097.0/339200.0,
           187.0/2100.0,
             1.0/40.0
    };

    assert( h > 0 );

    std::size_t k{0};
    double delta_t{tf - t0};

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

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

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

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

            if ( using_min_h ) {
                h = min_h;
            }

            t1 = t0 + h;

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

            // The last calculated slope will be the slope used for z1
            T slope;

            for ( std::size_t i{0}; i < DIM - 1; ++i ) {
                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 );
            }

            T slope_y{0.0};

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

            T y1{q_y.back() + h*slope_y};
            z1 = q_y.back() + h*slope;
            a = 0.9*std::pow( eps_abs*h/(delta_t*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_min_h );

        q_t.push( t1 );
        q_y.push( z1 );

        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<T>      y(n);
    std::vector<T>      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 );
}