<commit_before>﻿#pragma once

#include "integrators/embedded_explicit_runge_kutta_nyström_integrator.hpp"

#include <algorithm>
#include <cmath>
#include <ctime>
#include <vector>

#include "geometry/sign.hpp"
#include "glog/logging.h"
#include "quantities/quantities.hpp"

namespace principia {

using geometry::Sign;
using quantities::Difference;
using quantities::Quotient;

namespace integrators {

template<typename Position>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Position, 4, 3, 4, true> const&
DormandElMikkawyPrince1986RKN434FM() {
  static EmbeddedExplicitRungeKuttaNyströmIntegrator<
             Position, 4, 3, 4, true> const integrator(
      // c
      { 0.0         ,   1.0 /   4.0,   7.0 /  10.0,  1.0},
      // a
      {
        1.0 /   32.0,
        7.0 / 1000.0, 119.0 / 500.0,
        1.0 /   14.0,   8.0 /  27.0,  25.0 / 189.0},
      // b̂
      { 1.0 /   14.0,   8.0 /  27.0,  25.0 / 189.0,  0.0},
      // b̂′
      { 1.0 /   14.0,  32.0 /  81.0, 250.0 / 567.0,  5.0 / 54.0},
      // b
      {-7.0 /  150.0,  67.0 / 150.0,   3.0 /  20.0, -1.0 / 20.0},
      // b′
      {13.0 /   21.0, -20.0 /  27.0, 275.0 / 189.0, -1.0 /  3.0});
  return integrator;
}

template<typename Position, int higher_order, int lower_order, int stages,
         bool first_same_as_last>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Position, higher_order, lower_order,
                                            stages, first_same_as_last>::
EmbeddedExplicitRungeKuttaNyströmIntegrator(
    FixedVector<double, stages> const& c,
    FixedStrictlyLowerTriangularMatrix<double, stages> const& a,
    FixedVector<double, stages> const& b_hat,
    FixedVector<double, stages> const& b_prime_hat,
    FixedVector<double, stages> const& b,
    FixedVector<double, stages> const& b_prime)
    : c_(c),
      a_(a),
      b_hat_(b_hat),
      b_prime_hat_(b_prime_hat),
      b_(b),
      b_prime_(b_prime) {
  // the first node is always 0 in an explicit method.
  CHECK_EQ(0.0, c_[0]);
  if (first_same_as_last) {
    // Check that the conditions for the FSAL property are satisfied, see for
    // instance Dormand, El-Mikkawy and Prince (1986),
    // Families of Runge-Kutta-Nyström formulae, equation 3.1.
    CHECK_EQ(1.0, c_[stages - 1]);
    CHECK_EQ(0.0, b_hat_[stages - 1]);
    for (int j = 0; j < stages - 1; ++j) {
      CHECK_EQ(b_hat_[j], a_[stages - 1][j]);
    }
  }
}

template<typename Position, int higher_order, int lower_order, int stages,
         bool first_same_as_last>
void EmbeddedExplicitRungeKuttaNyströmIntegrator<Position,
                                                 higher_order,
                                                 lower_order,
                                                 stages,
                                                 first_same_as_last>::Solve(
    IntegrationProblem<ODE> const& problem,
    AdaptiveStepSize<ODE> const& adaptive_step_size) const {
  using Displacement = typename ODE::Displacement;
  using Velocity = typename ODE::Velocity;
  using Acceleration = typename ODE::Acceleration;

  // Argument checks.
  CHECK_NOTNULL(problem.initial_state);
  int const dimension = problem.initial_state->positions.size();
  CHECK_EQ(dimension, problem.initial_state->velocities.size());
  CHECK_NE(Time(), adaptive_step_size.first_time_step);
  Sign const integration_direction =
      Sign(adaptive_step_size.first_time_step);
  if (integration_direction.Positive()) {
    // Integrating forward.
    CHECK_LT(problem.initial_state->time.value, problem.t_final);
  } else {
    // Integrating backward.
    CHECK_GT(problem.initial_state->time.value, problem.t_final);
  }
  CHECK_GT(adaptive_step_size.safety_factor, 0);
  CHECK_LT(adaptive_step_size.safety_factor, 1);

  typename ODE::SystemState current_state = *problem.initial_state;

  // Time step.
  Time h = adaptive_step_size.first_time_step;
  // Current time.  This is a non-const reference whose purpose is to make the
  // equations more readable.
  DoublePrecision<Instant>& t = current_state.time;

  // Position increment (high-order).
  std::vector<Displacement> ∆q_hat(dimension);
  // Velocity increment (high-order).
  std::vector<Velocity> ∆v_hat(dimension);
  // Current position.  This is a non-const reference whose purpose is to make
  // the equations more readable.
  std::vector<DoublePrecision<Position>>& q_hat = current_state.positions;
  // Current velocity.  This is a non-const reference whose purpose is to make
  // the equations more readable.
  std::vector<DoublePrecision<Velocity>>& v_hat = current_state.velocities;

  // Difference between the low- and high-order approximations.
  typename ODE::SystemStateError error_estimate;
  error_estimate.position_error.resize(dimension);
  error_estimate.velocity_error.resize(dimension);

  // Current Runge-Kutta-Nyström stage.
  std::vector<Position> q_stage(dimension);
  // Accelerations at each stage.
  // TODO(egg): this is a rectangular container, use something more appropriate.
  std::vector<std::vector<Acceleration>> g(stages);
  for (auto& g_stage : g) {
    g_stage.resize(dimension);
  }

  bool at_end = false;
  double tolerance_to_error_ratio;

  // The first step of the Runge-Kutta-Nyström iteration.  In the FSAL case,
  // i0 = 1 after the first step, since the first RHS evaluation has already
  // occured in the previous step.  In the non-FSAL case and in the first step
  // of the FSAL case, i0 = 0.
  int i0 = 0;

  // No step size control on the first step.
  goto runge_kutta_nyström_step;

  while (!at_end) {
    // Compute the next step with decreasing step sizes until the error is
    // tolerable.
    do {
      // Adapt step size.
      // TODO(egg): find out whether there's a smarter way to compute that root,
      // especially if we make the order compile-time.
      h *= adaptive_step_size.safety_factor *
               std::pow(tolerance_to_error_ratio, 1.0 / (lower_order + 1));

    runge_kutta_nyström_step:
      // Termination condition.
      Time const time_to_end = (problem.t_final - t.value) - t.error;
      at_end = integration_direction * h >= integration_direction * time_to_end;
      if (at_end) {
        // The chosen step size will overshoot.  Clip it to just reach the end,
        // and terminate if the step is accepted.
        h = time_to_end;
      }

      // Runge-Kutta-Nyström iteration; fills |g|.
      for (int i = i0; i < stages; ++i) {
        Instant const t_stage = t.value + c_[i] * h;
        for (int k = 0; k < dimension; ++k) {
          Acceleration ∑j_a_ij_g_jk{};
          for (int j = 0; j < i; ++j) {
            ∑j_a_ij_g_jk += a_[i][j] * g[j][k];
          }
          q_stage[k] = q_hat[k].value +
                           h * (c_[i] * v_hat[k].value + h * ∑j_a_ij_g_jk);
        }
        problem.equation.compute_acceleration(t_stage, q_stage, &g[i]);
      }
      // TODO(egg): handle the FSAL case.

      // Increment computation and step size control.
      for (int k = 0; k < dimension; ++k) {
        Acceleration ∑i_b_hat_i_g_ik{};
        Acceleration ∑i_b_i_g_ik{};
        Acceleration ∑i_b_prime_hat_i_g_ik{};
        Acceleration ∑i_b_prime_i_g_ik{};
        // Please keep the eight assigments below aligned, they become illegible
        // otherwise.
        for (int i = 0; i < stages; ++i) {<commit_msg>that was the whole point.<commit_after>﻿#pragma once

#include "integrators/embedded_explicit_runge_kutta_nyström_integrator.hpp"

#include <algorithm>
#include <cmath>
#include <ctime>
#include <vector>

#include "geometry/sign.hpp"
#include "glog/logging.h"
#include "quantities/quantities.hpp"

namespace principia {

using geometry::Sign;
using quantities::Difference;
using quantities::Quotient;

namespace integrators {

template<typename Position>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Position, 4, 3, 4, true> const&
DormandElMikkawyPrince1986RKN434FM() {
  static EmbeddedExplicitRungeKuttaNyströmIntegrator<
             Position, 4, 3, 4, true> const integrator(
      // c
      { 0.0         ,   1.0 /   4.0,   7.0 /  10.0,  1.0},
      // a
      {
        1.0 /   32.0,
        7.0 / 1000.0, 119.0 / 500.0,
        1.0 /   14.0,   8.0 /  27.0,  25.0 / 189.0},
      // b̂
      { 1.0 /   14.0,   8.0 /  27.0,  25.0 / 189.0,  0.0},
      // b̂′
      { 1.0 /   14.0,  32.0 /  81.0, 250.0 / 567.0,  5.0 / 54.0},
      // b
      {-7.0 /  150.0,  67.0 / 150.0,   3.0 /  20.0, -1.0 / 20.0},
      // b′
      {13.0 /   21.0, -20.0 /  27.0, 275.0 / 189.0, -1.0 /  3.0});
  return integrator;
}

template<typename Position, int higher_order, int lower_order, int stages,
         bool first_same_as_last>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Position, higher_order, lower_order,
                                            stages, first_same_as_last>::
EmbeddedExplicitRungeKuttaNyströmIntegrator(
    FixedVector<double, stages> const& c,
    FixedStrictlyLowerTriangularMatrix<double, stages> const& a,
    FixedVector<double, stages> const& b_hat,
    FixedVector<double, stages> const& b_prime_hat,
    FixedVector<double, stages> const& b,
    FixedVector<double, stages> const& b_prime)
    : c_(c),
      a_(a),
      b_hat_(b_hat),
      b_prime_hat_(b_prime_hat),
      b_(b),
      b_prime_(b_prime) {
  // the first node is always 0 in an explicit method.
  CHECK_EQ(0.0, c_[0]);
  if (first_same_as_last) {
    // Check that the conditions for the FSAL property are satisfied, see for
    // instance Dormand, El-Mikkawy and Prince (1986),
    // Families of Runge-Kutta-Nyström formulae, equation 3.1.
    CHECK_EQ(1.0, c_[stages - 1]);
    CHECK_EQ(0.0, b_hat_[stages - 1]);
    for (int j = 0; j < stages - 1; ++j) {
      CHECK_EQ(b_hat_[j], a_[stages - 1][j]);
    }
  }
}

template<typename Position, int higher_order, int lower_order, int stages,
         bool first_same_as_last>
void EmbeddedExplicitRungeKuttaNyströmIntegrator<Position,
                                                 higher_order,
                                                 lower_order,
                                                 stages,
                                                 first_same_as_last>::Solve(
    IntegrationProblem<ODE> const& problem,
    AdaptiveStepSize<ODE> const& adaptive_step_size) const {
  using Displacement = typename ODE::Displacement;
  using Velocity = typename ODE::Velocity;
  using Acceleration = typename ODE::Acceleration;

  // Argument checks.
  CHECK_NOTNULL(problem.initial_state);
  int const dimension = problem.initial_state->positions.size();
  CHECK_EQ(dimension, problem.initial_state->velocities.size());
  CHECK_NE(Time(), adaptive_step_size.first_time_step);
  Sign const integration_direction =
      Sign(adaptive_step_size.first_time_step);
  if (integration_direction.Positive()) {
    // Integrating forward.
    CHECK_LT(problem.initial_state->time.value, problem.t_final);
  } else {
    // Integrating backward.
    CHECK_GT(problem.initial_state->time.value, problem.t_final);
  }
  CHECK_GT(adaptive_step_size.safety_factor, 0);
  CHECK_LT(adaptive_step_size.safety_factor, 1);

  typename ODE::SystemState current_state = *problem.initial_state;

  // Time step.
  Time h = adaptive_step_size.first_time_step;
  // Current time.  This is a non-const reference whose purpose is to make the
  // equations more readable.
  DoublePrecision<Instant>& t = current_state.time;

  // Position increment (high-order).
  std::vector<Displacement> ∆q_hat(dimension);
  // Velocity increment (high-order).
  std::vector<Velocity> ∆v_hat(dimension);
  // Current position.  This is a non-const reference whose purpose is to make
  // the equations more readable.
  std::vector<DoublePrecision<Position>>& q_hat = current_state.positions;
  // Current velocity.  This is a non-const reference whose purpose is to make
  // the equations more readable.
  std::vector<DoublePrecision<Velocity>>& v_hat = current_state.velocities;

  // Difference between the low- and high-order approximations.
  typename ODE::SystemStateError error_estimate;
  error_estimate.position_error.resize(dimension);
  error_estimate.velocity_error.resize(dimension);

  // Current Runge-Kutta-Nyström stage.
  std::vector<Position> q_stage(dimension);
  // Accelerations at each stage.
  // TODO(egg): this is a rectangular container, use something more appropriate.
  std::vector<std::vector<Acceleration>> g(stages);
  for (auto& g_stage : g) {
    g_stage.resize(dimension);
  }

  bool at_end = false;
  double tolerance_to_error_ratio;

  // The first step of the Runge-Kutta-Nyström iteration.  In the FSAL case,
  // i0 = 1 after the first step, since the first RHS evaluation has already
  // occured in the previous step.  In the non-FSAL case and in the first step
  // of the FSAL case, i0 = 0.
  int i0 = 0;

  // No step size control on the first step.
  goto runge_kutta_nyström_step;

  while (!at_end) {
    // Compute the next step with decreasing step sizes until the error is
    // tolerable.
    do {
      // Adapt step size.
      // TODO(egg): find out whether there's a smarter way to compute that root,
      // especially if we make the order compile-time.
      h *= adaptive_step_size.safety_factor *
               std::pow(tolerance_to_error_ratio, 1.0 / (lower_order + 1));

    runge_kutta_nyström_step:
      // Termination condition.
      Time const time_to_end = (problem.t_final - t.value) - t.error;
      at_end = integration_direction * h >= integration_direction * time_to_end;
      if (at_end) {
        // The chosen step size will overshoot.  Clip it to just reach the end,
        // and terminate if the step is accepted.
        h = time_to_end;
      }

      // Runge-Kutta-Nyström iteration; fills |g|.
      for (int i = i0; i < stages; ++i) {
        Instant const t_stage = t.value + c_[i] * h;
        for (int k = 0; k < dimension; ++k) {
          Acceleration ∑j_a_ij_g_jk{};
          for (int j = 0; j < i; ++j) {
            ∑j_a_ij_g_jk += a_[i][j] * g[j][k];
          }
          q_stage[k] = q_hat[k].value +
                           h * (c_[i] * v_hat[k].value + h * ∑j_a_ij_g_jk);
        }
        problem.equation.compute_acceleration(t_stage, q_stage, &g[i]);
      }

      // Increment computation and step size control.
      for (int k = 0; k < dimension; ++k) {
        Acceleration ∑i_b_hat_i_g_ik{};
        Acceleration ∑i_b_i_g_ik{};
        Acceleration ∑i_b_prime_hat_i_g_ik{};
        Acceleration ∑i_b_prime_i_g_ik{};
        // Please keep the eight assigments below aligned, they become illegible
        // otherwise.
        for (int i = 0; i < stages; ++i) {
{'fraction_non_alphanumeric': 0.11415493919934418, 'fraction_numerical': 0.02568656920344309, 'mean_word_length': 2.601230012300123, 'pattern_counts': {'":': 0, '<': 73, '<?xml version=': 0, '>': 71, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 20, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4825010', 'n_tokens_mistral': 5276, 'n_tokens_neox': 4843, 'n_words': 1233}