#include "taylor_order_2.hpp"

#include <cmath>

#include "math_utils.hpp"

namespace astrodyn_core {
namespace geqoe {

namespace {

constexpr std::size_t STATE_DIM = 6;

inline std::size_t idx_yprop(std::size_t step, std::size_t comp) {
    return step * STATE_DIM + comp;
}

inline void derivatives_of_inverse_full_2(double a0, double a1, double* out) {
    const double in_a[2] = {a0, a1};
    astrodyn_core::math::compute_derivatives_of_inverse(in_a, out, 2, false);
}

inline double derivatives_of_inverse_2(double a0, double a1) {
    double out = 0.0;
    const double in_a[2] = {a0, a1};
    astrodyn_core::math::compute_derivatives_of_inverse(in_a, &out, 2, true);
    return out;
}

inline double derivatives_of_inverse_3(double a0, double a1, double a2) {
    double out = 0.0;
    const double in_a[3] = {a0, a1, a2};
    astrodyn_core::math::compute_derivatives_of_inverse(in_a, &out, 3, true);
    return out;
}

inline double derivatives_of_inverse_wrt_param_1(double a0, double ad0) {
    double out = 0.0;
    const double in_a[1] = {a0};
    const double in_d[1] = {ad0};
    astrodyn_core::math::compute_derivatives_of_inverse_wrt_param(in_a, in_d, &out, 1, true);
    return out;
}

inline double derivatives_of_inverse_wrt_param_2(double a0, double a1, double ad0, double ad1) {
    double out = 0.0;
    const double in_a[2] = {a0, a1};
    const double in_d[2] = {ad0, ad1};
    astrodyn_core::math::compute_derivatives_of_inverse_wrt_param(in_a, in_d, &out, 2, true);
    return out;
}

inline double derivatives_of_inverse_wrt_param_3(double a0, double a1, double a2, double ad0, double ad1, double ad2) {
    double out = 0.0;
    const double in_a[3] = {a0, a1, a2};
    const double in_d[3] = {ad0, ad1, ad2};
    astrodyn_core::math::compute_derivatives_of_inverse_wrt_param(in_a, in_d, &out, 3, true);
    return out;
}

inline void derivatives_of_product_full_2(double a0, double a1, double* out) {
    const double in_a[2] = {a0, a1};
    astrodyn_core::math::compute_derivatives_of_product(in_a, out, 2, false);
}

inline double derivatives_of_product_3(double a0, double a1, double a2) {
    double out = 0.0;
    const double in_a[3] = {a0, a1, a2};
    astrodyn_core::math::compute_derivatives_of_product(in_a, &out, 3, true);
    return out;
}

inline double derivatives_of_product_wrt_param_2(double a0, double a1, double ad0, double ad1) {
    double out = 0.0;
    const double in_a[2] = {a0, a1};
    const double in_d[2] = {ad0, ad1};
    astrodyn_core::math::compute_derivatives_of_product_wrt_param(in_a, in_d, &out, 2, true);
    return out;
}

inline double derivatives_of_product_wrt_param_3(double a0, double a1, double a2, double ad0, double ad1, double ad2) {
    double out = 0.0;
    const double in_a[3] = {a0, a1, a2};
    const double in_d[3] = {ad0, ad1, ad2};
    astrodyn_core::math::compute_derivatives_of_product_wrt_param(in_a, in_d, &out, 3, true);
    return out;
}

} // namespace

void compute_coefficients_2(
    const double* y0,
    const PropagationConstants& constants,
    Order1Coefficients& out1,
    Order1Intermediates& inter1,
    Order2Coefficients& out,
    Order2Intermediates& inter
) {
    compute_coefficients_1(y0, constants, out1, inter1);

    const double fibm1 = inter1.fibm1;
    const double r3p = inter1.r3p;
    const double r3p_Lr = inter1.r3p_Lr;
    const double r3p_nu = inter1.r3p_nu;
    const double r3p_p1 = inter1.r3p_p1;
    const double r3p_p2 = inter1.r3p_p2;
    const double r3p_q1 = inter1.r3p_q1;
    const double r3p_q2 = inter1.r3p_q2;
    const double nu_0 = inter1.nu_0;
    const double q1_0 = inter1.q1_0;
    const double q2_0 = inter1.q2_0;
    const double p1_0 = inter1.p1_0;
    const double p2_0 = inter1.p2_0;
    const double mu_norm = inter1.mu_norm;
    const double A = inter1.A;
    const double a = inter1.a;
    const double a_nu = inter1.a_nu;
    const double r = inter1.r;
    const double rp = inter1.rp;
    const double r2 = inter1.r2;
    const double r3 = inter1.r3;
    const double X = inter1.X;
    const double Y = inter1.Y;
    const double Xp = inter1.Xp;
    const double Yp = inter1.Yp;
    const double cosL = inter1.cosL;
    const double sinL = inter1.sinL;
    const double alpha = inter1.alpha;
    const double beta = inter1.beta;
    const double fib = inter1.fib;
    double fic = inter1.fic;
    const double fih = inter1.fih;
    const double fir = inter1.fir;
    const double firp = inter1.firp;
    const double fir2 = inter1.fir2;
    const double fir2p = inter1.fir2p;
    const double fir3 = inter1.fir3;
    const double fir3p = inter1.fir3p;
    double fihr3 = inter1.fihr3;
    const double fiD = inter1.fiD;
    const double c = inter1.c;
    const double h = inter1.h;
    const double hr = inter1.hr;
    const double hr3 = inter1.hr3;
    const double d = inter1.d;
    const double wh = inter1.wh;
    const double I = inter1.I;
    const double U = inter1.U;
    const double delta = inter1.delta;
    const double zg = inter1.zg;
    const double GAMMA_ = inter1.GAMMA_;
    const double xi1 = inter1.xi1;
    const double xi2 = inter1.xi2;
    const double fUz = inter1.fUz;
    const double qs = inter1.qs;
    const double C = inter1.C;
    const double D = inter1.D;
    const double rpn = inter1.rpn;
    const double fialpha = inter1.fialpha;
    const double f2rp = inter1.f2rp;
    const double p1p_0 = inter1.p1p_0;
    const double p2p_0 = inter1.p2p_0;
    const double Lrp_0 = inter1.Lrp_0;
    const double q1p_0 = inter1.q1p_0;
    const double q2p_0 = inter1.q2p_0;
    const double r_nu = inter1.r_nu;
    const double r_Lr = inter1.r_Lr;
    const double r_q1 = inter1.r_q1;
    const double r_q2 = inter1.r_q2;
    const double r_p1 = inter1.r_p1;
    const double r_p2 = inter1.r_p2;
    const double rp_nu = inter1.rp_nu;
    const double rp_Lr = inter1.rp_Lr;
    const double rp_q1 = inter1.rp_q1;
    const double rp_q2 = inter1.rp_q2;
    const double rp_p1 = inter1.rp_p1;
    const double rp_p2 = inter1.rp_p2;
    const double r2_nu = inter1.r2_nu;
    const double r2_Lr = inter1.r2_Lr;
    const double r2_q1 = inter1.r2_q1;
    const double r2_q2 = inter1.r2_q2;
    const double r2_p1 = inter1.r2_p1;
    const double r2_p2 = inter1.r2_p2;
    const double r3_nu = inter1.r3_nu;
    const double r3_Lr = inter1.r3_Lr;
    const double r3_q1 = inter1.r3_q1;
    const double r3_q2 = inter1.r3_q2;
    const double r3_p1 = inter1.r3_p1;
    const double r3_p2 = inter1.r3_p2;
    const double beta_nu = inter1.beta_nu;
    const double beta_Lr = inter1.beta_Lr;
    const double beta_q1 = inter1.beta_q1;
    const double beta_q2 = inter1.beta_q2;
    const double beta_p1 = inter1.beta_p1;
    const double beta_p2 = inter1.beta_p2;
    const double alpha_nu = inter1.alpha_nu;
    const double alpha_Lr = inter1.alpha_Lr;
    const double alpha_q1 = inter1.alpha_q1;
    const double alpha_q2 = inter1.alpha_q2;
    const double alpha_p1 = inter1.alpha_p1;
    const double alpha_p2 = inter1.alpha_p2;
    const double fialpha_nu = inter1.fialpha_nu;
    const double fialpha_Lr = inter1.fialpha_Lr;
    const double fialpha_q1 = inter1.fialpha_q1;
    const double fialpha_q2 = inter1.fialpha_q2;
    const double fialpha_p1 = inter1.fialpha_p1;
    const double fialpha_p2 = inter1.fialpha_p2;
    const double c_nu = inter1.c_nu;
    const double c_Lr = inter1.c_Lr;
    const double c_q1 = inter1.c_q1;
    const double c_q2 = inter1.c_q2;
    const double c_p1 = inter1.c_p1;
    const double c_p2 = inter1.c_p2;
    const double fic_nu = inter1.fic_nu;
    const double fic_Lr = inter1.fic_Lr;
    const double fic_q1 = inter1.fic_q1;
    const double fic_q2 = inter1.fic_q2;
    const double fic_p1 = inter1.fic_p1;
    const double fic_p2 = inter1.fic_p2;
    const double h_nu = inter1.h_nu;
    const double h_Lr = inter1.h_Lr;
    const double h_q1 = inter1.h_q1;
    const double h_q2 = inter1.h_q2;
    const double h_p1 = inter1.h_p1;
    const double h_p2 = inter1.h_p2;
    const double fih_nu = inter1.fih_nu;
    const double fih_Lr = inter1.fih_Lr;
    const double fih_q1 = inter1.fih_q1;
    const double fih_q2 = inter1.fih_q2;
    const double fih_p1 = inter1.fih_p1;
    const double fih_p2 = inter1.fih_p2;
    const double X_nu = inter1.X_nu;
    const double X_Lr = inter1.X_Lr;
    const double X_q1 = inter1.X_q1;
    const double X_q2 = inter1.X_q2;
    const double X_p1 = inter1.X_p1;
    const double X_p2 = inter1.X_p2;
    const double Y_nu = inter1.Y_nu;
    const double Y_Lr = inter1.Y_Lr;
    const double Y_q1 = inter1.Y_q1;
    const double Y_q2 = inter1.Y_q2;
    const double Y_p1 = inter1.Y_p1;
    const double Y_p2 = inter1.Y_p2;
    const double cosL_nu = inter1.cosL_nu;
    const double cosL_Lr = inter1.cosL_Lr;
    const double cosL_q1 = inter1.cosL_q1;
    const double cosL_q2 = inter1.cosL_q2;
    const double cosL_p1 = inter1.cosL_p1;
    const double cosL_p2 = inter1.cosL_p2;
    const double sinL_nu = inter1.sinL_nu;
    const double sinL_Lr = inter1.sinL_Lr;
    const double sinL_q1 = inter1.sinL_q1;
    const double sinL_q2 = inter1.sinL_q2;
    const double sinL_p1 = inter1.sinL_p1;
    const double sinL_p2 = inter1.sinL_p2;
    const double zg_nu = inter1.zg_nu;
    const double zg_Lr = inter1.zg_Lr;
    const double zg_q1 = inter1.zg_q1;
    const double zg_q2 = inter1.zg_q2;
    const double zg_p1 = inter1.zg_p1;
    const double zg_p2 = inter1.zg_p2;
    const double fUz_nu = inter1.fUz_nu;
    const double fUz_Lr = inter1.fUz_Lr;
    const double fUz_q1 = inter1.fUz_q1;
    const double fUz_q2 = inter1.fUz_q2;
    const double fUz_p1 = inter1.fUz_p1;
    const double fUz_p2 = inter1.fUz_p2;
    const double U_nu = inter1.U_nu;
    const double U_Lr = inter1.U_Lr;
    const double U_q1 = inter1.U_q1;
    const double U_q2 = inter1.U_q2;
    const double U_p1 = inter1.U_p1;
    const double U_p2 = inter1.U_p2;
    const double hr3_nu = inter1.hr3_nu;
    const double hr3_Lr = inter1.hr3_Lr;
    const double hr3_q1 = inter1.hr3_q1;
    const double hr3_q2 = inter1.hr3_q2;
    const double hr3_p1 = inter1.hr3_p1;
    const double hr3_p2 = inter1.hr3_p2;
    const double fihr3_nu = inter1.fihr3_nu;
    const double fihr3_Lr = inter1.fihr3_Lr;
    const double fihr3_q1 = inter1.fihr3_q1;
    const double fihr3_q2 = inter1.fihr3_q2;
    const double fihr3_p1 = inter1.fihr3_p1;
    const double fihr3_p2 = inter1.fihr3_p2;
    const double delta_nu = inter1.delta_nu;
    const double delta_Lr = inter1.delta_Lr;
    const double delta_q1 = inter1.delta_q1;
    const double delta_q2 = inter1.delta_q2;
    const double delta_p1 = inter1.delta_p1;
    const double delta_p2 = inter1.delta_p2;
    const double I_nu = inter1.I_nu;
    const double I_Lr = inter1.I_Lr;
    const double I_q1 = inter1.I_q1;
    const double I_q2 = inter1.I_q2;
    const double I_p1 = inter1.I_p1;
    const double I_p2 = inter1.I_p2;
    const double d_nu = inter1.d_nu;
    const double d_Lr = inter1.d_Lr;
    const double d_q1 = inter1.d_q1;
    const double d_q2 = inter1.d_q2;
    const double d_p1 = inter1.d_p1;
    const double d_p2 = inter1.d_p2;
    const double wh_nu = inter1.wh_nu;
    const double wh_Lr = inter1.wh_Lr;
    const double wh_q1 = inter1.wh_q1;
    const double wh_q2 = inter1.wh_q2;
    const double wh_p1 = inter1.wh_p1;
    const double wh_p2 = inter1.wh_p2;
    const double GAMMA_nu = inter1.GAMMA_nu;
    const double GAMMA_Lr = inter1.GAMMA_Lr;
    const double GAMMA_q1 = inter1.GAMMA_q1;
    const double GAMMA_q2 = inter1.GAMMA_q2;
    const double GAMMA_p1 = inter1.GAMMA_p1;
    const double GAMMA_p2 = inter1.GAMMA_p2;
    const double xi1_nu = inter1.xi1_nu;
    const double xi1_Lr = inter1.xi1_Lr;
    const double xi1_q1 = inter1.xi1_q1;
    const double xi1_q2 = inter1.xi1_q2;
    const double xi1_p1 = inter1.xi1_p1;
    const double xi1_p2 = inter1.xi1_p2;
    const double xi2_nu = inter1.xi2_nu;
    const double xi2_Lr = inter1.xi2_Lr;
    const double xi2_q1 = inter1.xi2_q1;
    const double xi2_q2 = inter1.xi2_q2;
    const double xi2_p1 = inter1.xi2_p1;
    const double xi2_p2 = inter1.xi2_p2;
    const double q1p_nu = inter1.q1p_nu;
    const double q1p_Lr = inter1.q1p_Lr;
    const double q1p_q1 = inter1.q1p_q1;
    const double q1p_q2 = inter1.q1p_q2;
    const double q1p_p1 = inter1.q1p_p1;
    const double q1p_p2 = inter1.q1p_p2;
    const double q2p_nu = inter1.q2p_nu;
    const double q2p_Lr = inter1.q2p_Lr;
    const double q2p_q1 = inter1.q2p_q1;
    const double q2p_q2 = inter1.q2p_q2;
    const double q2p_p1 = inter1.q2p_p1;
    const double q2p_p2 = inter1.q2p_p2;
    const double p1p_nu = inter1.p1p_nu;
    const double p1p_Lr = inter1.p1p_Lr;
    const double p1p_q1 = inter1.p1p_q1;
    const double p1p_q2 = inter1.p1p_q2;
    const double p1p_p1 = inter1.p1p_p1;
    const double p1p_p2 = inter1.p1p_p2;
    const double p2p_nu = inter1.p2p_nu;
    const double p2p_Lr = inter1.p2p_Lr;
    const double p2p_q1 = inter1.p2p_q1;
    const double p2p_q2 = inter1.p2p_q2;
    const double p2p_p1 = inter1.p2p_p1;
    const double p2p_p2 = inter1.p2p_p2;
    const double Lrp_nu = inter1.Lrp_nu;
    const double Lrp_Lr = inter1.Lrp_Lr;
    const double Lrp_q1 = inter1.Lrp_q1;
    const double Lrp_q2 = inter1.Lrp_q2;
    const double Lrp_p1 = inter1.Lrp_p1;
    const double Lrp_p2 = inter1.Lrp_p2;
    const double rpn_nu = inter1.rpn_nu;
    const double rpn_Lr = inter1.rpn_Lr;
    const double rpn_q1 = inter1.rpn_q1;
    const double rpn_q2 = inter1.rpn_q2;
    const double rpn_p1 = inter1.rpn_p1;
    const double rpn_p2 = inter1.rpn_p2;
    const double D_nu = inter1.D_nu;
    const double D_Lr = inter1.D_Lr;
    const double D_q1 = inter1.D_q1;
    const double D_q2 = inter1.D_q2;
    const double D_p1 = inter1.D_p1;
    const double D_p2 = inter1.D_p2;
    const double fiD_nu = inter1.fiD_nu;
    const double fiD_Lr = inter1.fiD_Lr;
    const double fiD_q1 = inter1.fiD_q1;
    const double fiD_q2 = inter1.fiD_q2;
    const double fiD_p1 = inter1.fiD_p1;
    const double fiD_p2 = inter1.fiD_p2;
    const double C_nu = inter1.C_nu;
    const double C_Lr = inter1.C_Lr;
    const double C_q1 = inter1.C_q1;
    const double C_q2 = inter1.C_q2;
    const double C_p1 = inter1.C_p1;
    const double C_p2 = inter1.C_p2;
    const double fir3_p_nu = inter1.fir3_p_nu;
    const double fir3_p_Lr = inter1.fir3_p_Lr;
    const double fir3_p_q1 = inter1.fir3_p_q1;
    const double fir3_p_q2 = inter1.fir3_p_q2;
    const double fir3_p_p1 = inter1.fir3_p_p1;
    const double fir3_p_p2 = inter1.fir3_p_p2;
    const double fir_nu = inter1.fir_nu;
    const double fir_Lr = inter1.fir_Lr;
    const double fir_q1 = inter1.fir_q1;
    const double fir_q2 = inter1.fir_q2;
    const double fir_p1 = inter1.fir_p1;
    const double fir_p2 = inter1.fir_p2;
    const double firp_nu = inter1.firp_nu;
    const double firp_Lr = inter1.firp_Lr;
    const double firp_q1 = inter1.firp_q1;
    const double firp_q2 = inter1.firp_q2;
    const double firp_p1 = inter1.firp_p1;
    const double firp_p2 = inter1.firp_p2;
    const double fir2_nu = inter1.fir2_nu;
    const double fir2_Lr = inter1.fir2_Lr;
    const double fir2_q1 = inter1.fir2_q1;
    const double fir2_q2 = inter1.fir2_q2;
    const double fir2_p1 = inter1.fir2_p1;
    const double fir2_p2 = inter1.fir2_p2;
    const double fir2p_nu = inter1.fir2p_nu;
    const double fir2p_Lr = inter1.fir2p_Lr;
    const double fir2p_q1 = inter1.fir2p_q1;
    const double fir2p_q2 = inter1.fir2p_q2;
    const double fir2p_p1 = inter1.fir2p_p1;
    const double fir2p_p2 = inter1.fir2p_p2;
    const double fir3_nu = inter1.fir3_nu;
    const double fir3_Lr = inter1.fir3_Lr;
    const double fir3_q1 = inter1.fir3_q1;
    const double fir3_q2 = inter1.fir3_q2;
    const double fir3_p1 = inter1.fir3_p1;
    const double fir3_p2 = inter1.fir3_p2;
    const double f2rp_nu = inter1.f2rp_nu;
    const double f2rp_Lr = inter1.f2rp_Lr;
    const double f2rp_q1 = inter1.f2rp_q1;
    const double f2rp_q2 = inter1.f2rp_q2;
    const double f2rp_p1 = inter1.f2rp_p1;
    const double f2rp_p2 = inter1.f2rp_p2;
    const double qs_nu = 0.0;
    const double qs_Lr = 0.0;
    const double qs_p1 = 0.0;
    const double qs_p2 = 0.0;
    const double qs_q1 = 2.0 * q1_0;
    const double qs_q2 = 2.0 * q2_0;
    const double qsp = 2.0 * (q1_0 * q1p_0 + q2_0 * q2p_0);
    const double qsp_nu = 2.0 * (q1_0 * q1p_nu + q2_0 * q2p_nu);
    const double qsp_Lr = 2.0 * (q1_0 * q1p_Lr + q2_0 * q2p_Lr);
    const double qsp_p1 = 2.0 * (q1_0 * q1p_p1 + q2_0 * q2p_p1);
    const double qsp_p2 = 2.0 * (q1_0 * q1p_p2 + q2_0 * q2p_p2);
    const double qsp_q1 = 2.0 * (q1p_0 + q1_0 * q1p_q1 + q2_0 * q2p_q1);
    const double qsp_q2 = 2.0 * (q1_0 * q1p_q2 + q2p_0 + q2_0 * q2p_q2);
    const double psp = 2.0 * (p1_0 * p1p_0 + p2_0 * p2p_0);
    const double psp_nu = 2.0 * (p1_0 * p1p_nu + p2_0 * p2p_nu);
    const double psp_Lr = 2.0 * (p1_0 * p1p_Lr + p2_0 * p2p_Lr);
    const double psp_p1 = 2.0 * (p1p_0 + p1_0 * p1p_p1 + p2_0 * p2p_p1);
    const double psp_p2 = 2.0 * (p1_0 * p1p_p2 + p2p_0 + p2_0 * p2p_p2);
    const double psp_q1 = 2.0 * (p1_0 * p1p_q1 + p2_0 * p2p_q1);
    const double psp_q2 = 2.0 * (p1_0 * p1p_q2 + p2_0 * p2p_q2);
    const double hr_nu = h_nu * fir + h * fir_nu;
    const double hr_Lr = h_Lr * fir + h * fir_Lr;
    const double hr_q1 = h_q1 * fir + h * fir_q1;
    const double hr_q2 = h_q2 * fir + h * fir_q2;
    const double hr_p1 = h_p1 * fir + h * fir_p1;
    const double hr_p2 = h_p2 * fir + h * fir_p2;
    const double Xp_nu = rp_nu * cosL + rp * cosL_nu - hr_nu * sinL - hr * sinL_nu;
    const double Xp_Lr = rp_Lr * cosL + rp * cosL_Lr - hr_Lr * sinL - hr * sinL_Lr;
    const double Xp_q1 = rp_q1 * cosL + rp * cosL_q1 - hr_q1 * sinL - hr * sinL_q1;
    const double Xp_q2 = rp_q2 * cosL + rp * cosL_q2 - hr_q2 * sinL - hr * sinL_q2;
    const double Xp_p1 = rp_p1 * cosL + rp * cosL_p1 - hr_p1 * sinL - hr * sinL_p1;
    const double Xp_p2 = rp_p2 * cosL + rp * cosL_p2 - hr_p2 * sinL - hr * sinL_p2;
    const double Yp_nu = rp_nu * sinL + rp * sinL_nu + hr_nu * cosL + hr * cosL_nu;
    const double Yp_Lr = rp_Lr * sinL + rp * sinL_Lr + hr_Lr * cosL + hr * cosL_Lr;
    const double Yp_q1 = rp_q1 * sinL + rp * sinL_q1 + hr_q1 * cosL + hr * cosL_q1;
    const double Yp_q2 = rp_q2 * sinL + rp * sinL_q2 + hr_q2 * cosL + hr * cosL_q2;
    const double Yp_p1 = rp_p1 * sinL + rp * sinL_p1 + hr_p1 * cosL + hr * cosL_p1;
    const double Yp_p2 = rp_p2 * sinL + rp * sinL_p2 + hr_p2 * cosL + hr * cosL_p2;
    const double Cp = 2.0 * (Yp * q2_0 + Y * q2p_0 - Xp * q1_0 - X * q1p_0);
    const double Dp = rp * qs + r * qsp;
    const double fiDp = derivatives_of_inverse_2(D, Dp);
    const double zgp = Cp * fiD + C * fiDp;
    const double Cp_nu = 2.0 * (Yp_nu * q2_0 + Y_nu * q2p_0 + Y * q2p_nu - Xp_nu * q1_0 - X_nu * q1p_0 - X * q1p_nu);
    const double Cp_Lr = 2.0 * (Yp_Lr * q2_0 + Y_Lr * q2p_0 + Y * q2p_Lr - Xp_Lr * q1_0 - X_Lr * q1p_0 - X * q1p_Lr);
    const double Cp_p1 = 2.0 * (Yp_p1 * q2_0 + Y_p1 * q2p_0 + Y * q2p_p1 - Xp_p1 * q1_0 - X_p1 * q1p_0 - X * q1p_p1);
    const double Cp_p2 = 2.0 * (Yp_p2 * q2_0 + Y_p2 * q2p_0 + Y * q2p_p2 - Xp_p2 * q1_0 - X_p2 * q1p_0 - X * q1p_p2);
    const double Cp_q1 = 2.0 * (Yp_q1 * q2_0 + Y_q1 * q2p_0 + Y * q2p_q1 - Xp_q1 * q1_0 - Xp - X_q1 * q1p_0 - X * q1p_q1);
    const double Cp_q2 = 2.0 * (Yp_q2 * q2_0 + Yp + Y_q2 * q2p_0 + Y * q2p_q2 - Xp_q2 * q1_0 - X_q2 * q1p_0 - X * q1p_q2);
    const double Dp_nu = rp_nu * qs + rp * qs_nu + r_nu * qsp + r * qsp_nu;
    const double Dp_Lr = rp_Lr * qs + rp * qs_Lr + r_Lr * qsp + r * qsp_Lr;
    const double Dp_p1 = rp_p1 * qs + rp * qs_p1 + r_p1 * qsp + r * qsp_p1;
    const double Dp_p2 = rp_p2 * qs + rp * qs_p2 + r_p2 * qsp + r * qsp_p2;
    const double Dp_q1 = rp_q1 * qs + rp * qs_q1 + r_q1 * qsp + r * qsp_q1;
    const double Dp_q2 = rp_q2 * qs + rp * qs_q2 + r_q2 * qsp + r * qsp_q2;
    const double fiDp_nu = derivatives_of_inverse_wrt_param_2(D, Dp, D_nu, Dp_nu);
    const double fiDp_Lr = derivatives_of_inverse_wrt_param_2(D, Dp, D_Lr, Dp_Lr);
    const double fiDp_q1 = derivatives_of_inverse_wrt_param_2(D, Dp, D_q1, Dp_q1);
    const double fiDp_q2 = derivatives_of_inverse_wrt_param_2(D, Dp, D_q2, Dp_q2);
    const double fiDp_p1 = derivatives_of_inverse_wrt_param_2(D, Dp, D_p1, Dp_p1);
    const double fiDp_p2 = derivatives_of_inverse_wrt_param_2(D, Dp, D_p2, Dp_p2);
    const double zgp_nu = Cp_nu * fiD + Cp * fiD_nu + C_nu * fiDp + C * fiDp_nu;
    const double zgp_Lr = Cp_Lr * fiD + Cp * fiD_Lr + C_Lr * fiDp + C * fiDp_Lr;
    const double zgp_q1 = Cp_q1 * fiD + Cp * fiD_q1 + C_q1 * fiDp + C * fiDp_q1;
    const double zgp_q2 = Cp_q2 * fiD + Cp * fiD_q2 + C_q2 * fiDp + C * fiDp_q2;
    const double zgp_p1 = Cp_p1 * fiD + Cp * fiD_p1 + C_p1 * fiDp + C * fiDp_p1;
    const double zgp_p2 = Cp_p2 * fiD + Cp * fiD_p2 + C_p2 * fiDp + C * fiDp_p2;
    const double fUzp = -6.0 * zg * zgp;
    const double fUzp_nu = -6.0 * (zg_nu * zgp + zg * zgp_nu);
    const double fUzp_Lr = -6.0 * (zg_Lr * zgp + zg * zgp_Lr);
    const double fUzp_q1 = -6.0 * (zg_q1 * zgp + zg * zgp_q1);
    const double fUzp_q2 = -6.0 * (zg_q2 * zgp + zg * zgp_q2);
    const double fUzp_p1 = -6.0 * (zg_p1 * zgp + zg * zgp_p1);
    const double fUzp_p2 = -6.0 * (zg_p2 * zgp + zg * zgp_p2);
    const double Up = -A * (fUzp * fir3 + fUz * fir3p);
    const double Up_nu = -A * (fUzp_nu * fir3 + fUzp * fir3_nu + fUz_nu * fir3p + fUz * fir3_p_nu);
    const double Up_Lr = -A * (fUzp_Lr * fir3 + fUzp * fir3_Lr + fUz_Lr * fir3p + fUz * fir3_p_Lr);
    const double Up_q1 = -A * (fUzp_q1 * fir3 + fUzp * fir3_q1 + fUz_q1 * fir3p + fUz * fir3_p_q1);
    const double Up_q2 = -A * (fUzp_q2 * fir3 + fUzp * fir3_q2 + fUz_q2 * fir3p + fUz * fir3_p_q2);
    const double Up_p1 = -A * (fUzp_p1 * fir3 + fUzp * fir3_p1 + fUz_p1 * fir3p + fUz * fir3_p_p1);
    const double Up_p2 = -A * (fUzp_p2 * fir3 + fUzp * fir3_p2 + fUz_p2 * fir3p + fUz * fir3_p_p2);
    const double bp = -psp * fib / 2.0;
    const double fib_nu = 0.0;
    const double fib_Lr = 0.0;
    const double fib_q1 = 0.0;
    const double fib_q2 = 0.0;
    const double fib_p1 = derivatives_of_inverse_wrt_param_1(beta, beta_p1);
    const double fib_p2 = derivatives_of_inverse_wrt_param_1(beta, beta_p2);
    const double bp_nu = -psp_nu * fib / 2.0;
    const double bp_Lr = -psp_Lr * fib / 2.0;
    const double bp_q1 = -psp_q1 * fib / 2.0;
    const double bp_q2 = -psp_q2 * fib / 2.0;
    const double bp_p1 = -(psp_p1 * fib + psp * fib_p1) / 2.0;
    const double bp_p2 = -(psp_p2 * fib + psp * fib_p2) / 2.0;
    const double fibp = derivatives_of_inverse_2(beta, bp);
    const double fibp_nu = derivatives_of_inverse_wrt_param_2(beta, bp, beta_nu, bp_nu);
    const double fibp_Lr = derivatives_of_inverse_wrt_param_2(beta, bp, beta_Lr, bp_Lr);
    const double fibp_q1 = derivatives_of_inverse_wrt_param_2(beta, bp, beta_q1, bp_q1);
    const double fibp_q2 = derivatives_of_inverse_wrt_param_2(beta, bp, beta_q2, bp_q2);
    const double fibp_p1 = derivatives_of_inverse_wrt_param_2(beta, bp, beta_p1, bp_p1);
    const double fibp_p2 = derivatives_of_inverse_wrt_param_2(beta, bp, beta_p2, bp_p2);
    const double cosLp = Xp * fir + X * firp;
    const double sinLp = Yp * fir + Y * firp;
    const double cosLp_nu = Xp_nu * fir + Xp * fir_nu + X_nu * firp + X * firp_nu;
    const double cosLp_Lr = Xp_Lr * fir + Xp * fir_Lr + X_Lr * firp + X * firp_Lr;
    const double cosLp_q1 = Xp_q1 * fir + Xp * fir_q1 + X_q1 * firp + X * firp_q1;
    const double cosLp_q2 = Xp_q2 * fir + Xp * fir_q2 + X_q2 * firp + X * firp_q2;
    const double cosLp_p1 = Xp_p1 * fir + Xp * fir_p1 + X_p1 * firp + X * firp_p1;
    const double cosLp_p2 = Xp_p2 * fir + Xp * fir_p2 + X_p2 * firp + X * firp_p2;
    const double sinLp_nu = Yp_nu * fir + Yp * fir_nu + Y_nu * firp + Y * firp_nu;
    const double sinLp_Lr = Yp_Lr * fir + Yp * fir_Lr + Y_Lr * firp + Y * firp_Lr;
    const double sinLp_q1 = Yp_q1 * fir + Yp * fir_q1 + Y_q1 * firp + Y * firp_q1;
    const double sinLp_q2 = Yp_q2 * fir + Yp * fir_q2 + Y_q2 * firp + Y * firp_q2;
    const double sinLp_p1 = Yp_p1 * fir + Yp * fir_p1 + Y_p1 * firp + Y * firp_p1;
    const double sinLp_p2 = Yp_p2 * fir + Yp * fir_p2 + Y_p2 * firp + Y * firp_p2;
    const double cp = std::cbrt((mu_norm * mu_norm) / nu_0) * bp;
    double _buf_fic_vector[2];
    derivatives_of_inverse_full_2(c, cp, _buf_fic_vector);
    fic = _buf_fic_vector[0];
    const double ficp = _buf_fic_vector[1];
    double _buf_f2cp_vector[1];
    derivatives_of_product_full_2(c, cp, _buf_f2cp_vector);
    const double f2cp = _buf_f2cp_vector[0];
    const double cp_nu = std::cbrt(mu_norm * mu_norm) * (-1.0 / 3.0 / std::pow(nu_0, 4.0/3.0) * bp + std::cbrt(1.0 / nu_0) * bp_nu);
    const double cp_Lr = std::cbrt(mu_norm * mu_norm) / std::cbrt(nu_0) * bp_Lr;
    const double cp_q1 = std::cbrt(mu_norm * mu_norm) / std::cbrt(nu_0) * bp_q1;
    const double cp_q2 = std::cbrt(mu_norm * mu_norm) / std::cbrt(nu_0) * bp_q2;
    const double cp_p1 = std::cbrt(mu_norm * mu_norm) / std::cbrt(nu_0) * bp_p1;
    const double cp_p2 = std::cbrt(mu_norm * mu_norm) / std::cbrt(nu_0) * bp_p2;
    const double ficp_nu = derivatives_of_inverse_wrt_param_2(c, cp, c_nu, cp_nu);
    const double ficp_Lr = derivatives_of_inverse_wrt_param_2(c, cp, c_Lr, cp_Lr);
    const double ficp_q1 = derivatives_of_inverse_wrt_param_2(c, cp, c_q1, cp_q1);
    const double ficp_q2 = derivatives_of_inverse_wrt_param_2(c, cp, c_q2, cp_q2);
    const double ficp_p1 = derivatives_of_inverse_wrt_param_2(c, cp, c_p1, cp_p1);
    const double ficp_p2 = derivatives_of_inverse_wrt_param_2(c, cp, c_p2, cp_p2);
    const double f2cp_nu = derivatives_of_product_wrt_param_2(c, cp, c_nu, cp_nu);
    const double f2cp_Lr = derivatives_of_product_wrt_param_2(c, cp, c_Lr, cp_Lr);
    const double f2cp_q1 = derivatives_of_product_wrt_param_2(c, cp, c_q1, cp_q1);
    const double f2cp_q2 = derivatives_of_product_wrt_param_2(c, cp, c_q2, cp_q2);
    const double f2cp_p1 = derivatives_of_product_wrt_param_2(c, cp, c_p1, cp_p1);
    const double f2cp_p2 = derivatives_of_product_wrt_param_2(c, cp, c_p2, cp_p2);
    const double rpn_p = p2p_0 * sinL + p2_0 * sinLp - (p1p_0 * cosL + p1_0 * cosLp);
    const double rpn_p_nu = p2p_nu * sinL + p2p_0 * sinL_nu + p2_0 * sinLp_nu - (p1p_nu * cosL + p1p_0 * cosL_nu + p1_0 * cosLp_nu);
    const double rpn_p_Lr = p2p_Lr * sinL + p2p_0 * sinL_Lr + p2_0 * sinLp_Lr - (p1p_Lr * cosL + p1p_0 * cosL_Lr + p1_0 * cosLp_Lr);
    const double rpn_p_q1 = p2p_q1 * sinL + p2p_0 * sinL_q1 + p2_0 * sinLp_q1 - (p1p_q1 * cosL + p1p_0 * cosL_q1 + p1_0 * cosLp_q1);
    const double rpn_p_q2 = p2p_q2 * sinL + p2p_0 * sinL_q2 + p2_0 * sinLp_q2 - (p1p_q2 * cosL + p1p_0 * cosL_q2 + p1_0 * cosLp_q2);
    const double rpn_p_p1 = p2p_p1 * sinL + p2p_0 * sinL_p1 + p2_0 * sinLp_p1 - (p1p_p1 * cosL + p1p_0 * cosL_p1 + cosLp + p1_0 * cosLp_p1);
    const double rpn_p_p2 = p2p_p2 * sinL + p2p_0 * sinL_p2 + sinLp + p2_0 * sinLp_p2 - (p1p_p2 * cosL + p1p_0 * cosL_p2 + p1_0 * cosLp_p2);
    const double rpp = mu_norm * (rpn_p * fic + rpn * ficp);
    const double firp2 = derivatives_of_inverse_3(r, rp, rpp);
    const double f2rpp = derivatives_of_product_3(r, rp, rpp);
    const double fir2p2 = derivatives_of_inverse_3(r2, 2.0 * f2rp, 2.0 * f2rpp);
    const double r3p2 = 3.0 * (2.0 * r * (rp * rp) + r2 * rpp);
    const double fir3p2 = derivatives_of_inverse_3(r3, r3p, r3p2);
    const double rpp_nu = mu_norm * (rpn_p_nu * fic + rpn_p * fic_nu + rpn_nu * ficp + rpn * ficp_nu);
    const double rpp_Lr = mu_norm * (rpn_p_Lr * fic + rpn_p * fic_Lr + rpn_Lr * ficp + rpn * ficp_Lr);
    const double rpp_q1 = mu_norm * (rpn_p_q1 * fic + rpn_p * fic_q1 + rpn_q1 * ficp + rpn * ficp_q1);
    const double rpp_q2 = mu_norm * (rpn_p_q2 * fic + rpn_p * fic_q2 + rpn_q2 * ficp + rpn * ficp_q2);
    const double rpp_p1 = mu_norm * (rpn_p_p1 * fic + rpn_p * fic_p1 + rpn_p1 * ficp + rpn * ficp_p1);
    const double rpp_p2 = mu_norm * (rpn_p_p2 * fic + rpn_p * fic_p2 + rpn_p2 * ficp + rpn * ficp_p2);
    const double firp2_nu = derivatives_of_inverse_wrt_param_3(r, rp, rpp, r_nu, rp_nu, rpp_nu);
    const double firp2_Lr = derivatives_of_inverse_wrt_param_3(r, rp, rpp, r_Lr, rp_Lr, rpp_Lr);
    const double firp2_q1 = derivatives_of_inverse_wrt_param_3(r, rp, rpp, r_q1, rp_q1, rpp_q1);
    const double firp2_q2 = derivatives_of_inverse_wrt_param_3(r, rp, rpp, r_q2, rp_q2, rpp_q2);
    const double firp2_p1 = derivatives_of_inverse_wrt_param_3(r, rp, rpp, r_p1, rp_p1, rpp_p1);
    const double firp2_p2 = derivatives_of_inverse_wrt_param_3(r, rp, rpp, r_p2, rp_p2, rpp_p2);
    const double f2rpp_nu = derivatives_of_product_wrt_param_3(r, rp, rpp, r_nu, rp_nu, rpp_nu);
    const double f2rpp_Lr = derivatives_of_product_wrt_param_3(r, rp, rpp, r_Lr, rp_Lr, rpp_Lr);
    const double f2rpp_q1 = derivatives_of_product_wrt_param_3(r, rp, rpp, r_q1, rp_q1, rpp_q1);
    const double f2rpp_q2 = derivatives_of_product_wrt_param_3(r, rp, rpp, r_q2, rp_q2, rpp_q2);
    const double f2rpp_p1 = derivatives_of_product_wrt_param_3(r, rp, rpp, r_p1, rp_p1, rpp_p1);
    const double f2rpp_p2 = derivatives_of_product_wrt_param_3(r, rp, rpp, r_p2, rp_p2, rpp_p2);
    const double r3p2_nu = 3.0 * (2.0 * r_nu * (rp * rp) + 4.0 * r * rp * rp_nu + 2.0 * r * r_nu * rpp + r2 * rpp_nu);
    const double r3p2_Lr = 3.0 * (2.0 * r_Lr * (rp * rp) + 4.0 * r * rp * rp_Lr + 2.0 * r * r_Lr * rpp + r2 * rpp_Lr);
    const double r3p2_q1 = 3.0 * (2.0 * r_q1 * (rp * rp) + 4.0 * r * rp * rp_q1 + 2.0 * r * r_q1 * rpp + r2 * rpp_q1);
    const double r3p2_q2 = 3.0 * (2.0 * r_q2 * (rp * rp) + 4.0 * r * rp * rp_q2 + 2.0 * r * r_q2 * rpp + r2 * rpp_q2);
    const double r3p2_p1 = 3.0 * (2.0 * r_p1 * (rp * rp) + 4.0 * r * rp * rp_p1 + 2.0 * r * r_p1 * rpp + r2 * rpp_p1);
    const double r3p2_p2 = 3.0 * (2.0 * r_p2 * (rp * rp) + 4.0 * r * rp * rp_p2 + 2.0 * r * r_p2 * rpp + r2 * rpp_p2);
    const double fir3p2_nu = derivatives_of_inverse_wrt_param_3(r3, r3p, r3p2, r3_nu, r3p_nu, r3p2_nu);
    const double fir3p2_Lr = derivatives_of_inverse_wrt_param_3(r3, r3p, r3p2, r3_Lr, r3p_Lr, r3p2_Lr);
    const double fir3p2_q1 = derivatives_of_inverse_wrt_param_3(r3, r3p, r3p2, r3_q1, r3p_q1, r3p2_q1);
    const double fir3p2_q2 = derivatives_of_inverse_wrt_param_3(r3, r3p, r3p2, r3_q2, r3p_q2, r3p2_q2);
    const double fir3p2_p1 = derivatives_of_inverse_wrt_param_3(r3, r3p, r3p2, r3_p1, r3p_p1, r3p2_p1);
    const double fir3p2_p2 = derivatives_of_inverse_wrt_param_3(r3, r3p, r3p2, r3_p2, r3p_p2, r3p2_p2);
    const double fir2p2_nu = derivatives_of_inverse_wrt_param_3(r2, 2.0 * f2rp, 2.0 * f2rpp, r2_nu, 2.0 * f2rp_nu, 2.0 * f2rpp_nu);
    const double fir2p2_Lr = derivatives_of_inverse_wrt_param_3(r2, 2.0 * f2rp, 2.0 * f2rpp, r2_Lr, 2.0 * f2rp_Lr, 2.0 * f2rpp_Lr);
    const double fir2p2_q1 = derivatives_of_inverse_wrt_param_3(r2, 2.0 * f2rp, 2.0 * f2rpp, r2_q1, 2.0 * f2rp_q1, 2.0 * f2rpp_q1);
    const double fir2p2_q2 = derivatives_of_inverse_wrt_param_3(r2, 2.0 * f2rp, 2.0 * f2rpp, r2_q2, 2.0 * f2rp_q2, 2.0 * f2rpp_q2);
    const double fir2p2_p1 = derivatives_of_inverse_wrt_param_3(r2, 2.0 * f2rp, 2.0 * f2rpp, r2_p1, 2.0 * f2rp_p1, 2.0 * f2rpp_p1);
    const double fir2p2_p2 = derivatives_of_inverse_wrt_param_3(r2, 2.0 * f2rp, 2.0 * f2rpp, r2_p2, 2.0 * f2rp_p2, 2.0 * f2rpp_p2);
    const double hp = (f2cp - 2.0 * f2rp * U - r2 * Up) * fih;
    const double fihp = derivatives_of_inverse_2(h, hp);
    const double hp_nu = (f2cp_nu - 2.0 * f2rp_nu * U - 2.0 * f2rp * U_nu - 2.0 * r * r_nu * Up - r2 * Up_nu) * fih + (f2cp - 2.0 * f2rp * U - r2 * Up) * fih_nu;
    const double hp_Lr = (f2cp_Lr - 2.0 * f2rp_Lr * U - 2.0 * f2rp * U_Lr - 2.0 * r * r_Lr * Up - r2 * Up_Lr) * fih + (f2cp - 2.0 * f2rp * U - r2 * Up) * fih_Lr;
    const double hp_q1 = (f2cp_q1 - 2.0 * f2rp_q1 * U - 2.0 * f2rp * U_q1 - 2.0 * r * r_q1 * Up - r2 * Up_q1) * fih + (f2cp - 2.0 * f2rp * U - r2 * Up) * fih_q1;
    const double hp_q2 = (f2cp_q2 - 2.0 * f2rp_q2 * U - 2.0 * f2rp * U_q2 - 2.0 * r * r_q2 * Up - r2 * Up_q2) * fih + (f2cp - 2.0 * f2rp * U - r2 * Up) * fih_q2;
    const double hp_p1 = (f2cp_p1 - 2.0 * f2rp_p1 * U - 2.0 * f2rp * U_p1 - 2.0 * r * r_p1 * Up - r2 * Up_p1) * fih + (f2cp - 2.0 * f2rp * U - r2 * Up) * fih_p1;
    const double hp_p2 = (f2cp_p2 - 2.0 * f2rp_p2 * U - 2.0 * f2rp * U_p2 - 2.0 * r * r_p2 * Up - r2 * Up_p2) * fih + (f2cp - 2.0 * f2rp * U - r2 * Up) * fih_p2;
    const double fihp_nu = derivatives_of_inverse_wrt_param_2(h, hp, h_nu, hp_nu);
    const double fihp_Lr = derivatives_of_inverse_wrt_param_2(h, hp, h_Lr, hp_Lr);
    const double fihp_q1 = derivatives_of_inverse_wrt_param_2(h, hp, h_q1, hp_q1);
    const double fihp_q2 = derivatives_of_inverse_wrt_param_2(h, hp, h_q2, hp_q2);
    const double fihp_p1 = derivatives_of_inverse_wrt_param_2(h, hp, h_p1, hp_p1);
    const double fihp_p2 = derivatives_of_inverse_wrt_param_2(h, hp, h_p2, hp_p2);
    const double fibm1_p = derivatives_of_inverse_2(beta + 1.0, bp);
    const double fibm1_p_nu = derivatives_of_inverse_wrt_param_2(beta + 1.0, bp, beta_nu, bp_nu);
    const double fibm1_p_Lr = derivatives_of_inverse_wrt_param_2(beta + 1.0, bp, beta_Lr, bp_Lr);
    const double fibm1_p_q1 = derivatives_of_inverse_wrt_param_2(beta + 1.0, bp, beta_q1, bp_q1);
    const double fibm1_p_q2 = derivatives_of_inverse_wrt_param_2(beta + 1.0, bp, beta_q2, bp_q2);
    const double fibm1_p_p1 = derivatives_of_inverse_wrt_param_2(beta + 1.0, bp, beta_p1, bp_p1);
    const double fibm1_p_p2 = derivatives_of_inverse_wrt_param_2(beta + 1.0, bp, beta_p2, bp_p2);
    const double alphap = fibm1_p;
    const double fialphap = derivatives_of_inverse_2(fibm1, alphap);
    const double alphap_nu = fibm1_p_nu;
    const double alphap_Lr = fibm1_p_Lr;
    const double alphap_q1 = fibm1_p_q1;
    const double alphap_q2 = fibm1_p_q2;
    const double alphap_p1 = fibm1_p_p1;
    const double alphap_p2 = fibm1_p_p2;
    const double fialphap_nu = derivatives_of_inverse_wrt_param_2(fibm1, alphap, alpha_nu, alphap_nu);
    const double fialphap_Lr = derivatives_of_inverse_wrt_param_2(fibm1, alphap, alpha_Lr, alphap_Lr);
    const double fialphap_q1 = derivatives_of_inverse_wrt_param_2(fibm1, alphap, alpha_q1, alphap_q1);
    const double fialphap_q2 = derivatives_of_inverse_wrt_param_2(fibm1, alphap, alpha_q2, alphap_q2);
    const double fialphap_p1 = derivatives_of_inverse_wrt_param_2(fibm1, alphap, alpha_p1, alphap_p1);
    const double fialphap_p2 = derivatives_of_inverse_wrt_param_2(fibm1, alphap, alpha_p2, alphap_p2);
    const double deltap = -qsp;
    const double deltap_nu = -qsp_nu;
    const double deltap_Lr = -qsp_Lr;
    const double deltap_q1 = -qsp_q1;
    const double deltap_q2 = -qsp_q2;
    const double deltap_p1 = -qsp_p1;
    const double deltap_p2 = -qsp_p2;
    const double hr3p = hp * r3 + 3.0 * h * r2 * rp;
    double _buf__unpack_fihr3_fihr3p[2];
    derivatives_of_inverse_full_2(hr3, hr3p, _buf__unpack_fihr3_fihr3p);
    fihr3 = _buf__unpack_fihr3_fihr3p[0];
    const double fihr3p = _buf__unpack_fihr3_fihr3p[1];
    const double hr3p_nu = hp_nu * r3 + hp * r3_nu + 3.0 * h_nu * r2 * rp + 3.0 * h * (r2_nu * rp + r2 * rp_nu);
    const double hr3p_Lr = hp_Lr * r3 + hp * r3_Lr + 3.0 * h_Lr * r2 * rp + 3.0 * h * (r2_Lr * rp + r2 * rp_Lr);
    const double hr3p_q1 = hp_q1 * r3 + hp * r3_q1 + 3.0 * h_q1 * r2 * rp + 3.0 * h * (r2_q1 * rp + r2 * rp_q1);
    const double hr3p_q2 = hp_q2 * r3 + hp * r3_q2 + 3.0 * h_q2 * r2 * rp + 3.0 * h * (r2_q2 * rp + r2 * rp_q2);
    const double hr3p_p1 = hp_p1 * r3 + hp * r3_p1 + 3.0 * h_p1 * r2 * rp + 3.0 * h * (r2_p1 * rp + r2 * rp_p1);
    const double hr3p_p2 = hp_p2 * r3 + hp * r3_p2 + 3.0 * h_p2 * r2 * rp + 3.0 * h * (r2_p2 * rp + r2 * rp_p2);
    const double fihr3p_nu = derivatives_of_inverse_wrt_param_2(hr3, hr3p, hr3_nu, hr3p_nu);
    const double fihr3p_Lr = derivatives_of_inverse_wrt_param_2(hr3, hr3p, hr3_Lr, hr3p_Lr);
    const double fihr3p_q1 = derivatives_of_inverse_wrt_param_2(hr3, hr3p, hr3_q1, hr3p_q1);
    const double fihr3p_q2 = derivatives_of_inverse_wrt_param_2(hr3, hr3p, hr3_q2, hr3p_q2);
    const double fihr3p_p1 = derivatives_of_inverse_wrt_param_2(hr3, hr3p, hr3_p1, hr3p_p1);
    const double fihr3p_p2 = derivatives_of_inverse_wrt_param_2(hr3, hr3p, hr3_p2, hr3p_p2);
    const double GAMMAp_ = fialphap + alphap * (1.0 - r / a) - alpha * rp / a;
    const double Ip = 3.0 * A * ((zgp * delta + zg * deltap) * fihr3 + zg * delta * fihr3p);
    const double dp = (hp - cp) * fir2 + (h - c) * fir2p;
    const double whp = Ip * zg + I * zgp;
    const double GAMMAp_nu = fialphap_nu + alphap_nu * (1.0 - r / a) + alphap * (-r_nu / a + r / (a * a) * a_nu) - alpha_nu * rp / a - alpha * (rp_nu / a - rp / (a * a) * a_nu);
    const double GAMMAp_Lr = fialphap_Lr + alphap_Lr * (1.0 - r / a) - alphap * r_Lr / a - alpha_Lr * rp / a - alpha * rp_Lr / a;
    const double GAMMAp_q1 = fialphap_q1 + alphap_q1 * (1.0 - r / a) - alphap * r_q1 / a - alpha_q1 * rp / a - alpha * rp_q1 / a;
    const double GAMMAp_q2 = fialphap_q2 + alphap_q2 * (1.0 - r / a) - alphap * r_q2 / a - alpha_q2 * rp / a - alpha * rp_q2 / a;
    const double GAMMAp_p1 = fialphap_p1 + alphap_p1 * (1.0 - r / a) - alphap * r_p1 / a - alpha_p1 * rp / a - alpha * rp_p1 / a;
    const double GAMMAp_p2 = fialphap_p2 + alphap_p2 * (1.0 - r / a) - alphap * r_p2 / a - alpha_p2 * rp / a - alpha * rp_p2 / a;
    const double Ip_nu = 3.0 * A * ((zgp_nu * delta + zgp * delta_nu + zg_nu * deltap + zg * deltap_nu) * fihr3 + (zgp * delta + zg * deltap) * fihr3_nu + (zg_nu * delta + zg * delta_nu) * fihr3p + zg * delta * fihr3p_nu);
    const double Ip_Lr = 3.0 * A * ((zgp_Lr * delta + zgp * delta_Lr + zg_Lr * deltap + zg * deltap_Lr) * fihr3 + (zgp * delta + zg * deltap) * fihr3_Lr + (zg_Lr * delta + zg * delta_Lr) * fihr3p + zg * delta * fihr3p_Lr);
    const double Ip_q1 = 3.0 * A * ((zgp_q1 * delta + zgp * delta_q1 + zg_q1 * deltap + zg * deltap_q1) * fihr3 + (zgp * delta + zg * deltap) * fihr3_q1 + (zg_q1 * delta + zg * delta_q1) * fihr3p + zg * delta * fihr3p_q1);
    const double Ip_q2 = 3.0 * A * ((zgp_q2 * delta + zgp * delta_q2 + zg_q2 * deltap + zg * deltap_q2) * fihr3 + (zgp * delta + zg * deltap) * fihr3_q2 + (zg_q2 * delta + zg * delta_q2) * fihr3p + zg * delta * fihr3p_q2);
    const double Ip_p1 = 3.0 * A * ((zgp_p1 * delta + zgp * delta_p1 + zg_p1 * deltap + zg * deltap_p1) * fihr3 + (zgp * delta + zg * deltap) * fihr3_p1 + (zg_p1 * delta + zg * delta_p1) * fihr3p + zg * delta * fihr3p_p1);
    const double Ip_p2 = 3.0 * A * ((zgp_p2 * delta + zgp * delta_p2 + zg_p2 * deltap + zg * deltap_p2) * fihr3 + (zgp * delta + zg * deltap) * fihr3_p2 + (zg_p2 * delta + zg * delta_p2) * fihr3p + zg * delta * fihr3p_p2);
    const double dp_nu = (hp_nu - cp_nu) * fir2 + (hp - cp) * fir2_nu + (h_nu - c_nu) * fir2p + (h - c) * fir2p_nu;
    const double dp_Lr = (hp_Lr - cp_Lr) * fir2 + (hp - cp) * fir2_Lr + (h_Lr - c_Lr) * fir2p + (h - c) * fir2p_Lr;
    const double dp_q1 = (hp_q1 - cp_q1) * fir2 + (hp - cp) * fir2_q1 + (h_q1 - c_q1) * fir2p + (h - c) * fir2p_q1;
    const double dp_q2 = (hp_q2 - cp_q2) * fir2 + (hp - cp) * fir2_q2 + (h_q2 - c_q2) * fir2p + (h - c) * fir2p_q2;
    const double dp_p1 = (hp_p1 - cp_p1) * fir2 + (hp - cp) * fir2_p1 + (h_p1 - c_p1) * fir2p + (h - c) * fir2p_p1;
    const double dp_p2 = (hp_p2 - cp_p2) * fir2 + (hp - cp) * fir2_p2 + (h_p2 - c_p2) * fir2p + (h - c) * fir2p_p2;
    const double whp_nu = Ip_nu * zg + Ip * zg_nu + I_nu * zgp + I * zgp_nu;
    const double whp_Lr = Ip_Lr * zg + Ip * zg_Lr + I_Lr * zgp + I * zgp_Lr;
    const double whp_q1 = Ip_q1 * zg + Ip * zg_q1 + I_q1 * zgp + I * zgp_q1;
    const double whp_q2 = Ip_q2 * zg + Ip * zg_q2 + I_q2 * zgp + I * zgp_q2;
    const double whp_p1 = Ip_p1 * zg + Ip * zg_p1 + I_p1 * zgp + I * zgp_p1;
    const double whp_p2 = Ip_p2 * zg + Ip * zg_p2 + I_p2 * zgp + I * zgp_p2;
    const double xi1p = Xp / a + 2.0 * p2p_0;
    const double xi2p = Yp / a + 2.0 * p1p_0;
    const double xi1p_nu = Xp_nu / a - Xp / (a * a) * a_nu + 2.0 * p2p_nu;
    const double xi1p_Lr = Xp_Lr / a + 2.0 * p2p_Lr;
    const double xi1p_q1 = Xp_q1 / a + 2.0 * p2p_q1;
    const double xi1p_q2 = Xp_q2 / a + 2.0 * p2p_q2;
    const double xi1p_p1 = Xp_p1 / a + 2.0 * p2p_p1;
    const double xi1p_p2 = Xp_p2 / a + 2.0 * p2p_p2;
    const double xi2p_nu = Yp_nu / a - Yp / (a * a) * a_nu + 2.0 * p1p_nu;
    const double xi2p_Lr = Yp_Lr / a + 2.0 * p1p_Lr;
    const double xi2p_q1 = Yp_q1 / a + 2.0 * p1p_q1;
    const double xi2p_q2 = Yp_q2 / a + 2.0 * p1p_q2;
    const double xi2p_p1 = Yp_p1 / a + 2.0 * p1p_p1;
    const double xi2p_p2 = Yp_p2 / a + 2.0 * p1p_p2;
    const double p1pp_0 = p2p_0 * (d - wh) + p2_0 * (dp - whp) - U * (ficp * xi1 + fic * xi1p) - Up * fic * xi1;
    const double p2pp_0 = p1p_0 * (-d + wh) + p1_0 * (-dp + whp) + U * (ficp * xi2 + fic * xi2p) + Up * fic * xi2;
    const double Lrpp_0 = dp - whp - U * (ficp * GAMMA_ + fic * GAMMAp_) - Up * fic * GAMMA_;
    const double q1pp_0 = -Ip * sinL - I * sinLp;
    const double q2pp_0 = -Ip * cosL - I * cosLp;
    out.map_components_col1[0] = 0.0;
    out.map_components_col1[1] = q1pp_0;
    out.map_components_col1[2] = q2pp_0;
    out.map_components_col1[3] = p1pp_0;
    out.map_components_col1[4] = p2pp_0;
    out.map_components_col1[5] = Lrpp_0;
    const double q1p2_nu = -Ip_nu * sinL - Ip * sinL_nu - I_nu * sinLp - I * sinLp_nu;
    const double q1p2_Lr = -Ip_Lr * sinL - Ip * sinL_Lr - I_Lr * sinLp - I * sinLp_Lr;
    const double q1p2_q1 = -Ip_q1 * sinL - Ip * sinL_q1 - I_q1 * sinLp - I * sinLp_q1;
    const double q1p2_q2 = -Ip_q2 * sinL - Ip * sinL_q2 - I_q2 * sinLp - I * sinLp_q2;
    const double q1p2_p1 = -Ip_p1 * sinL - Ip * sinL_p1 - I_p1 * sinLp - I * sinLp_p1;
    const double q1p2_p2 = -Ip_p2 * sinL - Ip * sinL_p2 - I_p2 * sinLp - I * sinLp_p2;
    const double q2p2_nu = -Ip_nu * cosL - Ip * cosL_nu - I_nu * cosLp - I * cosLp_nu;
    const double q2p2_Lr = -Ip_Lr * cosL - Ip * cosL_Lr - I_Lr * cosLp - I * cosLp_Lr;
    const double q2p2_q1 = -Ip_q1 * cosL - Ip * cosL_q1 - I_q1 * cosLp - I * cosLp_q1;
    const double q2p2_q2 = -Ip_q2 * cosL - Ip * cosL_q2 - I_q2 * cosLp - I * cosLp_q2;
    const double q2p2_p1 = -Ip_p1 * cosL - Ip * cosL_p1 - I_p1 * cosLp - I * cosLp_p1;
    const double q2p2_p2 = -Ip_p2 * cosL - Ip * cosL_p2 - I_p2 * cosLp - I * cosLp_p2;
    const double p1p2_nu = p2p_nu * (d - wh) + p2p_0 * (d_nu - wh_nu) + p2_0 * (dp_nu - whp_nu) - U_nu * (ficp * xi1 + fic * xi1p) - U * (ficp_nu * xi1 + ficp * xi1_nu + fic_nu * xi1p + fic * xi1p_nu) - Up_nu * fic * xi1 - Up * (fic_nu * xi1 + fic * xi1_nu);
    const double p1p2_Lr = p2p_Lr * (d - wh) + p2p_0 * (d_Lr - wh_Lr) + p2_0 * (dp_Lr - whp_Lr) - U_Lr * (ficp * xi1 + fic * xi1p) - U * (ficp_Lr * xi1 + ficp * xi1_Lr + fic_Lr * xi1p + fic * xi1p_Lr) - Up_Lr * fic * xi1 - Up * (fic_Lr * xi1 + fic * xi1_Lr);
    const double p1p2_q1 = p2p_q1 * (d - wh) + p2p_0 * (d_q1 - wh_q1) + p2_0 * (dp_q1 - whp_q1) - U_q1 * (ficp * xi1 + fic * xi1p) - U * (ficp_q1 * xi1 + ficp * xi1_q1 + fic_q1 * xi1p + fic * xi1p_q1) - Up_q1 * fic * xi1 - Up * (fic_q1 * xi1 + fic * xi1_q1);
    const double p1p2_q2 = p2p_q2 * (d - wh) + p2p_0 * (d_q2 - wh_q2) + p2_0 * (dp_q2 - whp_q2) - U_q2 * (ficp * xi1 + fic * xi1p) - U * (ficp_q2 * xi1 + ficp * xi1_q2 + fic_q2 * xi1p + fic * xi1p_q2) - Up_q2 * fic * xi1 - Up * (fic_q2 * xi1 + fic * xi1_q2);
    const double p1p2_p1 = p2p_p1 * (d - wh) + p2p_0 * (d_p1 - wh_p1) + p2_0 * (dp_p1 - whp_p1) - U_p1 * (ficp * xi1 + fic * xi1p) - U * (ficp_p1 * xi1 + ficp * xi1_p1 + fic_p1 * xi1p + fic * xi1p_p1) - Up_p1 * fic * xi1 - Up * (fic_p1 * xi1 + fic * xi1_p1);
    const double p1p2_p2 = p2p_p2 * (d - wh) + p2p_0 * (d_p2 - wh_p2) + dp - whp + p2_0 * (dp_p2 - whp_p2) - U_p2 * (ficp * xi1 + fic * xi1p) - U * (ficp_p2 * xi1 + ficp * xi1_p2 + fic_p2 * xi1p + fic * xi1p_p2) - Up_p2 * fic * xi1 - Up * (fic_p2 * xi1 + fic * xi1_p2);
    const double p2p2_nu = p1p_nu * (-d + wh) + p1p_0 * (-d_nu + wh_nu) + p1_0 * (-dp_nu + whp_nu) + U_nu * (ficp * xi2 + fic * xi2p) + U * (ficp_nu * xi2 + ficp * xi2_nu + fic_nu * xi2p + fic * xi2p_nu) + Up_nu * fic * xi2 + Up * (fic_nu * xi2 + fic * xi2_nu);
    const double p2p2_Lr = p1p_Lr * (-d + wh) + p1p_0 * (-d_Lr + wh_Lr) + p1_0 * (-dp_Lr + whp_Lr) + U_Lr * (ficp * xi2 + fic * xi2p) + U * (ficp_Lr * xi2 + ficp * xi2_Lr + fic_Lr * xi2p + fic * xi2p_Lr) + Up_Lr * fic * xi2 + Up * (fic_Lr * xi2 + fic * xi2_Lr);
    const double p2p2_q1 = p1p_q1 * (-d + wh) + p1p_0 * (-d_q1 + wh_q1) + p1_0 * (-dp_q1 + whp_q1) + U_q1 * (ficp * xi2 + fic * xi2p) + U * (ficp_q1 * xi2 + ficp * xi2_q1 + fic_q1 * xi2p + fic * xi2p_q1) + Up_q1 * fic * xi2 + Up * (fic_q1 * xi2 + fic * xi2_q1);
    const double p2p2_q2 = p1p_q2 * (-d + wh) + p1p_0 * (-d_q2 + wh_q2) + p1_0 * (-dp_q2 + whp_q2) + U_q2 * (ficp * xi2 + fic * xi2p) + U * (ficp_q2 * xi2 + ficp * xi2_q2 + fic_q2 * xi2p + fic * xi2p_q2) + Up_q2 * fic * xi2 + Up * (fic_q2 * xi2 + fic * xi2_q2);
    const double p2p2_p1 = p1p_p1 * (-d + wh) + p1p_0 * (-d_p1 + wh_p1) + -dp + whp + p1_0 * (-dp_p1 + whp_p1) + U_p1 * (ficp * xi2 + fic * xi2p) + U * (ficp_p1 * xi2 + ficp * xi2_p1 + fic_p1 * xi2p + fic * xi2p_p1) + Up_p1 * fic * xi2 + Up * (fic_p1 * xi2 + fic * xi2_p1);
    const double p2p2_p2 = p1p_p2 * (-d + wh) + p1p_0 * (-d_p2 + wh_p2) + p1_0 * (-dp_p2 + whp_p2) + U_p2 * (ficp * xi2 + fic * xi2p) + U * (ficp_p2 * xi2 + ficp * xi2_p2 + fic_p2 * xi2p + fic * xi2p_p2) + Up_p2 * fic * xi2 + Up * (fic_p2 * xi2 + fic * xi2_p2);
    const double Lrp2_nu = dp_nu - whp_nu - U_nu * (ficp * GAMMA_ + fic * GAMMAp_) - U * (ficp_nu * GAMMA_ + ficp * GAMMA_nu + fic_nu * GAMMAp_ + fic * GAMMAp_nu) - Up_nu * fic * GAMMA_ - Up * (fic_nu * GAMMA_ + fic * GAMMA_nu);
    const double Lrp2_Lr = dp_Lr - whp_Lr - U_Lr * (ficp * GAMMA_ + fic * GAMMAp_) - U * (ficp_Lr * GAMMA_ + ficp * GAMMA_Lr + fic_Lr * GAMMAp_ + fic * GAMMAp_Lr) - Up_Lr * fic * GAMMA_ - Up * (fic_Lr * GAMMA_ + fic * GAMMA_Lr);
    const double Lrp2_q1 = dp_q1 - whp_q1 - U_q1 * (ficp * GAMMA_ + fic * GAMMAp_) - U * (ficp_q1 * GAMMA_ + ficp * GAMMA_q1 + fic_q1 * GAMMAp_ + fic * GAMMAp_q1) - Up_q1 * fic * GAMMA_ - Up * (fic_q1 * GAMMA_ + fic * GAMMA_q1);
    const double Lrp2_q2 = dp_q2 - whp_q2 - U_q2 * (ficp * GAMMA_ + fic * GAMMAp_) - U * (ficp_q2 * GAMMA_ + ficp * GAMMA_q2 + fic_q2 * GAMMAp_ + fic * GAMMAp_q2) - Up_q2 * fic * GAMMA_ - Up * (fic_q2 * GAMMA_ + fic * GAMMA_q2);
    const double Lrp2_p1 = dp_p1 - whp_p1 - U_p1 * (ficp * GAMMA_ + fic * GAMMAp_) - U * (ficp_p1 * GAMMA_ + ficp * GAMMA_p1 + fic_p1 * GAMMAp_ + fic * GAMMAp_p1) - Up_p1 * fic * GAMMA_ - Up * (fic_p1 * GAMMA_ + fic * GAMMA_p1);
    const double Lrp2_p2 = dp_p2 - whp_p2 - U_p2 * (ficp * GAMMA_ + fic * GAMMAp_) - U * (ficp_p2 * GAMMA_ + ficp * GAMMA_p2 + fic_p2 * GAMMAp_ + fic * GAMMAp_p2) - Up_p2 * fic * GAMMA_ - Up * (fic_p2 * GAMMA_ + fic * GAMMA_p2);
    inter.Xp = Xp;
    inter.Yp = Yp;
    inter.cosLp = cosLp;
    inter.sinLp = sinLp;
    inter.hp = hp;
    inter.dp = dp;
    inter.whp = whp;
    inter.Ip = Ip;
    inter.Up = Up;
    inter.bp = bp;
    inter.cp = cp;
    inter.alphap = alphap;
    inter.deltap = deltap;
    inter.zgp = zgp;
    inter.fUzp = fUzp;
    inter.GAMMAp_ = GAMMAp_;
    inter.xi1p = xi1p;
    inter.xi2p = xi2p;
    inter.qsp = qsp;
    inter.psp = psp;
    inter.Cp = Cp;
    inter.Dp = Dp;
    inter.rpn_p = rpn_p;
    inter.rpp = rpp;
    inter.fiDp = fiDp;
    inter.fibp = fibp;
    inter.ficp = ficp;
    inter.fihp = fihp;
    inter.fialphap = fialphap;
    inter.fibm1_p = fibm1_p;
    inter.fihr3 = fihr3;
    inter.fihr3p = fihr3p;
    inter.firp2 = firp2;
    inter.fir2p2 = fir2p2;
    inter.fir3p2 = fir3p2;
    inter.f2rpp = f2rpp;
    inter.f2cp = f2cp;
    inter.r3p2 = r3p2;
    inter.hr3p = hr3p;
    inter.Xp_nu = Xp_nu;
    inter.Xp_Lr = Xp_Lr;
    inter.Xp_q1 = Xp_q1;
    inter.Xp_q2 = Xp_q2;
    inter.Xp_p1 = Xp_p1;
    inter.Xp_p2 = Xp_p2;
    inter.Yp_nu = Yp_nu;
    inter.Yp_Lr = Yp_Lr;
    inter.Yp_q1 = Yp_q1;
    inter.Yp_q2 = Yp_q2;
    inter.Yp_p1 = Yp_p1;
    inter.Yp_p2 = Yp_p2;
    inter.cosLp_nu = cosLp_nu;
    inter.cosLp_Lr = cosLp_Lr;
    inter.cosLp_q1 = cosLp_q1;
    inter.cosLp_q2 = cosLp_q2;
    inter.cosLp_p1 = cosLp_p1;
    inter.cosLp_p2 = cosLp_p2;
    inter.sinLp_nu = sinLp_nu;
    inter.sinLp_Lr = sinLp_Lr;
    inter.sinLp_q1 = sinLp_q1;
    inter.sinLp_q2 = sinLp_q2;
    inter.sinLp_p1 = sinLp_p1;
    inter.sinLp_p2 = sinLp_p2;
    inter.zgp_nu = zgp_nu;
    inter.zgp_Lr = zgp_Lr;
    inter.zgp_q1 = zgp_q1;
    inter.zgp_q2 = zgp_q2;
    inter.zgp_p1 = zgp_p1;
    inter.zgp_p2 = zgp_p2;
    inter.fUzp_nu = fUzp_nu;
    inter.fUzp_Lr = fUzp_Lr;
    inter.fUzp_q1 = fUzp_q1;
    inter.fUzp_q2 = fUzp_q2;
    inter.fUzp_p1 = fUzp_p1;
    inter.fUzp_p2 = fUzp_p2;
    inter.Up_nu = Up_nu;
    inter.Up_Lr = Up_Lr;
    inter.Up_q1 = Up_q1;
    inter.Up_q2 = Up_q2;
    inter.Up_p1 = Up_p1;
    inter.Up_p2 = Up_p2;
    inter.bp_nu = bp_nu;
    inter.bp_Lr = bp_Lr;
    inter.bp_q1 = bp_q1;
    inter.bp_q2 = bp_q2;
    inter.bp_p1 = bp_p1;
    inter.bp_p2 = bp_p2;
    inter.cp_nu = cp_nu;
    inter.cp_Lr = cp_Lr;
    inter.cp_q1 = cp_q1;
    inter.cp_q2 = cp_q2;
    inter.cp_p1 = cp_p1;
    inter.cp_p2 = cp_p2;
    inter.hp_nu = hp_nu;
    inter.hp_Lr = hp_Lr;
    inter.hp_q1 = hp_q1;
    inter.hp_q2 = hp_q2;
    inter.hp_p1 = hp_p1;
    inter.hp_p2 = hp_p2;
    inter.alphap_nu = alphap_nu;
    inter.alphap_Lr = alphap_Lr;
    inter.alphap_q1 = alphap_q1;
    inter.alphap_q2 = alphap_q2;
    inter.alphap_p1 = alphap_p1;
    inter.alphap_p2 = alphap_p2;
    inter.deltap_nu = deltap_nu;
    inter.deltap_Lr = deltap_Lr;
    inter.deltap_q1 = deltap_q1;
    inter.deltap_q2 = deltap_q2;
    inter.deltap_p1 = deltap_p1;
    inter.deltap_p2 = deltap_p2;
    inter.Ip_nu = Ip_nu;
    inter.Ip_Lr = Ip_Lr;
    inter.Ip_q1 = Ip_q1;
    inter.Ip_q2 = Ip_q2;
    inter.Ip_p1 = Ip_p1;
    inter.Ip_p2 = Ip_p2;
    inter.dp_nu = dp_nu;
    inter.dp_Lr = dp_Lr;
    inter.dp_q1 = dp_q1;
    inter.dp_q2 = dp_q2;
    inter.dp_p1 = dp_p1;
    inter.dp_p2 = dp_p2;
    inter.whp_nu = whp_nu;
    inter.whp_Lr = whp_Lr;
    inter.whp_q1 = whp_q1;
    inter.whp_q2 = whp_q2;
    inter.whp_p1 = whp_p1;
    inter.whp_p2 = whp_p2;
    inter.GAMMAp_nu = GAMMAp_nu;
    inter.GAMMAp_Lr = GAMMAp_Lr;
    inter.GAMMAp_q1 = GAMMAp_q1;
    inter.GAMMAp_q2 = GAMMAp_q2;
    inter.GAMMAp_p1 = GAMMAp_p1;
    inter.GAMMAp_p2 = GAMMAp_p2;
    inter.xi1p_nu = xi1p_nu;
    inter.xi1p_Lr = xi1p_Lr;
    inter.xi1p_q1 = xi1p_q1;
    inter.xi1p_q2 = xi1p_q2;
    inter.xi1p_p1 = xi1p_p1;
    inter.xi1p_p2 = xi1p_p2;
    inter.xi2p_nu = xi2p_nu;
    inter.xi2p_Lr = xi2p_Lr;
    inter.xi2p_q1 = xi2p_q1;
    inter.xi2p_q2 = xi2p_q2;
    inter.xi2p_p1 = xi2p_p1;
    inter.xi2p_p2 = xi2p_p2;
    inter.qsp_nu = qsp_nu;
    inter.qsp_Lr = qsp_Lr;
    inter.qsp_q1 = qsp_q1;
    inter.qsp_q2 = qsp_q2;
    inter.qsp_p1 = qsp_p1;
    inter.qsp_p2 = qsp_p2;
    inter.psp_nu = psp_nu;
    inter.psp_Lr = psp_Lr;
    inter.psp_q1 = psp_q1;
    inter.psp_q2 = psp_q2;
    inter.psp_p1 = psp_p1;
    inter.psp_p2 = psp_p2;
    inter.Cp_nu = Cp_nu;
    inter.Cp_Lr = Cp_Lr;
    inter.Cp_q1 = Cp_q1;
    inter.Cp_q2 = Cp_q2;
    inter.Cp_p1 = Cp_p1;
    inter.Cp_p2 = Cp_p2;
    inter.Dp_nu = Dp_nu;
    inter.Dp_Lr = Dp_Lr;
    inter.Dp_q1 = Dp_q1;
    inter.Dp_q2 = Dp_q2;
    inter.Dp_p1 = Dp_p1;
    inter.Dp_p2 = Dp_p2;
    inter.fiDp_nu = fiDp_nu;
    inter.fiDp_Lr = fiDp_Lr;
    inter.fiDp_q1 = fiDp_q1;
    inter.fiDp_q2 = fiDp_q2;
    inter.fiDp_p1 = fiDp_p1;
    inter.fiDp_p2 = fiDp_p2;
    inter.rpn_p_nu = rpn_p_nu;
    inter.rpn_p_Lr = rpn_p_Lr;
    inter.rpn_p_q1 = rpn_p_q1;
    inter.rpn_p_q2 = rpn_p_q2;
    inter.rpn_p_p1 = rpn_p_p1;
    inter.rpn_p_p2 = rpn_p_p2;
    inter.rpp_nu = rpp_nu;
    inter.rpp_Lr = rpp_Lr;
    inter.rpp_q1 = rpp_q1;
    inter.rpp_q2 = rpp_q2;
    inter.rpp_p1 = rpp_p1;
    inter.rpp_p2 = rpp_p2;
    inter.r3p2_nu = r3p2_nu;
    inter.r3p2_Lr = r3p2_Lr;
    inter.r3p2_q1 = r3p2_q1;
    inter.r3p2_q2 = r3p2_q2;
    inter.r3p2_p1 = r3p2_p1;
    inter.r3p2_p2 = r3p2_p2;
    inter.hr3p_nu = hr3p_nu;
    inter.hr3p_Lr = hr3p_Lr;
    inter.hr3p_q1 = hr3p_q1;
    inter.hr3p_q2 = hr3p_q2;
    inter.hr3p_p1 = hr3p_p1;
    inter.hr3p_p2 = hr3p_p2;
    inter.fihr3p_nu = fihr3p_nu;
    inter.fihr3p_Lr = fihr3p_Lr;
    inter.fihr3p_q1 = fihr3p_q1;
    inter.fihr3p_q2 = fihr3p_q2;
    inter.fihr3p_p1 = fihr3p_p1;
    inter.fihr3p_p2 = fihr3p_p2;
    inter.firp2_nu = firp2_nu;
    inter.firp2_Lr = firp2_Lr;
    inter.firp2_q1 = firp2_q1;
    inter.firp2_q2 = firp2_q2;
    inter.firp2_p1 = firp2_p1;
    inter.firp2_p2 = firp2_p2;
    inter.fir2p2_nu = fir2p2_nu;
    inter.fir2p2_Lr = fir2p2_Lr;
    inter.fir2p2_q1 = fir2p2_q1;
    inter.fir2p2_q2 = fir2p2_q2;
    inter.fir2p2_p1 = fir2p2_p1;
    inter.fir2p2_p2 = fir2p2_p2;
    inter.fir3p2_nu = fir3p2_nu;
    inter.fir3p2_Lr = fir3p2_Lr;
    inter.fir3p2_q1 = fir3p2_q1;
    inter.fir3p2_q2 = fir3p2_q2;
    inter.fir3p2_p1 = fir3p2_p1;
    inter.fir3p2_p2 = fir3p2_p2;
    inter.f2rpp_nu = f2rpp_nu;
    inter.f2rpp_Lr = f2rpp_Lr;
    inter.f2rpp_q1 = f2rpp_q1;
    inter.f2rpp_q2 = f2rpp_q2;
    inter.f2rpp_p1 = f2rpp_p1;
    inter.f2rpp_p2 = f2rpp_p2;
    inter.f2cp_nu = f2cp_nu;
    inter.f2cp_Lr = f2cp_Lr;
    inter.f2cp_q1 = f2cp_q1;
    inter.f2cp_q2 = f2cp_q2;
    inter.f2cp_p1 = f2cp_p1;
    inter.f2cp_p2 = f2cp_p2;
    inter.ficp_nu = ficp_nu;
    inter.ficp_Lr = ficp_Lr;
    inter.ficp_q1 = ficp_q1;
    inter.ficp_q2 = ficp_q2;
    inter.ficp_p1 = ficp_p1;
    inter.ficp_p2 = ficp_p2;
    inter.fihp_nu = fihp_nu;
    inter.fihp_Lr = fihp_Lr;
    inter.fihp_q1 = fihp_q1;
    inter.fihp_q2 = fihp_q2;
    inter.fihp_p1 = fihp_p1;
    inter.fihp_p2 = fihp_p2;
    inter.fialphap_nu = fialphap_nu;
    inter.fialphap_Lr = fialphap_Lr;
    inter.fialphap_q1 = fialphap_q1;
    inter.fialphap_q2 = fialphap_q2;
    inter.fialphap_p1 = fialphap_p1;
    inter.fialphap_p2 = fialphap_p2;
    inter.fib_nu = fib_nu;
    inter.fib_Lr = fib_Lr;
    inter.fib_q1 = fib_q1;
    inter.fib_q2 = fib_q2;
    inter.fib_p1 = fib_p1;
    inter.fib_p2 = fib_p2;
    inter.fibp_nu = fibp_nu;
    inter.fibp_Lr = fibp_Lr;
    inter.fibp_q1 = fibp_q1;
    inter.fibp_q2 = fibp_q2;
    inter.fibp_p1 = fibp_p1;
    inter.fibp_p2 = fibp_p2;
    inter.fibm1_p_nu = fibm1_p_nu;
    inter.fibm1_p_Lr = fibm1_p_Lr;
    inter.fibm1_p_q1 = fibm1_p_q1;
    inter.fibm1_p_q2 = fibm1_p_q2;
    inter.fibm1_p_p1 = fibm1_p_p1;
    inter.fibm1_p_p2 = fibm1_p_p2;
    inter.hr_nu = hr_nu;
    inter.hr_Lr = hr_Lr;
    inter.hr_q1 = hr_q1;
    inter.hr_q2 = hr_q2;
    inter.hr_p1 = hr_p1;
    inter.hr_p2 = hr_p2;
    inter.qs_nu = qs_nu;
    inter.qs_Lr = qs_Lr;
    inter.qs_q1 = qs_q1;
    inter.qs_q2 = qs_q2;
    inter.qs_p1 = qs_p1;
    inter.qs_p2 = qs_p2;
    inter.fic = fic;
    inter.q1p2_nu = q1p2_nu;
    inter.q1p2_Lr = q1p2_Lr;
    inter.q1p2_q1 = q1p2_q1;
    inter.q1p2_q2 = q1p2_q2;
    inter.q1p2_p1 = q1p2_p1;
    inter.q1p2_p2 = q1p2_p2;
    inter.q2p2_nu = q2p2_nu;
    inter.q2p2_Lr = q2p2_Lr;
    inter.q2p2_q1 = q2p2_q1;
    inter.q2p2_q2 = q2p2_q2;
    inter.q2p2_p1 = q2p2_p1;
    inter.q2p2_p2 = q2p2_p2;
    inter.p1p2_nu = p1p2_nu;
    inter.p1p2_Lr = p1p2_Lr;
    inter.p1p2_q1 = p1p2_q1;
    inter.p1p2_q2 = p1p2_q2;
    inter.p1p2_p1 = p1p2_p1;
    inter.p1p2_p2 = p1p2_p2;
    inter.p2p2_nu = p2p2_nu;
    inter.p2p2_Lr = p2p2_Lr;
    inter.p2p2_q1 = p2p2_q1;
    inter.p2p2_q2 = p2p2_q2;
    inter.p2p2_p1 = p2p2_p1;
    inter.p2p2_p2 = p2p2_p2;
    inter.Lrp2_nu = Lrp2_nu;
    inter.Lrp2_Lr = Lrp2_Lr;
    inter.Lrp2_q1 = Lrp2_q1;
    inter.Lrp2_q2 = Lrp2_q2;
    inter.Lrp2_p1 = Lrp2_p1;
    inter.Lrp2_p2 = Lrp2_p2;
    inter.p1pp_0 = p1pp_0;
    inter.p2pp_0 = p2pp_0;
    inter.Lrpp_0 = Lrpp_0;
    inter.q1pp_0 = q1pp_0;
    inter.q2pp_0 = q2pp_0;

    // Fill coefficients struct
    out.q1pp_0 = q1pp_0;
    out.q2pp_0 = q2pp_0;
    out.p1pp_0 = p1pp_0;
    out.p2pp_0 = p2pp_0;
    out.Lrpp_0 = Lrpp_0;
    out.q1p2_nu = q1p2_nu;
    out.q1p2_Lr = q1p2_Lr;
    out.q1p2_q1 = q1p2_q1;
    out.q1p2_q2 = q1p2_q2;
    out.q1p2_p1 = q1p2_p1;
    out.q1p2_p2 = q1p2_p2;
    out.q2p2_nu = q2p2_nu;
    out.q2p2_Lr = q2p2_Lr;
    out.q2p2_q1 = q2p2_q1;
    out.q2p2_q2 = q2p2_q2;
    out.q2p2_p1 = q2p2_p1;
    out.q2p2_p2 = q2p2_p2;
    out.p1p2_nu = p1p2_nu;
    out.p1p2_Lr = p1p2_Lr;
    out.p1p2_q1 = p1p2_q1;
    out.p1p2_q2 = p1p2_q2;
    out.p1p2_p1 = p1p2_p1;
    out.p1p2_p2 = p1p2_p2;
    out.p2p2_nu = p2p2_nu;
    out.p2p2_Lr = p2p2_Lr;
    out.p2p2_q1 = p2p2_q1;
    out.p2p2_q2 = p2p2_q2;
    out.p2p2_p1 = p2p2_p1;
    out.p2p2_p2 = p2p2_p2;
    out.Lrp2_nu = Lrp2_nu;
    out.Lrp2_Lr = Lrp2_Lr;
    out.Lrp2_q1 = Lrp2_q1;
    out.Lrp2_q2 = Lrp2_q2;
    out.Lrp2_p1 = Lrp2_p1;
    out.Lrp2_p2 = Lrp2_p2;
}

void evaluate_order_2(
    const Order2Coefficients& coeffs,
    const double* dt_norm,
    std::size_t M,
    double* y_prop,
    Order1EvaluationScratch& scratch
) {
    for (std::size_t i = 0; i < M; ++i) {
        const double dt = dt_norm[i];
        const double q1pp_0 = coeffs.q1pp_0;
        const double q2pp_0 = coeffs.q2pp_0;
        const double p1pp_0 = coeffs.p1pp_0;
        const double p2pp_0 = coeffs.p2pp_0;
        const double Lrpp_0 = coeffs.Lrpp_0;
        const double q1p2_nu = coeffs.q1p2_nu;
        const double q1p2_Lr = coeffs.q1p2_Lr;
        const double q1p2_q1 = coeffs.q1p2_q1;
        const double q1p2_q2 = coeffs.q1p2_q2;
        const double q1p2_p1 = coeffs.q1p2_p1;
        const double q1p2_p2 = coeffs.q1p2_p2;
        const double q2p2_nu = coeffs.q2p2_nu;
        const double q2p2_Lr = coeffs.q2p2_Lr;
        const double q2p2_q1 = coeffs.q2p2_q1;
        const double q2p2_q2 = coeffs.q2p2_q2;
        const double q2p2_p1 = coeffs.q2p2_p1;
        const double q2p2_p2 = coeffs.q2p2_p2;
        const double p1p2_nu = coeffs.p1p2_nu;
        const double p1p2_Lr = coeffs.p1p2_Lr;
        const double p1p2_q1 = coeffs.p1p2_q1;
        const double p1p2_q2 = coeffs.p1p2_q2;
        const double p1p2_p1 = coeffs.p1p2_p1;
        const double p1p2_p2 = coeffs.p1p2_p2;
        const double p2p2_nu = coeffs.p2p2_nu;
        const double p2p2_Lr = coeffs.p2p2_Lr;
        const double p2p2_q1 = coeffs.p2p2_q1;
        const double p2p2_q2 = coeffs.p2p2_q2;
        const double p2p2_p1 = coeffs.p2p2_p1;
        const double p2p2_p2 = coeffs.p2p2_p2;
        const double Lrp2_nu = coeffs.Lrp2_nu;
        const double Lrp2_Lr = coeffs.Lrp2_Lr;
        const double Lrp2_q1 = coeffs.Lrp2_q1;
        const double Lrp2_q2 = coeffs.Lrp2_q2;
        const double Lrp2_p1 = coeffs.Lrp2_p1;
        const double Lrp2_p2 = coeffs.Lrp2_p2;
        const double dt2 = (dt * dt);
        y_prop[idx_yprop(i, 1)] += q1pp_0 * dt2 / 2.0;
        y_prop[idx_yprop(i, 2)] += q2pp_0 * dt2 / 2.0;
        y_prop[idx_yprop(i, 3)] += p1pp_0 * dt2 / 2.0;
        y_prop[idx_yprop(i, 4)] += p2pp_0 * dt2 / 2.0;
        y_prop[idx_yprop(i, 5)] += Lrpp_0 * dt2 / 2.0;
        scratch.Lr_nu[i] += Lrp2_nu * dt2 / 2.0;
        scratch.Lr_Lr[i] += Lrp2_Lr * dt2 / 2.0;
        scratch.Lr_q1[i] += Lrp2_q1 * dt2 / 2.0;
        scratch.Lr_q2[i] += Lrp2_q2 * dt2 / 2.0;
        scratch.Lr_p1[i] += Lrp2_p1 * dt2 / 2.0;
        scratch.Lr_p2[i] += Lrp2_p2 * dt2 / 2.0;
        scratch.q1_nu[i] += q1p2_nu * dt2 / 2.0;
        scratch.q1_Lr[i] += q1p2_Lr * dt2 / 2.0;
        scratch.q1_q1[i] += q1p2_q1 * dt2 / 2.0;
        scratch.q1_q2[i] += q1p2_q2 * dt2 / 2.0;
        scratch.q1_p1[i] += q1p2_p1 * dt2 / 2.0;
        scratch.q1_p2[i] += q1p2_p2 * dt2 / 2.0;
        scratch.q2_nu[i] += q2p2_nu * dt2 / 2.0;
        scratch.q2_Lr[i] += q2p2_Lr * dt2 / 2.0;
        scratch.q2_q1[i] += q2p2_q1 * dt2 / 2.0;
        scratch.q2_q2[i] += q2p2_q2 * dt2 / 2.0;
        scratch.q2_p1[i] += q2p2_p1 * dt2 / 2.0;
        scratch.q2_p2[i] += q2p2_p2 * dt2 / 2.0;
        scratch.p1_nu[i] += p1p2_nu * dt2 / 2.0;
        scratch.p1_Lr[i] += p1p2_Lr * dt2 / 2.0;
        scratch.p1_q1[i] += p1p2_q1 * dt2 / 2.0;
        scratch.p1_q2[i] += p1p2_q2 * dt2 / 2.0;
        scratch.p1_p1[i] += p1p2_p1 * dt2 / 2.0;
        scratch.p1_p2[i] += p1p2_p2 * dt2 / 2.0;
        scratch.p2_nu[i] += p2p2_nu * dt2 / 2.0;
        scratch.p2_Lr[i] += p2p2_Lr * dt2 / 2.0;
        scratch.p2_q1[i] += p2p2_q1 * dt2 / 2.0;
        scratch.p2_q2[i] += p2p2_q2 * dt2 / 2.0;
        scratch.p2_p1[i] += p2p2_p1 * dt2 / 2.0;
        scratch.p2_p2[i] += p2p2_p2 * dt2 / 2.0;
    }
}

} // namespace geqoe
} // namespace astrodyn_core
