dop853.hpp

#pragma once
#include <core/adaptive_integrator.hpp>
#include <core/state_creator.hpp>
#include <integrators/ode/dop853_coefficients.hpp>
#include <cmath>
#include <stdexcept>
 
namespace diffeq {
 
template<system_state S>
class DOP853Integrator;
 
template<system_state S>
class DOP853DenseOutputHelper {
public:
    using value_type = typename DOP853Integrator<S>::value_type;
    
    // Dense output for DOP853: ported from Fortran CONTD8
    // CON: continuous output coefficients, size 8*nd
    // ICOMP: index mapping, size nd
    // nd: number of dense output components
    // xold: left endpoint of interval, h: step size
    // x: interpolation point (xold <= x <= xold+h)
    // Returns: interpolated value for component ii at x
    static value_type contd8(
        int ii, value_type x, const value_type* con, const int* icomp, int nd,
        value_type xold, value_type h) {
        int i = -1;
        for (int j = 0; j < nd; ++j) {
            if (icomp[j] == ii) { i = j; break; }
        }
        if (i == -1) {
            throw std::runtime_error("No dense output available for component " + std::to_string(ii));
        }
        value_type s = (x - xold) / h;
        value_type s1 = 1.0 - s;
        value_type conpar = con[i + nd*4] + s * (con[i + nd*5] + s1 * (con[i + nd*6] + s * con[i + nd*7]));
        value_type result = con[i] + s * (con[i + nd] + s1 * (con[i + nd*2] + s * (con[i + nd*3] + s1 * conpar)));
        return result;
    }
};
 
 
template<system_state S>
class DOP853Integrator : public core::AdaptiveIntegrator<S> {
    
 
public:
    using base_type = core::AdaptiveIntegrator<S>;
    using state_type = typename base_type::state_type;
    using time_type = typename base_type::time_type;
    using value_type = typename base_type::value_type;
    using system_function = typename base_type::system_function;
 
    // Fortran default parameters (do not change unless you know what you are doing)
    static constexpr time_type fortran_safety = static_cast<time_type>(0.9);   // SAFE
    static constexpr time_type fortran_fac1 = static_cast<time_type>(0.333);   // FAC1 (min step size factor)
    static constexpr time_type fortran_fac2 = static_cast<time_type>(6.0);     // FAC2 (max step size factor)
    static constexpr time_type fortran_beta = static_cast<time_type>(0.0);     // BETA (step size stabilization)
    static constexpr time_type fortran_dt_max = static_cast<time_type>(1e100); // HMAX (max step size)
    static constexpr time_type fortran_dt_min = static_cast<time_type>(1e-16); // practical min (not in Fortran, but practical)
    static constexpr int fortran_nmax = 100000;                                // NMAX (max steps)
    static constexpr int fortran_nstiff = 1000;                                // NSTIFF (stiffness test interval)
 
    // Internal state (Fortran default values)
    time_type safety_factor_ = fortran_safety;
    time_type fac1_ = fortran_fac1;
    time_type fac2_ = fortran_fac2;
    time_type beta_ = fortran_beta;
    time_type dt_max_ = fortran_dt_max;
    time_type dt_min_ = fortran_dt_min;
    int nmax_ = fortran_nmax;
    int nstiff_ = fortran_nstiff;
    time_type facold_ = static_cast<time_type>(1e-4); // Fortran FACOLD
    // Stiffness detection state
    int iastiff_ = 0;
    int nonsti_ = 0;
    time_type hlamb_ = 0;
    // For statistics (optional)
    int nstep_ = 0;
    int naccpt_ = 0;
    int nrejct_ = 0;
    int nfcn_ = 0;
 
private:
    void check_nan_inf(const std::string& context, const state_type& state, const state_type& y_new, 
                       const state_type& error, time_type dt, time_type err, time_type err2, time_type deno) {
        // Check for NaN/Inf in all vectors and scalars
        for (std::size_t i = 0; i < state.size(); ++i) {
            if (std::isnan(state[i]) || std::isinf(state[i])) {
                throw std::runtime_error("DOP853: NaN/Inf detected in " + context + " state[" + std::to_string(i) + "]=" + std::to_string(state[i]));
            }
        }
        for (std::size_t i = 0; i < y_new.size(); ++i) {
            if (std::isnan(y_new[i]) || std::isinf(y_new[i])) {
                throw std::runtime_error("DOP853: NaN/Inf detected in " + context + " y_new[" + std::to_string(i) + "]=" + std::to_string(y_new[i]));
            }
        }
        for (std::size_t i = 0; i < error.size(); ++i) {
            if (std::isnan(error[i]) || std::isinf(error[i])) {
                throw std::runtime_error("DOP853: NaN/Inf detected in " + context + " error[" + std::to_string(i) + "]=" + std::to_string(error[i]));
            }
        }
        if (std::isnan(dt) || std::isinf(dt)) {
            throw std::runtime_error("DOP853: NaN/Inf detected in " + context + " dt=" + std::to_string(dt));
        }
        if (std::isnan(err) || std::isinf(err)) {
            throw std::runtime_error("DOP853: NaN/Inf detected in " + context + " err=" + std::to_string(err));
        }
        if (std::isnan(err2) || std::isinf(err2)) {
            throw std::runtime_error("DOP853: NaN/Inf detected in " + context + " err2=" + std::to_string(err2));
        }
        if (std::isnan(deno) || std::isinf(deno)) {
            throw std::runtime_error("DOP853: NaN/Inf detected in " + context + " deno=" + std::to_string(deno));
        }
    }
 
    // Compute a good initial step size (HINIT from Fortran)
    time_type compute_initial_step(const state_type& y, time_type t, const system_function& sys, time_type t_end) const {
        // Compute f0 = f(t, y)
        state_type f0 = StateCreator<state_type>::create(y);
        sys(t, y, f0);
 
        // Compute a norm for y and f0
        time_type dnf = 0.0, dny = 0.0;
        for (std::size_t i = 0; i < y.size(); ++i) {
            time_type sk = this->atol_ + this->rtol_ * std::abs(y[i]);
            dnf += (f0[i] / sk) * (f0[i] / sk);
            dny += (y[i] / sk) * (y[i] / sk);
        }
        time_type h = 1e-6;
        if (dnf > 1e-10 && dny > 1e-10) {
            h = std::sqrt(dny / dnf) * 0.01;
        }
        h = std::min<time_type>(h, std::abs(t_end - t));
        h = std::copysign(h, t_end - t);
 
        // Perform an explicit Euler step
        state_type y1 = StateCreator<state_type>::create(y);
        for (std::size_t i = 0; i < y.size(); ++i)
            y1[i] = y[i] + h * f0[i];
        state_type f1 = StateCreator<state_type>::create(y);
        sys(t + h, y1, f1);
 
        // Estimate the second derivative
        time_type der2 = 0.0;
        for (std::size_t i = 0; i < y.size(); ++i) {
            time_type sk = this->atol_ + this->rtol_ * std::abs(y[i]);
            der2 += ((f1[i] - f0[i]) / sk) * ((f1[i] - f0[i]) / sk);
        }
        der2 = std::sqrt(der2) / h;
 
        // Step size is computed such that h^order * max(norm(f0), norm(der2)) = 0.01
        time_type der12 = std::max<time_type>(std::abs(der2), std::sqrt(dnf));
        time_type h1 = h;
        if (der12 > 1e-15) {
            h1 = std::pow(0.01 / der12, 1.0 / 8.0);
        } else {
            h1 = std::max<time_type>(1e-6, std::abs(h) * 1e-3);
        }
        // Avoid std::min(a, b, c) which is not standard C++
        time_type hmax = 100 * std::abs(h);
        time_type htmp = (h1 < hmax) ? h1 : hmax;
        htmp = (htmp < std::abs(t_end - t)) ? htmp : std::abs(t_end - t);
        h = std::copysign(htmp, t_end - t);
        return h;
    }
 
public:
    explicit DOP853Integrator(system_function sys, 
                             time_type rtol = static_cast<time_type>(1e-8),
                             time_type atol = static_cast<time_type>(1e-10))
        : base_type(std::move(sys), rtol, atol) {}
 
    void step(state_type& state, time_type dt) override {
        adaptive_step(state, dt);
    }
 
    // To match Fortran DOP853, we need to know the integration target time for HINIT
    // This version assumes you set target_time_ before calling adaptive_step
    time_type target_time_ = 0; // User must set this before integration
 
    time_type adaptive_step(state_type& state, time_type dt) override {
        // Fortran DOP853: if dt <= 0, estimate initial step size using HINIT (compute_initial_step)
        time_type t = this->current_time_;
        time_type t_end = target_time_;
        if (t_end == t) t_end = t + 1.0; // fallback if not set
        time_type current_dt = dt;
        if (current_dt <= 0) {
            // Use the system function and current state to estimate initial step
            current_dt = compute_initial_step(state, t, this->sys_, t_end);
            // Clamp to allowed min/max
            current_dt = std::max<time_type>(dt_min_, std::min<time_type>(dt_max_, current_dt));
        }
        int attempt = 0;
        for (; attempt < nmax_; ++attempt) {
            
            state_type y_new = StateCreator<state_type>::create(state);
            state_type error = StateCreator<state_type>::create(state);
            dop853_step(state, y_new, error, current_dt);
            nfcn_ += 12; // 12 stages per step
 
            // Check for NaN/Inf in step computation
            check_nan_inf("step_computation", state, y_new, error, current_dt, 0.0, 0.0, 0.0);
 
            // Fortran error norm (ERR, ERR2, DENO, etc.)
            time_type err = 0.0, err2 = 0.0;
            for (std::size_t i = 0; i < state.size(); ++i) {
                time_type sk = this->atol_ + this->rtol_ * std::max<time_type>(std::abs(state[i]), std::abs(y_new[i]));
                // Fortran: ERRI=K4(I)-BHH1*K1(I)-BHH2*K9(I)-BHH3*K3(I)  (here, error[i] is 8th-5th order diff)
                // We use error[i] as the embedded error estimate, so for full Fortran, you may need to store all k's
                err2 += (error[i] / sk) * (error[i] / sk); // proxy for Fortran's ERR2
                err += (error[i] / sk) * (error[i] / sk); // Fortran's ERR
            }
            time_type deno = err + 0.01 * err2;
            if (deno <= 0.0 || std::isnan(deno) || std::isinf(deno)) {
                deno = 1.0;
            }
            err = std::abs(current_dt) * err * std::sqrt(1.0 / (state.size() * deno));
            if (std::isnan(err) || std::isinf(err)) {
                err = 1.0;
            }
 
            // Check for NaN/Inf in error norm calculation
            check_nan_inf("error_norm", state, y_new, error, current_dt, err, err2, deno);
 
            // Fortran: FAC11 = ERR**EXPO1, FAC = FAC11 / FACOLD**BETA
            time_type expo1 = 1.0 / 8.0 - beta_ * 0.2;
            time_type fac11 = std::pow(std::max<time_type>(err, static_cast<time_type>(1e-16)), expo1);
            time_type fac = fac11 / std::pow(facold_, beta_);
            // Clamp fac between fac1_ (min, <1) and fac2_ (max, >1)
            fac = std::min<time_type>(fac2_, std::max<time_type>(fac1_, fac / safety_factor_));
            if (std::isnan(fac) || std::isinf(fac)) {
                fac = 1.0;
            }
            time_type next_dt = current_dt / fac;
            if (next_dt <= 0.0 || std::isnan(next_dt) || std::isinf(next_dt)) {
                next_dt = dt_min_;
            }
 
            if (err <= 1.0) {
                facold_ = std::max<time_type>(err, static_cast<time_type>(1e-4));
                naccpt_++;
                nstep_++;
                state = y_new;
                this->advance_time(current_dt);
                
                // stiffness detection (Fortran HLAMB)
                if (nstiff_ > 0 && (naccpt_ % nstiff_ == 0 || iastiff_ > 0)) {
                    // Compute HLAMB = |h| * sqrt(stnum / stden)
                    time_type stnum = 0, stden = 0;
                    for (std::size_t i = 0; i < state.size(); ++i) {
                        stnum += (error[i]) * (error[i]);
                        stden += (y_new[i] - state[i]) * (y_new[i] - state[i]);
                    }
                    if (stden > 0) hlamb_ = std::abs(current_dt) * std::sqrt(stnum / stden);
                    if (hlamb_ > 6.1) {
                        nonsti_ = 0;
                        iastiff_++;
                        if (iastiff_ == 15) {
                            throw std::runtime_error("DOP853: Problem seems to become stiff");
                        }
                    } else {
                        nonsti_++;
                        if (nonsti_ == 6) iastiff_ = 0;
                    }
                }
                // Clamp next step size
                next_dt = std::max<time_type>(dt_min_, std::min<time_type>(dt_max_, next_dt));
                return next_dt;
            } else {
                // Step rejected
                nrejct_++;
                nstep_++;
                next_dt = current_dt / std::min<time_type>(fac1_, fac11 / safety_factor_);
                current_dt = std::max<time_type>(dt_min_, next_dt);
            }
        }
        throw std::runtime_error("DOP853: Maximum number of step size reductions or steps exceeded");
    }
 
private:
    // Helper functions to access coefficients
    static constexpr time_type get_c(int i) { return diffeq::integrators::ode::dop853::C<time_type>[i]; }
    static constexpr time_type get_a(int i, int j) { return diffeq::integrators::ode::dop853::A<time_type>[i][j]; }
    static constexpr time_type get_b(int i) { return diffeq::integrators::ode::dop853::B<time_type>[i]; }
    static constexpr time_type get_e5(int i) { return diffeq::integrators::ode::dop853::E5<time_type>[i]; }
 
    void dop853_step(const state_type& y, state_type& y_new, state_type& error, time_type dt) {
        // Allocate all needed k vectors
        std::vector<state_type> k(12, StateCreator<state_type>::create(y));
        state_type temp = StateCreator<state_type>::create(y);
        time_type t = this->current_time_;
 
        // k1 = f(t, y)
        this->sys_(t, y, k[0]);
 
        // k2 = f(t + c2*dt, y + dt*a21*k1)
        for (std::size_t i = 0; i < y.size(); ++i)
            temp[i] = y[i] + dt * get_a(1, 0) * k[0][i];
        this->sys_(t + get_c(1) * dt, temp, k[1]);
 
        // k3 = f(t + c3*dt, y + dt*(a31*k1 + a32*k2))
        for (std::size_t i = 0; i < y.size(); ++i)
            temp[i] = y[i] + dt * (get_a(2, 0) * k[0][i] + get_a(2, 1) * k[1][i]);
        this->sys_(t + get_c(2) * dt, temp, k[2]);
 
        // k4 = f(t + c4*dt, y + dt*(a41*k1 + a43*k3))
        for (std::size_t i = 0; i < y.size(); ++i)
            temp[i] = y[i] + dt * (get_a(3, 0) * k[0][i] + get_a(3, 2) * k[2][i]);
        this->sys_(t + get_c(3) * dt, temp, k[3]);
 
        // k5 = f(t + c5*dt, y + dt*(a51*k1 + a53*k3 + a54*k4))
        for (std::size_t i = 0; i < y.size(); ++i)
            temp[i] = y[i] + dt * (get_a(4, 0) * k[0][i] + get_a(4, 2) * k[2][i] + get_a(4, 3) * k[3][i]);
        this->sys_(t + get_c(4) * dt, temp, k[4]);
 
        // k6 = f(t + c6*dt, y + dt*(a61*k1 + a64*k4 + a65*k5))
        for (std::size_t i = 0; i < y.size(); ++i)
            temp[i] = y[i] + dt * (get_a(5, 0) * k[0][i] + get_a(5, 3) * k[3][i] + get_a(5, 4) * k[4][i]);
        this->sys_(t + get_c(5) * dt, temp, k[5]);
 
        // k7 = f(t + c7*dt, y + dt*(a71*k1 + a74*k4 + a75*k5 + a76*k6))
        for (std::size_t i = 0; i < y.size(); ++i)
            temp[i] = y[i] + dt * (get_a(6, 0) * k[0][i] + get_a(6, 3) * k[3][i] + get_a(6, 4) * k[4][i] + get_a(6, 5) * k[5][i]);
        this->sys_(t + get_c(6) * dt, temp, k[6]);
 
        // k8 = f(t + c8*dt, y + dt*(a81*k1 + a84*k4 + a85*k5 + a86*k6 + a87*k7))
        for (std::size_t i = 0; i < y.size(); ++i)
            temp[i] = y[i] + dt * (get_a(7, 0) * k[0][i] + get_a(7, 3) * k[3][i] + get_a(7, 4) * k[4][i] + get_a(7, 5) * k[5][i] + get_a(7, 6) * k[6][i]);
        this->sys_(t + get_c(7) * dt, temp, k[7]);
 
        // k9 = f(t + c9*dt, y + dt*(a91*k1 + a94*k4 + a95*k5 + a96*k6 + a97*k7 + a98*k8))
        for (std::size_t i = 0; i < y.size(); ++i)
            temp[i] = y[i] + dt * (get_a(8, 0) * k[0][i] + get_a(8, 3) * k[3][i] + get_a(8, 4) * k[4][i] + get_a(8, 5) * k[5][i] + get_a(8, 6) * k[6][i] + get_a(8, 7) * k[7][i]);
        this->sys_(t + get_c(8) * dt, temp, k[8]);
 
        // k10 = f(t + c10*dt, y + dt*(a101*k1 + a104*k4 + a105*k5 + a106*k6 + a107*k7 + a108*k8 + a109*k9))
        for (std::size_t i = 0; i < y.size(); ++i)
            temp[i] = y[i] + dt * (get_a(9, 0) * k[0][i] + get_a(9, 3) * k[3][i] + get_a(9, 4) * k[4][i] + get_a(9, 5) * k[5][i] + get_a(9, 6) * k[6][i] + get_a(9, 7) * k[7][i] + get_a(9, 8) * k[8][i]);
        this->sys_(t + get_c(9) * dt, temp, k[9]);
 
        // k11 = f(t + c11*dt, y + dt*(a111*k1 + a114*k4 + a115*k5 + a116*k6 + a117*k7 + a118*k8 + a119*k9 + a1110*k10))
        for (std::size_t i = 0; i < y.size(); ++i)
            temp[i] = y[i] + dt * (get_a(10, 0) * k[0][i] + get_a(10, 3) * k[3][i] + get_a(10, 4) * k[4][i] + get_a(10, 5) * k[5][i] + get_a(10, 6) * k[6][i] + get_a(10, 7) * k[7][i] + get_a(10, 8) * k[8][i] + get_a(10, 9) * k[9][i]);
        this->sys_(t + get_c(10) * dt, temp, k[10]);
 
        // k12 = f(t + dt, y + dt*(a121*k1 + a124*k4 + a125*k5 + a126*k6 + a127*k7 + a128*k8 + a129*k9 + a1210*k10 + a1211*k11))
        for (std::size_t i = 0; i < y.size(); ++i)
            temp[i] = y[i] + dt * (get_a(11, 0) * k[0][i] + get_a(11, 3) * k[3][i] + get_a(11, 4) * k[4][i] + get_a(11, 5) * k[5][i] + get_a(11, 6) * k[6][i] + get_a(11, 7) * k[7][i] + get_a(11, 8) * k[8][i] + get_a(11, 9) * k[9][i] + get_a(11, 10) * k[10][i]);
        this->sys_(t + dt, temp, k[11]);
 
        // 8th order solution (y_new)
        for (std::size_t i = 0; i < y.size(); ++i) {
            y_new[i] = y[i] + dt * (get_b(0) * k[0][i] + get_b(5) * k[5][i] + get_b(6) * k[6][i] + get_b(7) * k[7][i] + get_b(8) * k[8][i] + get_b(9) * k[9][i] + get_b(10) * k[10][i] + get_b(11) * k[11][i]);
        }
 
        // 5th order error estimate (embedded)
        for (std::size_t i = 0; i < y.size(); ++i) {
            error[i] = dt * (get_e5(0) * k[0][i] + get_e5(5) * k[5][i] + get_e5(6) * k[6][i] + get_e5(7) * k[7][i] + get_e5(8) * k[8][i] + get_e5(9) * k[9][i] + get_e5(10) * k[10][i] + get_e5(11) * k[11][i]);
        }
    }
};
 
} // namespace diffeq