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

std::tuple< std::vector<double>, std::vector<double>, std::vector<double>>
  adaptive_euler_heun( double f(double, double),
                       double t0, double y0, double tf,
                       double h, std::pair<double, double> h_range,
                       double eps_abs, std::size_t max_iterations ) {
    assert( h > 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 s0{f( t0, q_y.back() )};
        q_d.push( s0 );
        double t1, y1, z1, 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_range.first);

            if ( using_h_min ) {
                h = h_range.first;
            }

            if ( h > h_range.second ) {
                h = h_range.second;
            }

            t1 = t0 + h;

            // If we are beyond the end of the interval [t0, tf],
            // align so tf equals the final bound 'tf'
            if ( t1 > tf ) {
                t1 = tf;
                h = tf - t0;
            }

            // Approximation using Euler's method
            y1 = q_y.back() + h*s0;

            // Approximation using Heun's method
            double s1{f( t0 + h, y1 )};
            z1 = q_y.back() + h*(s0 + s1)/2.0;

            a = 0.9*eps_abs*h/(2.0*std::abs( z1 - y1 ));

		std::cout << "--" << a << std::endl;

            if ( a >= 2.0 ) {
                h = 2.0*h;
		std::cout << "-- doubling h" << std::endl;
            } else if ( a <= 0.5 ) {
                h = 0.5*h;
            } else {
                h = a*h;
            }
	} while ( (a < 0.9) && !using_h_min );

        q_t.push( t1 );
        // Use the better approximation from Heun's method
        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 );
}