rebound-bind 5.0.0

/**
 * @file 	integrator_bs.c
 * @brief 	BS integration scheme.
 * @author 	Hanno Rein <hanno@hanno-rein.de>
 * @details	This file implements the Gragg-Bulirsch-Stoer integration scheme.
 *          It is a reimplementation of the fortran code by E. Hairer and G. Wanner.
 *          The starting point was the JAVA implementation in hipparchus:
 *          https://github.com/Hipparchus-Math/hipparchus/blob/master/hipparchus-ode/src/main/java/org/hipparchus/ode/nonstiff/GraggBulirschStoerIntegrator.java
 *
 * @section 	LICENSE
 * Copyright (c) 2021 Hanno Rein
 *
 * This file is part of rebound.
 *
 * rebound is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * rebound is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with rebound.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Copyright (c) 2004, Ernst Hairer
 *
 * Redistribution and use in source and binary forms, with or
 * without modification, are permitted provided that the following
 * conditions are met:
 * 
 *  - Redistributions of source code must retain the above copyright
 *      notice, this list of conditions and the following disclaimer.
 *  - Redistributions in binary form must reproduce the above copyright
 *      notice, this list of conditions and the following disclaimer in the
 *      documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "rebound.h"
#include "rebound_internal.h"
#include <string.h> // memset
#include <math.h>
#include <float.h> // for DBL_MAX
#include "gravity.h"
#include "integrator_bs.h"
#include "integrator_trace.h"
#include "binarydata.h"
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) < (b) ? (a) : (b))

// Default configuration parameter. 
// They are hard coded here because it
// is unlikely that these need to be changed by the user.
// static const int maxOrder = 18;
static const int sequence_length = 9; // = maxOrder / 2; 
static const double stepControl1 = 0.65;
static const double stepControl2 = 0.94;
static const double stepControl3 = 0.02;
static const double stepControl4 = 4.0;
static const double orderControl1 = 0.8;
static const double orderControl2 = 0.9;
static const double stabilityReduction = 0.5;
static const int maxIter = 2; // maximal number of iterations for which checks are performed
static const int maxChecks = 1; // maximal number of checks for each iteration

void reb_integrator_bs_step(struct reb_simulation* r, void* p);
void* reb_integrator_bs_create();
void reb_integrator_bs_free(void* state);
const struct reb_binarydata_field_descriptor reb_integrator_bs_field_descriptor_list[];

const struct reb_integrator reb_integrator_bs = {
    .documentation =
    "The Gragg-Bulirsch-Stoer integrator (short BS for Bulirsch-Stoer) is an adaptive integrator "
    "which uses Richardson extrapolation and the modified midpoint method to obtain solutions to "
    "ordinary differential equations. The version in REBOUND is based on the method described in "
    "Hairer, Norsett, and Wanner 1993 (see section II.9, page 224ff), specifically the JAVA "
    "implementation available in the [Hipparchus package]. The Hipparchus and the REBOUND "
    "versions are adaptive in both the timestep and the order of the method for optimal performance. "
    "The BS implementation in REBOUND can integrate first and second order variational equations. "
    "The BS integrator is particularly useful for short integrations where only medium accuracy is "
    "required. For long integrations a symplectic integratoris such as WHFast perform better. "
    "For high accuracy integrations the IAS15 integrator generally performs better. "
    "Because BS is adaptive, it can handle close encounters. Currently a collision search is "
    "only performed after every timestep, i.e. not after a every sub-timestep. "
    "The BS integrator tries to keep the error of each coordinate $y$ below $\\epsilon_{abs} + "
    "\\epsilon_{rel} \\cdot  \\left|y\\right|$. Note that this applies to both position and "
    "velocity coordinates of all particles which implies that the code units you're "
    "choosing for the integration matter. If you need fine control over the scales "
    "used internally, you can set the `getscale` function pointer in `nbody_ode`. "
    "\n\n"
    "Note: The code does not guarantee that the errors remain below the tolerances. "
    "In particular, note that BS is not a symplectic integrator which results "
    "in errors growing linearly in time (phase errors grow quadratically in time). "
    "It requires some experimentation to find the tolerances that offer the best "
    "compromise between accuracy and speed for your specific problem. "
    "\n\n"
    "Compared to the other integrators in REBOUND, BS can be used to integrate arbitrary "
    "ordinary differential equations (ODEs), not just the N-body problem. REBOUND exposes an "
    "ODE-API which allows you to make use of this. User-defined ODEs are always integrated "
    "with BS. You can choose to integrate the N-body equations with BS as well, or any of the "
    "other integrators. If you choose BS for the N-body equations, then BS will treat all "
    "ODEs (N-body + all user-defined ones) as one big system of coupled ODEs. This means "
    "your timestep will be set by either the N-body problem or the user-defined ODEs, "
    "whichever involves the shorter timescale."
    "\n\n"
    "If you choose IAS15 or WHFast for the N-body equation but also have user-defined ODEs, "
    "then they cannot be treated as one big coupled system of ODEs anymore. In that case "
    "the N-body integration is done first. Then the user-defined ODEs are advanced to the "
    "exact same time as the N-body system using BS using whatever timestep is required "
    "to achieve the set tolerance. During the integration of the user-defined ODEs, "
    "the coordinates of the particles in the N-body simulation are assumed to be fixed "
    "at their final position and velocity. This introduces an error. However, if the system "
    "evolves adiabatically (the timescales in the user-defined ODEs are much longer than "
    "in the N-body problem), then the error will be small. "
    "\n\n"
    "[Hipparchus package]: https://github.com/Hipparchus-Math/hipparchus/blob/master/hipparchus-ode/src/main/java/org/hipparchus/ode/nonstiff/GraggBulirschStoerIntegrator.java\n"
    ,
    .step = reb_integrator_bs_step,
    .create = reb_integrator_bs_create,
    .free = reb_integrator_bs_free,
    .field_descriptor_list = reb_integrator_bs_field_descriptor_list,
};

const struct reb_binarydata_field_descriptor reb_integrator_bs_field_descriptor_list[] = {
    { "Sets the absolute tolerance for both position and velocity coordinates", 
        REB_DOUBLE,      "eps_abs",                offsetof(struct reb_integrator_bs_state, eps_abs), 0, 0, 0},
    { "Sets the relative tolerance for both position and velocity coordinates", 
        REB_DOUBLE,      "eps_rel",                offsetof(struct reb_integrator_bs_state, eps_rel), 0, 0, 0},
    { "Minimum allowed timestep for the adaptive timestepping algorithm", 
        REB_DOUBLE,      "min_dt",                 offsetof(struct reb_integrator_bs_state, min_dt), 0, 0, 0},
    { "Maximum allowed timestep for the adaptive timestepping algorithm", 
        REB_DOUBLE,      "max_dt",                 offsetof(struct reb_integrator_bs_state, max_dt), 0, 0, 0},
    { "This flag can be set to 1 to indicate that particle data has changed in between steps", 
        REB_INT,         "first_or_last_step",     offsetof(struct reb_integrator_bs_state, first_or_last_step), 0, 0, 0},
    { "", REB_INT,         "previous_rejected",      offsetof(struct reb_integrator_bs_state, previous_rejected), 0, 0, 0},
    { "", REB_INT,         "target_iter",            offsetof(struct reb_integrator_bs_state, target_iter), 0, 0, 0},
    { 0 }, // Null terminated list
};



void* reb_integrator_bs_create(){
    struct reb_integrator_bs_state* bs = calloc(sizeof(struct reb_integrator_bs_state),1);
    // Default settings
    bs->eps_abs          = 1e-8;
    bs->eps_rel          = 1e-8;
    bs->max_dt           = 0;
    bs->min_dt           = 0; 
    bs->first_or_last_step= 1;
    bs->previous_rejected = 0;
    bs->target_iter       = 0;
    bs->user_ode_needs_nbody = 0;

    // Allocate and init sequence arrays (not: these are independent of number of particles/ODEs)
    bs->sequence            = malloc(sizeof(int)*sequence_length);
    bs->cost_per_step       = malloc(sizeof(int)*sequence_length);
    bs->coeff               = malloc(sizeof(double)*sequence_length);
    bs->cost_per_time_unit  = malloc(sizeof(double)*sequence_length);
    bs->optimal_step        = malloc(sizeof(double)*sequence_length);

    for (int k = 0; k < sequence_length; ++k) {
        // step size sequence: 2, 6, 10, 14, ... 
        bs->sequence[k] = 4 * k + 2;
        // initialize the extrapolation tables
        double r = 1./((double) bs->sequence[k]);
        bs->coeff[k] = r*r;
    }

    // initialize the order selection cost array
    // (number of function calls for each column of the extrapolation table)
    bs->cost_per_step[0] = bs->sequence[0] + 1;
    for (int k = 1; k < sequence_length; ++k) {
        bs->cost_per_step[k] = bs->cost_per_step[k - 1] + bs->sequence[k];
    }
    bs->cost_per_time_unit[0]       = 0;

    return bs;
}

void reb_integrator_bs_free(void* state){
    struct reb_integrator_bs_state* bs = state;

    reb_ode_free(bs->nbody_ode);
    free(bs->sequence);
    free(bs->coeff);
    free(bs->cost_per_step);
    free(bs->cost_per_time_unit);
    free(bs->optimal_step);
    free(bs);
}

void reb_integrator_bs_update_particles(struct reb_simulation* r, const double* y){
    if (r==NULL){
        reb_simulation_error(r, "Update particles called without valid simulation pointer.");
        return;
    }
    if (y==NULL){
        reb_simulation_error(r, "Update particles called without valid y pointer.");
        return;
    }
    for (size_t i=0; i<r->N; i++){
        struct reb_particle* const p = &(r->particles[i]);
        p->x  = y[i*6+0];
        p->y  = y[i*6+1];
        p->z  = y[i*6+2];
        p->vx = y[i*6+3];
        p->vy = y[i*6+4];
        p->vz = y[i*6+5];
    }
    for (size_t i=0; i<r->N_var; i++){
        struct reb_particle* const p = &(r->particles_var[i]);
        size_t io = (i+r->N)*6;
        p->x  = y[io+0];
        p->y  = y[io+1];
        p->z  = y[io+2];
        p->vx = y[io+3];
        p->vy = y[io+4];
        p->vz = y[io+5];
    }
}


static int tryStep(struct reb_simulation* r, struct reb_integrator_bs_state* bs, const int Ns, const int k, const int n, const double t0, const double step) {
    struct reb_ode** odes = r->odes;
    const double subStep  = step / n;
    double t = t0;
    int needs_nbody = bs->user_ode_needs_nbody;

    // Modified Midpoint method
    // first substep
    t += subStep;
    for (int s=0; s < Ns; s++){
        double* y0 = odes[s]->y;
        double* y1 = odes[s]->y1;
        double* y0Dot = odes[s]->y0Dot;
        const int length = odes[s]->length;
        for (int i = 0; i < length; ++i) {
            y1[i] = y0[i] + subStep * y0Dot[i];
        }
    }

    // other substeps
    if (needs_nbody){
        // Particle do not get updated if user provided ODE does not need N-body data.
        // Also not getting updates if integrator for N-body ODEs is not BS.
        reb_integrator_bs_update_particles(r, bs->nbody_ode->y1);
    }
    for (int s=0; s < Ns; s++){
        odes[s]->derivatives(odes[s], odes[s]->yDot, odes[s]->y1, t);
    }
    for (int s=0; s < Ns; s++){
        double* y0 = odes[s]->y;
        double* yTmp = odes[s]->yTmp;
        const int length = odes[s]->length;
        for (int i = 0; i < length; ++i) {
            yTmp[i] = y0[i];
        }
    }

    for (int j = 1; j < n; ++j) {  // Note: iterating n substeps, not 2n substeps as in Eq. (9.13)
        t += subStep;
        for (int s=0; s < Ns; s++){
            double* y1 = odes[s]->y1;
            double* yDot = odes[s]->yDot;
            double* yTmp = odes[s]->yTmp;
            const int length = odes[s]->length;
            for (int i = 0; i < length; ++i) {
                const double middle = y1[i];
                y1[i]       = yTmp[i] + 2.* subStep * yDot[i];
                yTmp[i]       = middle;
            }
        }

        if (needs_nbody){
            reb_integrator_bs_update_particles(r, bs->nbody_ode->y1);
        }
        for (int s=0; s < Ns; s++){
            odes[s]->derivatives(odes[s], odes[s]->yDot, odes[s]->y1, t);
        }

        // stability check
        if (j <= maxChecks && k < maxIter) {
            double initialNorm = 0.0;
            double deltaNorm = 0.0;
            for (int s=0; s < Ns; s++){
                double* yDot = odes[s]->yDot;
                double* y0Dot = odes[s]->y0Dot;
                double* scale = odes[s]->scale;
                const int length = odes[s]->length;
                for (int l = 0; l < length; ++l) {
                    const double ratio1 = y0Dot[l] / scale[l];
                    initialNorm += ratio1 * ratio1;
                    const double ratio2 = (yDot[l] - y0Dot[l]) / scale[l];
                    deltaNorm += ratio2 * ratio2;
                }
            }
            if (deltaNorm > 4 * MAX(1.0e-15, initialNorm)) {
                return 0;
            }
        }

    }

    // correction of the last substep (at t0 + step)
    for (int s=0; s < Ns; s++){
        double* y1 = odes[s]->y1;
        double* yTmp = odes[s]->yTmp;
        double* yDot = odes[s]->yDot;
        const int length = odes[s]->length;
        for (int i = 0; i < length; ++i) {
            y1[i] = 0.5 * (yTmp[i] + y1[i] + subStep * yDot[i]); 
        }
    }

    return 1;
}

static void extrapolate(const struct reb_ode* ode, double * const coeff, const int k) {
    double* const y1 = ode->y1;
    double* const C = ode->C;
    double** const D =  ode->D;
    double const length = ode->length;
    for (int j = 0; j < k; ++j) {
        double xi = coeff[k-j-1];
        double xim1 = coeff[k];
        double facC = xi/(xi-xim1);
        double facD = xim1/(xi-xim1);
        for (int i = 0; i < length; ++i) {
            double CD = C[i] - D[k - j -1][i];
            C[i] = facC * CD;
            D[k - j - 1][i] = facD * CD;
        }
    }
    for (int i = 0; i < length; ++i) {
        y1[i] = D[0][i];
    }
    for (int j = 1; j <= k; ++j) {
        for (int i = 0; i < length; ++i) {
            y1[i] += D[j][i];
        }
    }
}


void reb_integrator_bs_nbody_derivatives(struct reb_ode* ode, double* const yDot, const double* const y, double const t){
    struct reb_simulation* const r = ode->r;
    const double t_backup = r->t;
    r->t = t; // Set correct time for time dependent additional forces
    reb_integrator_bs_update_particles(r, y);
    reb_simulation_update_acceleration(r);
    r->t = t_backup;

    for (size_t i=0; i<r->N; i++){
        const struct reb_particle p = r->particles[i];
        yDot[i*6+0] = p.vx;
        yDot[i*6+1] = p.vy;
        yDot[i*6+2] = p.vz;
        yDot[i*6+3] = p.ax;
        yDot[i*6+4] = p.ay;
        yDot[i*6+5] = p.az;
    }
    for (size_t i=0; i<r->N_var; i++){
        const struct reb_particle p = r->particles_var[i];
        size_t io = (i+r->N)*6;
        yDot[io+0] = p.vx;
        yDot[io+1] = p.vy;
        yDot[io+2] = p.vz;
        yDot[io+3] = p.ax;
        yDot[io+4] = p.ay;
        yDot[io+5] = p.az;
    }
}


static void reb_integrator_bs_default_scale(struct reb_ode* ode, double* y1, double* y2, double relTol, double absTol){
    double* scale = ode->scale;
    int length = ode->length;
    for (int i = 0; i < length; i++) {
        scale[i] = absTol + relTol * MAX(fabs(y1[i]), fabs(y2[i]));
    }
}

// Performs one step on all ODEs in r->odes. 
// Gravity not included automatically. Is added
// in reb_integrator_bs_step()
int reb_integrator_bs_step_odes(struct reb_simulation* r, struct reb_integrator_bs_state* bs, double dt){
    // return 1 if step was successful
    //        0 if rejected 
    double t = r->t;
    bs->dt_proposed = dt; // In case of early fail

    // initial order selection
    if (bs->target_iter == 0){
        const double tol    = bs->eps_rel;
        const double log10R = log10(MAX(1.0e-10, tol));
        bs->target_iter = MAX(1, MIN(sequence_length - 2, (int) floor(0.5 - 0.6 * log10R)));
    }

    // maxError not used at the moment.
    // double  maxError = DBL_MAX;

    size_t Ns = r->N_odes; // Number of ode sets
    struct reb_ode** odes = r->odes;
    double error;
    int reject = 0;

    // Check if ODEs have been set up correctly 
    for (size_t s=0; s < Ns; s++){
        if (!odes[s]->derivatives){
            reb_simulation_error(r,"A user-specified set of ODEs has not been provided with a derivatives function.");
            r->status = REB_STATUS_GENERIC_ERROR;
            return 0;
        }
    }

    for (size_t s=0; s < Ns; s++){
        // Check if ODEs need pre timestep setup
        if (odes[s]->pre_timestep){
            odes[s]->pre_timestep(odes[s], odes[s]->y);
        }
        // Scaling
        if (odes[s]->getscale){
            odes[s]->getscale(odes[s], odes[s]->y, odes[s]->y); // initial scaling
        }else{
            reb_integrator_bs_default_scale(odes[s], odes[s]->y, odes[s]->y, bs->eps_rel, bs->eps_abs);
        }
    }

    // first evaluation, at the beginning of the step
    for (size_t s=0; s < Ns; s++){
        odes[s]->derivatives(odes[s], odes[s]->y0Dot, odes[s]->y, t);
    }

    const int forward = (dt >= 0.);

    // iterate over several substep sizes
    int k = -1;
    for (int loop = 1; loop; ) {

        ++k;

        // modified midpoint integration with the current substep
        if ( ! tryStep(r, bs, Ns, k, bs->sequence[k], t, dt)) {

            // the stability check failed, we reduce the global step
            dt  = fabs(dt * stabilityReduction);
            reject = 1;
            loop   = 0;

        } else {
            for (size_t s=0; s < Ns; s++){
                const size_t length = odes[s]->length;
                for (size_t i = 0; i < length; ++i) {
                    double CD = odes[s]->y1[i];
                    odes[s]->C[i] = CD;
                    odes[s]->D[k][i] = CD;
                }
            }

            // the substep was computed successfully
            if (k > 0) {

                // extrapolate the state at the end of the step
                // using last iteration data
                for (size_t s=0; s < Ns; s++){
                    extrapolate(odes[s], bs->coeff, k);
                    if (odes[s]->getscale){
                        odes[s]->getscale(odes[s], odes[s]->y, odes[s]->y1);
                    }else{
                        reb_integrator_bs_default_scale(odes[s], odes[s]->y, odes[s]->y, bs->eps_rel, bs->eps_abs);
                    }
                }

                // estimate the error at the end of the step.
                error = 0;
                //long int combined_length = 0;
                for (size_t s=0; s < Ns; s++){
                    const int length = odes[s]->length;
                    //combined_length += length;
                    double * C = odes[s]->C;
                    double * scale = odes[s]->scale;
                    for (int j = 0; j < length; ++j) {
                        const double e = C[j] / scale[j];
                        error = MAX(error, e * e);
                    }
                }
                // Note: Used to be: error = sqrt(error / combined_length). But for N-body applications it might be more consistent to use:
                error = sqrt(error);
                if (isnan(error)) {
                    reb_simulation_error(r, "NaN appearing during ODE integration.");
                    r->status = REB_STATUS_GENERIC_ERROR;
                    return 0;
                }

                if ((error > 1.0e25)){ // TODO: Think about what to do when error increases: || ((k > 1) && (error > maxError))) 
                                       // error is too big, we reduce the global step
                    dt  = fabs(dt * stabilityReduction);
                    reject = 1;
                    loop   = 0;
                } else {

                    // Not used at the moment
                    // maxError = MAX(4 * error, 1.0);

                    // compute optimal stepsize for this order
                    const double exp = 1.0 / (2 * k + 1);
                    double fac = stepControl2 / pow(error / stepControl1, exp);
                    const double power = pow(stepControl3, exp);
                    fac = MAX(power / stepControl4, MIN(1. / power, fac));
                    bs->optimal_step[k]     = fabs(dt * fac);
                    bs->cost_per_time_unit[k] = bs->cost_per_step[k] / bs->optimal_step[k];

                    // check convergence
                    switch (k - bs->target_iter) {

                        case -1 : // one before target
                            if ((bs->target_iter > 1) && ! bs->previous_rejected) {

                                // check if we can stop iterations now
                                if (error <= 1.0) {
                                    // convergence have been reached just before target_iter
                                    loop = 0;
                                } else {
                                    // estimate if there is a chance convergence will
                                    // be reached on next iteration, using the
                                    // asymptotic evolution of error
                                    const double ratio = ((double) bs->sequence[bs->target_iter] * bs->sequence[bs->target_iter + 1]) / (bs->sequence[0] * bs->sequence[0]);
                                    if (error > ratio * ratio) {
                                        // we don't expect to converge on next iteration
                                        // we reject the step immediately and reduce order
                                        reject = 1;
                                        loop   = 0;
                                        bs->target_iter = k;
                                        if ((bs->target_iter > 1) &&
                                                (bs->cost_per_time_unit[bs->target_iter - 1] <
                                                 orderControl1 * bs->cost_per_time_unit[bs->target_iter])) {
                                            bs->target_iter -= 1;
                                        }
                                        dt = bs->optimal_step[bs->target_iter];
                                    }
                                }
                            }
                            break;

                        case 0: // exactly on target
                            if (error <= 1.0) {
                                // convergence has been reached exactly at target_iter
                                loop = 0;
                            } else {
                                // estimate if there is a chance convergence will
                                // be reached on next iteration, using the
                                // asymptotic evolution of error
                                const double ratio = ((double) bs->sequence[k + 1]) / bs->sequence[0];
                                if (error > ratio * ratio) {
                                    // we don't expect to converge on next iteration
                                    // we reject the step immediately
                                    reject = 1;
                                    loop = 0;
                                    if ((bs->target_iter > 1) &&
                                            (bs->cost_per_time_unit[bs->target_iter - 1] <
                                             orderControl1 * bs->cost_per_time_unit[bs->target_iter])) {
                                        --bs->target_iter;
                                    }
                                    dt = bs->optimal_step[bs->target_iter];
                                }
                            }
                            break;

                        case 1 : // one past target
                            if (error > 1.0) {
                                reject = 1;
                                if ((bs->target_iter > 1) &&
                                        (bs->cost_per_time_unit[bs->target_iter - 1] <
                                         orderControl1 * bs->cost_per_time_unit[bs->target_iter])) {
                                    --bs->target_iter;
                                }
                                dt = bs->optimal_step[bs->target_iter];
                            }
                            loop = 0;
                            break;

                        default :
                            if (bs->first_or_last_step && (error <= 1.0)) {
                                loop = 0;
                            }
                            break;

                    }
                }
            }
        }
    }


    if (! reject) {
        // Swap arrays
        for (size_t s=0; s < Ns; s++){
            double* y_tmp = odes[s]->y;
            odes[s]->y = odes[s]->y1; 
            odes[s]->y1 = y_tmp; 
            // Check if ODEs need post timestep call
            if (odes[s]->post_timestep){
                odes[s]->post_timestep(odes[s], odes[s]->y);
            }
        }

        int optimalIter;
        if (k == 1) {
            optimalIter = 2;
            if (bs->previous_rejected) {
                optimalIter = 1;
            }
        } else if (k <= bs->target_iter) { // Converged before or on target
            optimalIter = k;
            if (bs->cost_per_time_unit[k - 1] < orderControl1 * bs->cost_per_time_unit[k]) {
                optimalIter = k - 1;
            } else if (bs->cost_per_time_unit[k] < orderControl2 * bs->cost_per_time_unit[k - 1]) {
                optimalIter = MIN(k + 1, sequence_length - 2);
            }
        } else {                            // converged after target
            optimalIter = k - 1;
            if ((k > 2) && (bs->cost_per_time_unit[k - 2] < orderControl1 * bs->cost_per_time_unit[k - 1])) {
                optimalIter = k - 2;
            }
            if (bs->cost_per_time_unit[k] < orderControl2 * bs->cost_per_time_unit[optimalIter]) {
                optimalIter = MIN(k, sequence_length - 2);
            }
        }

        if (bs->previous_rejected) {
            // after a rejected step neither order nor stepsize
            // should increase
            bs->target_iter = MIN(optimalIter, k);
            dt = MIN(fabs(dt), bs->optimal_step[bs->target_iter]);
        } else {
            // stepsize control
            if (optimalIter <= k) {
                dt = bs->optimal_step[optimalIter];
            } else {
                if ((k < bs->target_iter) &&
                        (bs->cost_per_time_unit[k] < orderControl2 * bs->cost_per_time_unit[k - 1])) {
                    dt = bs->optimal_step[k] * bs->cost_per_step[optimalIter + 1] / bs->cost_per_step[k];
                } else {
                    dt = bs->optimal_step[k] * bs->cost_per_step[optimalIter] / bs->cost_per_step[k];
                }
            }

            bs->target_iter = optimalIter;

        }
    }

    dt = fabs(dt);

    if (bs->min_dt !=0.0 && dt < bs->min_dt) {
        dt = bs->min_dt;
        reb_simulation_warning(r,"Minimal stepsize reached during ODE integration.");
    }

    if (bs->max_dt !=0.0 && dt > bs->max_dt) {
        dt = bs->max_dt;
        reb_simulation_warning(r,"Maximum stepsize reached during ODE integration.");
    }

    if (! forward) {
        dt = -dt;
    }
    bs->dt_proposed = dt;

    if (reject) {
        bs->previous_rejected = 1;
    } else {
        bs->previous_rejected = 0;
        bs->first_or_last_step = 0;
    }
    return !reject;
}

struct reb_ode* reb_ode_create(struct reb_simulation* r, unsigned int length){
    struct reb_ode* ode = malloc(sizeof(struct reb_ode));

    memset(ode, 0, sizeof(struct reb_ode)); // not really necessary

    if (r->N_allocated_odes <= r->N_odes){
        r->N_allocated_odes += 32;
        r->odes = realloc(r->odes,sizeof(struct reb_ode*)*r->N_allocated_odes);
    }

    r->odes[r->N_odes] = ode;
    r->N_odes += 1;


    ode->r = r; // weak reference
    ode->length = length;
    ode->needs_nbody = 1;
    ode->N_allocated = length;
    ode->getscale = NULL;
    ode->derivatives = NULL;
    ode->pre_timestep = NULL;
    ode->post_timestep = NULL;
    ode->D   = malloc(sizeof(double*)*(sequence_length));
    for (int k = 0; k < sequence_length; ++k) {
        ode->D[k]   = malloc(sizeof(double)*length);
    }

    ode->C     = malloc(sizeof(double)*length);
    ode->y     = malloc(sizeof(double)*length);
    ode->y1    = malloc(sizeof(double)*length);
    ode->y0Dot = malloc(sizeof(double)*length);
    ode->yTmp  = malloc(sizeof(double)*length);
    ode->yDot  = malloc(sizeof(double)*length);

    ode->scale = malloc(sizeof(double)*length);

    if (strcmp(r->integrator.name, "bs")==0){
        struct reb_integrator_bs_state* bs = r->integrator.state;
        bs->first_or_last_step = 1;
    }

    return ode;
}

void reb_integrator_bs_step(struct reb_simulation* r, void* state){
    if (r->calculate_megno){
        reb_simulation_error(r, "The BS integrator does currently not support MEGNO.");
    }

    struct reb_ode** odes = r->odes;
    size_t Ns = r->N_odes;
    for (size_t s=0; s < Ns; s++){
        const size_t length = odes[s]->length;
        double* y0 = odes[s]->y;
        double* y1 = odes[s]->y1;
        for (size_t i = 0; i < length; ++i) {
            y1[i] = y0[i];
        }
    }

    struct reb_integrator_bs_state* bs = state;

    size_t nbody_length = (r->N+r->N_var)*3*2;
    // Check if particle numbers changed, if so delete and recreate ode.
    if (bs->nbody_ode != NULL){ 
        if (bs->nbody_ode->length != nbody_length){
            reb_ode_free(bs->nbody_ode);
            bs->nbody_ode = NULL;
        }
    }
    if (bs->nbody_ode == NULL){ 
        bs->nbody_ode = reb_ode_create(r, nbody_length);
        bs->nbody_ode->derivatives = reb_integrator_bs_nbody_derivatives;
        bs->nbody_ode->needs_nbody = 0; // No need to update unless there's another ode
        bs->first_or_last_step = 1;
    }

    double* const y = bs->nbody_ode->y;
    for (size_t i=0; i<r->N; i++){
        const struct reb_particle p = r->particles[i];
        y[i*6+0] = p.x;
        y[i*6+1] = p.y;
        y[i*6+2] = p.z;
        y[i*6+3] = p.vx;
        y[i*6+4] = p.vy;
        y[i*6+5] = p.vz;
    }
    for (size_t i=0; i<r->N_var; i++){
        const struct reb_particle p = r->particles_var[i];
        size_t io = (i+r->N)*6;
        y[io+0] = p.x;
        y[io+1] = p.y;
        y[io+2] = p.z;
        y[io+3] = p.vx;
        y[io+4] = p.vy;
        y[io+5] = p.vz;
    }

    bs->user_ode_needs_nbody = 0;
    for (size_t s=0; s < r->N_odes; s++){
        if (r->odes[s]->needs_nbody){
            bs->user_ode_needs_nbody = 1;
        }
    }

    int success = reb_integrator_bs_step_odes(r, bs, r->dt);
    if (success){
        r->t += r->dt;
        r->dt_last_done = r->dt;
    }
    r->dt = bs->dt_proposed;

    reb_integrator_bs_update_particles(r, bs->nbody_ode->y);
}

void reb_ode_free(struct reb_ode* ode){
    if (!ode) return;
    // Free data array
    free(ode->y);
    free(ode->y1);
    free(ode->C);
    free(ode->scale);

    if (ode->D){
        for (int k=0; k < sequence_length; k++) {
            free(ode->D[k]);
        }
        free(ode->D);
    }
    free(ode->y0Dot);
    free(ode->yTmp);
    free(ode->yDot);

    struct reb_simulation* r = ode->r;
    if (r){ // only do this if ode is in a simulation
        int shift = 0;
        for (size_t s=0; s < r->N_odes; s++){
            if (r->odes[s] == ode){
                r->N_odes--;
                shift = 1;
            }
            if (shift && s <= r->N_odes ){
                r->odes[s] = r->odes[s+1];
            }
        }
    }
    free(ode);
}