/*

* This file is part of CasADi.

*

* CasADi -- A symbolic framework for dynamic optimization.

* Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl,

* KU Leuven. All rights reserved.

* Copyright (C) 2011-2014 Greg Horn

*

* CasADi is free software; you can redistribute it and/or

* modify it under the terms of the GNU Lesser General Public

* License as published by the Free Software Foundation; either

* version 3 of the License, or (at your option) any later version.

*

* CasADi 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

* Lesser General Public License for more details.

*

* You should have received a copy of the GNU Lesser General Public

* License along with CasADi; if not, write to the Free Software

* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA

*

*/

#include "integrator_impl.hpp"

#include "casadi_misc.hpp"

namespace casadi {

std::string to_string(DynIn v) {

switch (v) {

case DYN_T: return "t";

case DYN_X: return "x";

case DYN_Z: return "z";

case DYN_P: return "p";

case DYN_U: return "u";

default: break;

}

return "";

}

std::string to_string(DynOut v) {

switch (v) {

case DYN_ODE: return "ode";

case DYN_ALG: return "alg";

case DYN_QUAD: return "quad";

case DYN_ZERO: return "zero";

default: break;

}

return "";

}

std::string to_string(EventIn v) {

switch (v) {

case EVENT_INDEX: return "index";

case EVENT_T: return "t";

case EVENT_X: return "x";

case EVENT_Z: return "z";

case EVENT_P: return "p";

case EVENT_U: return "u";

default: break;

}

return "";

}

std::string to_string(EventOut v) {

switch (v) {

case EVENT_POST_X: return "post_x";

case EVENT_POST_Z: return "post_z";

default: break;

}

return "";

}

std::string Integrator::bdyn_in(casadi_int i) {

switch (i) {

case BDYN_T: return "t";

case BDYN_X: return "x";

case BDYN_Z: return "z";

case BDYN_P: return "p";

case BDYN_U: return "u";

case BDYN_OUT_ODE: return "out_ode";

case BDYN_OUT_ALG: return "out_alg";

case BDYN_OUT_QUAD: return "out_quad";

case BDYN_OUT_ZERO: return "out_zero";

case BDYN_ADJ_ODE: return "adj_ode";

case BDYN_ADJ_ALG: return "adj_alg";

case BDYN_ADJ_QUAD: return "adj_quad";

case BDYN_ADJ_ZERO: return "adj_zero";

default: break;

}

return "";

}

std::vector<std::string> Integrator::bdyn_in() {

std::vector<std::string> ret(BDYN_NUM_IN);

for (casadi_int i = 0; i < BDYN_NUM_IN; ++i)

ret[i] = bdyn_in(i);

return ret;

}

std::string Integrator::bdyn_out(casadi_int i) {

switch (i) {

case BDYN_ADJ_T: return "adj_t";

case BDYN_ADJ_X: return "adj_x";

case BDYN_ADJ_Z: return "adj_z";

case BDYN_ADJ_P: return "adj_p";

case BDYN_ADJ_U: return "adj_u";

default: break;

}

return "";

}

std::vector<std::string> Integrator::bdyn_out() {

std::vector<std::string> ret(BDYN_NUM_OUT);

for (casadi_int i = 0; i < BDYN_NUM_OUT; ++i)

ret[i] = bdyn_out(i);

return ret;

}

bool has_integrator(const std::string& name) {

return Integrator::has_plugin(name);

}

void load_integrator(const std::string& name) {

Integrator::load_plugin(name);

}

std::string doc_integrator(const std::string& name) {

return Integrator::getPlugin(name).doc;

}

Function integrator(const std::string& name, const std::string& solver,

const SXDict& dae, const Dict& opts) {

return integrator(name, solver, dae, 0.0, std::vector<double>{1.0}, opts);

}

Function integrator(const std::string& name, const std::string& solver,

const MXDict& dae, const Dict& opts) {

return integrator(name, solver, dae, 0.0, std::vector<double>{1.0}, opts);

}

Function integrator(const std::string& name, const std::string& solver,

const Function& dae, const Dict& opts) {

return integrator(name, solver, dae, 0.0, std::vector<double>{1.0}, opts);

}

Function integrator(const std::string& name, const std::string& solver,

const SXDict& dae, double t0, const std::vector<double>& tout, const Dict& opts) {

return integrator(name, solver, Integrator::map2oracle("dae", dae), t0, tout, opts);

}

Function integrator(const std::string& name, const std::string& solver,

const MXDict& dae, double t0, const std::vector<double>& tout, const Dict& opts) {

return integrator(name, solver, Integrator::map2oracle("dae", dae), t0, tout, opts);

}

Function integrator(const std::string& name, const std::string& solver,

const Function& dae, double t0, const std::vector<double>& tout, const Dict& opts) {

// Make sure that dae is sound

if (dae.has_free()) {

casadi_error("Cannot create '" + name + "' since " + str(dae.get_free()) + " are free.");

}

Integrator* intg = Integrator::getPlugin(solver).creator(name, dae, t0, tout);

return intg->create_advanced(opts);

}

Function integrator(const std::string& name, const std::string& solver,

const SXDict& dae, double t0, double tf, const Dict& opts) {

return integrator(name, solver, dae, t0, std::vector<double>{tf}, opts);

}

Function integrator(const std::string& name, const std::string& solver,

const MXDict& dae, double t0, double tf, const Dict& opts) {

return integrator(name, solver, dae, t0, std::vector<double>{tf}, opts);

}

Function integrator(const std::string& name, const std::string& solver,

const Function& dae, double t0, double tf, const Dict& opts) {

return integrator(name, solver, dae, t0, std::vector<double>{tf}, opts);

}

std::vector<std::string> integrator_in() {

std::vector<std::string> ret(integrator_n_in());

for (size_t i=0; i<ret.size(); ++i) ret[i]=integrator_in(i);

return ret;

}

std::vector<std::string> integrator_out() {

std::vector<std::string> ret(integrator_n_out());

for (size_t i=0; i<ret.size(); ++i) ret[i]=integrator_out(i);

return ret;

}

std::string integrator_in(casadi_int ind) {

switch (static_cast<IntegratorInput>(ind)) {

case INTEGRATOR_X0: return "x0";

case INTEGRATOR_Z0: return "z0";

case INTEGRATOR_P: return "p";

case INTEGRATOR_U: return "u";

case INTEGRATOR_ADJ_XF: return "adj_xf";

case INTEGRATOR_ADJ_ZF: return "adj_zf";

case INTEGRATOR_ADJ_QF: return "adj_qf";

case INTEGRATOR_NUM_IN: break;

}

return std::string();

}

std::string integrator_out(casadi_int ind) {

switch (static_cast<IntegratorOutput>(ind)) {

case INTEGRATOR_XF: return "xf";

case INTEGRATOR_ZF: return "zf";

case INTEGRATOR_QF: return "qf";

case INTEGRATOR_ADJ_X0: return "adj_x0";

case INTEGRATOR_ADJ_Z0: return "adj_z0";

case INTEGRATOR_ADJ_P: return "adj_p";

case INTEGRATOR_ADJ_U: return "adj_u";

case INTEGRATOR_NUM_OUT: break;

}

return std::string();

}

casadi_int integrator_n_in() {

return INTEGRATOR_NUM_IN;

}

casadi_int integrator_n_out() {

return INTEGRATOR_NUM_OUT;

}

std::vector<std::string> dyn_in() {

return enum_names<DynIn>();

}

std::vector<std::string> dyn_out() {

return enum_names<DynOut>();

}

std::string dyn_in(casadi_int ind) {

return to_string(static_cast<DynIn>(ind));

}

std::string dyn_out(casadi_int ind) {

return to_string(static_cast<DynOut>(ind));

}

casadi_int dyn_n_in() {

return DYN_NUM_IN;

}

casadi_int dyn_n_out() {

return DYN_NUM_OUT;

}

std::vector<std::string> event_in() {

return enum_names<EventIn>();

}

std::vector<std::string> event_out() {

return enum_names<EventOut>();

}

Integrator::Integrator(const std::string& name, const Function& oracle,

double t0, const std::vector<double>& tout)

: OracleFunction(name, oracle), t0_(t0), tout_(tout) {

// Negative number of parameters for consistancy checking

np_ = -1;

// Default options

nfwd_ = 0;

nadj_ = 0;

print_stats_ = false;

max_event_iter_ = 3;

max_events_ = 20;

event_tol_ = 1e-6;

event_acceptable_tol_ = inf;

}

Integrator::~Integrator() {

}

Sparsity Integrator::get_sparsity_in(casadi_int i) {

switch (static_cast<IntegratorInput>(i)) {

case INTEGRATOR_X0: return Sparsity::dense(nx1_, 1 + nfwd_);

case INTEGRATOR_Z0: return Sparsity::dense(nz1_, 1 + nfwd_);

case INTEGRATOR_P: return Sparsity::dense(np1_, 1 + nfwd_);

case INTEGRATOR_U: return Sparsity::dense(nu1_, nt() * (1 + nfwd_));

case INTEGRATOR_ADJ_XF: return Sparsity::dense(nrx1_, nadj_ * (1 + nfwd_) * nt());

case INTEGRATOR_ADJ_ZF: return Sparsity::dense(nrz1_, nadj_ * (1 + nfwd_) * nt());

case INTEGRATOR_ADJ_QF: return Sparsity::dense(nrp1_, nadj_ * (1 + nfwd_) * nt());

case INTEGRATOR_NUM_IN: break;

}

return Sparsity();

}

Sparsity Integrator::get_sparsity_out(casadi_int i) {

switch (static_cast<IntegratorOutput>(i)) {

case INTEGRATOR_XF: return Sparsity::dense(nx1_, nt() * (1 + nfwd_));

case INTEGRATOR_ZF: return Sparsity::dense(nz1_, nt() * (1 + nfwd_));

case INTEGRATOR_QF: return Sparsity::dense(nq1_, nt() * (1 + nfwd_));

case INTEGRATOR_ADJ_X0: return Sparsity::dense(nrx1_, nadj_ * (1 + nfwd_));

case INTEGRATOR_ADJ_Z0: return Sparsity(nrz1_, nadj_ * (1 + nfwd_)); // always zero

case INTEGRATOR_ADJ_P: return Sparsity::dense(nrq1_, nadj_ * (1 + nfwd_));

case INTEGRATOR_ADJ_U: return Sparsity::dense(nuq1_, nadj_ * (1 + nfwd_) * nt());

case INTEGRATOR_NUM_OUT: break;

}

return Sparsity();

}

bool Integrator::grid_in(casadi_int i) {

switch (static_cast<IntegratorInput>(i)) {

case INTEGRATOR_U:

case INTEGRATOR_ADJ_XF:

case INTEGRATOR_ADJ_ZF:

case INTEGRATOR_ADJ_QF:

return true;

default: break;

}

return false;

}

bool Integrator::grid_out(casadi_int i) {

switch (static_cast<IntegratorOutput>(i)) {

case INTEGRATOR_XF:

case INTEGRATOR_ZF:

case INTEGRATOR_QF:

case INTEGRATOR_ADJ_U:

return true;

default: break;

}

return false;

}

casadi_int Integrator::adjmap_out(casadi_int i) {

switch (static_cast<IntegratorInput>(i)) {

case INTEGRATOR_X0: return INTEGRATOR_ADJ_X0;

case INTEGRATOR_Z0: return INTEGRATOR_ADJ_Z0;

case INTEGRATOR_P: return INTEGRATOR_ADJ_P;

case INTEGRATOR_U: return INTEGRATOR_ADJ_U;

case INTEGRATOR_ADJ_XF: return INTEGRATOR_XF;

case INTEGRATOR_ADJ_ZF: return INTEGRATOR_ZF;

case INTEGRATOR_ADJ_QF: return INTEGRATOR_QF;

default: break;

}

return -1;

}

Function Integrator::create_advanced(const Dict& opts) {

return Function::create(this, opts);

}

int Integrator::eval(const double** arg, double** res,

casadi_int* iw, double* w, void* mem) const {

auto m = static_cast<IntegratorMemory*>(mem);

// Read inputs

const double* x0 = arg[INTEGRATOR_X0];

const double* z0 = arg[INTEGRATOR_Z0];

const double* p = arg[INTEGRATOR_P];

const double* u = arg[INTEGRATOR_U];

const double* adj_xf = arg[INTEGRATOR_ADJ_XF];

const double* rz0 = arg[INTEGRATOR_ADJ_ZF];

const double* rp = arg[INTEGRATOR_ADJ_QF];

arg += INTEGRATOR_NUM_IN;

// Read outputs

double* x = res[INTEGRATOR_XF];

double* z = res[INTEGRATOR_ZF];

double* q = res[INTEGRATOR_QF];

double* adj_x = res[INTEGRATOR_ADJ_X0];

double* adj_p = res[INTEGRATOR_ADJ_P];

double* adj_u = res[INTEGRATOR_ADJ_U];

res += INTEGRATOR_NUM_OUT;

// Setup memory object

setup(m, arg, res, iw, w);

// Pass initial state, parameters

set_q(m, 0);

set_x(m, x0);

set_z(m, z0);

set_p(m, p);

// Reset number of events

m->num_events = 0;

// Is this the first call to reset?

bool first_call = true;

// Take time to t0

m->t = t0_;

// Ensure that control is updated at the first iteration

casadi_int k_stop = -1;

// Do we need to reset the solver?

m->reset_solver = false;

// Integrate forward

for (m->k = 0; m->k < nt(); ++m->k) {

// Start of the current interval

m->t_start = m->t;

// Next output time

m->t_next_out = tout_[m->k];

// By default, integrate until the next output time

m->t_next = m->t_next_out;

// Handle changes in control input

if (m->k > k_stop) {

// Pass new controls

set_u(m, u);

// Detect next stopping time

k_stop = next_stop(m->k, u);

m->t_stop = m->t_step = tout_[k_stop];

// Need to reset solver

m->reset_solver = true;

}

// Mark all events as not triggered

std::fill_n(m->event_triggered, ne_, 0);

// Keep integrating until we reach the next output time

do {

// Reset the solver

if (m->reset_solver) {

reset(m, first_call);

m->reset_solver = false;

first_call = false;

}

// Advance solution

if (verbose_) {

casadi_message("Interval " + str(m->k) + ": Integrating forward from "

+ str(m->t) + " to " + str(m->t_next) + ", t_stop = " + str(m->t_stop));

}

if (advance(m)) return 1;

// Trigger all event, if any

if (m->event_index >= 0) {

// Clear list of triggered events

std::fill_n(m->event_triggered, ne_, 0);

// Trigger the specific event and any chained events

while (m->event_index >= 0) {

// Trigger event, get any chained event

if (trigger_event(m, &m->event_index)) return 1;

// Solver needs to be reset

m->reset_solver = true;

}

// Move past event

m->t_start = m->t;

m->t_stop = m->t_step;

m->t_next = m->t_next_out;

}

} while (m->t != m->t_next);

// Get solution

get_x(m, x);

get_z(m, z);

get_q(m, q);

if (x) x += nx_;

if (z) z += nz_;

if (q) q += nq_;

if (u) u += nu_;

}

// Backwards integration, if needed

if (nrx_ > 0) {

// Take adj_xf, rz0, rp past the last grid point

if (adj_xf) adj_xf += nrx_ * nt();

if (rz0) rz0 += nrz_ * nt();

if (rp) rp += nrp_ * nt();

if (adj_u) adj_u += nuq_ * nt();

// Next stop time due to step change in input

k_stop = nt();

// Reset the solver

resetB(m);

// Any adjoint seed so far?

bool any_impulse = false;

// Integrate backward

for (m->k = nt(); m->k-- > 0; ) {

m->t = tout_[m->k];

// Add impulse to backwards integration

if (adj_xf) adj_xf -= nrx_;

if (rz0) rz0 -= nrz_;

if (rp) rp -= nrp_;

if (adj_u) adj_u -= nuq_;

if (u) u -= nu_;

if (!all_zero(adj_xf, nrx_) || !all_zero(rz0, nrz_) || !all_zero(rp, nrp_)) {

if (verbose_) casadi_message("Impulse from adjoint seeds at output time " + str(m->k));

impulseB(m, adj_xf, rz0, rp);

any_impulse = true;

}

// Next output time, or beginning

casadi_int k_next = m->k - 1;

m->t_next = k_next < 0 ? t0_ : tout_[k_next];

// Update integrator stopping time

if (k_next < k_stop) k_stop = next_stopB(m->k, u);

m->t_stop = k_stop < 0 ? t0_ : tout_[k_stop];

// Proceed to the previous time point or t0

if (any_impulse) {

if (verbose_) casadi_message("Integrating backward from output time " + str(m->k)

+ ": t_next = " + str(m->t_next) + ", t_stop = " + str(m->t_stop));

if (m->k > 0) {

retreat(m, u, 0, 0, adj_u);

} else {

retreat(m, u, adj_x, adj_p, adj_u);

}

} else {

if (verbose_) casadi_message("No adjoint seeds from output time " + str(m->k)

+ ": t_next = " + str(m->t_next) + ", t_stop = " + str(m->t_stop));

casadi_clear(adj_u, nuq_);

if (m->k == 0) {

casadi_clear(adj_x, nrx_);

casadi_clear(adj_p, nrq_);

}

}

}

// adj_u should contain the contribution from the grid point, not cumulative

if (adj_u) {

for (m->k = 0; m->k < nt() - 1; ++m->k) {

casadi_axpy(nuq_, -1., adj_u + nuq_, adj_u);

adj_u += nuq_;

}

}

}

// Collect oracle statistics

join_results(m);

// Print integrator statistics

if (print_stats_) print_stats(m);

return 0;

}

int Integrator::advance(IntegratorMemory* m) const {

// Predict next event

if (ne_ > 0 && m->t_next_out != m->t_start) {

if (predict_events(m)) return 1;

}

// Event iterations

m->event_iter = 0;

while (true) {

// Start a new event iteration

m->event_iter++;

// No event triggered

m->event_index = -1;

// Advance solution in time

if (advance_noevent(m)) return 1;

// Update current time

m->t = m->t_next;

m->t_next = m->t_next_out;

// If no events or no interval, done

if (ne_ == 0 || m->t_next_out == m->t_start) break;

// Recalculate m->e and m->edot

if (calc_edot(m)) return 1;

// By default, let integrator continue to the next input step change

m->t_stop = m->t_step;

// Detect events

for (casadi_int i = 0; i < ne_; ++i) {

// Make sure that event was not already triggered

if (m->event_triggered[i] || m->old_e[i] <= 0) continue;

// Check if event was triggered or is projected to be triggered before next output time

if (m->e[i] < 0 || (m->edot[i] < 0 && m->e[i] + (m->t_next_out - m->t) * m->edot[i] < 0)) {

// Projected zero-crossing time

double t_zero = m->t - m->e[i] / m->edot[i];

// If t_zero is too small or m->edot[i] has the wrong sign, fall back to bisection

if (t_zero <= m->t_start || (m->e[i] < 0 && m->edot[i] >= 0)) {

t_zero = 0.5 * (m->t_start + m->t);

}

// Update t_next if earliest event so far

if (t_zero < m->t_next) {

m->event_index = i;

m->t_next = t_zero;

m->t_stop = std::max(m->t, m->t_next);

}

}

}

// If no events, done

if (m->event_index < 0) break;

// Distance to new time step

double t_diff = std::fabs(m->t_next - m->t);

// Check if converged

if (t_diff < event_tol_) {

if (verbose_) casadi_message("Event iteration converged, |dt| == " + str(t_diff));

break;

}

// Maximum number of iterations reached?

if (m->event_iter == max_event_iter_) {

// Throw error?

if (t_diff >= event_acceptable_tol_) {

casadi_error("Maximum number of event iterations reached without convergence");

}

if (verbose_) casadi_message("Max event iterations, |dt| == " + str(t_diff));

break;

}

// More iterations needed

if (verbose_) casadi_message("Event iteration " + str(m->event_iter) + ", |dt| == "

+ str(t_diff));

}

// Successful return

return 0;

}

const Options Integrator::options_

= {{&OracleFunction::options_},

{{"print_stats",

{OT_BOOL,

"Print out statistics after integration"}},

{"nfwd",

{OT_INT,

"Number of forward sensitivities to be calculated [0]"}},

{"nadj",

{OT_INT,

"Number of adjoint sensitivities to be calculated [0]"}},

{"t0",

{OT_DOUBLE,

"[DEPRECATED] Beginning of the time horizon"}},

{"tf",

{OT_DOUBLE,

"[DEPRECATED] End of the time horizon"}},

{"grid",

{OT_DOUBLEVECTOR,

"[DEPRECATED] Time grid"}},

{"augmented_options",

{OT_DICT,

"Options to be passed down to the augmented integrator, if one is constructed"}},

{"transition",

{OT_FUNCTION,

"Function to be called a zero-crossing events"}},

{"max_event_iter",

{OT_INT,

"Maximum number of iterations to zero in on a single event"}},

{"max_events",

{OT_INT,

"Maximum total number of events"}},

{"event_tol",

{OT_DOUBLE,

"Termination tolerance for the event iteration"}},

{"output_t0",

{OT_BOOL,

"[DEPRECATED] Output the state at the initial time"}}

}

};

void Integrator::init(const Dict& opts) {

// Default (temporary) options

double t0 = 0, tf = 1;

bool output_t0 = false;

std::vector<double> grid;

bool uses_legacy_options = false;

// Read options

for (auto&& op : opts) {

if (op.first=="output_t0") {

output_t0 = op.second;

uses_legacy_options = true;

} else if (op.first=="print_stats") {

print_stats_ = op.second;

} else if (op.first=="nfwd") {

nfwd_ = op.second;

} else if (op.first=="nadj") {

nadj_ = op.second;

} else if (op.first=="grid") {

grid = op.second;

uses_legacy_options = true;

} else if (op.first=="augmented_options") {

augmented_options_ = op.second;

} else if (op.first=="transition") {

transition_ = op.second;

} else if (op.first=="max_event_iter") {

max_event_iter_ = op.second;

} else if (op.first=="max_events") {

max_events_ = op.second;

} else if (op.first=="event_tol") {

event_tol_ = op.second;

} else if (op.first=="event_acceptable_tol") {

event_acceptable_tol_ = op.second;

} else if (op.first=="t0") {

t0 = op.second;

uses_legacy_options = true;

} else if (op.first=="tf") {

tf = op.second;

uses_legacy_options = true;

}

}

// Store a copy of the options, for creating augmented integrators

opts_ = opts;

// Construct t0_ and tout_ gbased on legacy options

if (uses_legacy_options) {

static bool first_encounter = true;

if (first_encounter) {

// Deprecation warning

casadi_warning("The options 't0', 'tf', 'grid' and 'output_t0' have been deprecated.\n"

"The same functionality is provided by providing additional input arguments to "

"the 'integrator' function, in particular:\n"

" * Call integrator(..., t0, tf, options) for a single output time, or\n"

" * Call integrator(..., t0, grid, options) for multiple grid points.\n"

"The legacy 'output_t0' option can be emulated by including or excluding 't0' in 'grid'.\n"

"Backwards compatibility is provided in this release only.");

first_encounter = false;

}

// If grid unset, default to [t0, tf]

if (grid.empty()) grid = {t0, tf};

// Construct t0 and tout from grid and output_t0

t0_ = grid.front();

tout_ = grid;

if (!output_t0) tout_.erase(tout_.begin());

}

// Consistency checks: Sensitivities

casadi_assert(nfwd_ >= 0, "Number of forward sensitivities must be non-negative");

casadi_assert(nadj_ >= 0, "Number of adjoint sensitivities must be non-negative");

// Consistency check: Valid oracle

casadi_assert(oracle_.n_in() == DYN_NUM_IN, "DAE has wrong number of inputs");

casadi_assert(oracle_.n_out() == DYN_NUM_OUT, "DAE has wrong number of outputs");

// Consistency checks, input sparsities

for (casadi_int i = 0; i < DYN_NUM_IN; ++i) {

const Sparsity& sp = oracle_.sparsity_in(i);

if (i == DYN_T) {

casadi_assert(sp.is_empty() || sp.is_scalar(), "DAE time variable must be empty or scalar. "

"Got dimension " + str(sp.size()));

} else {

casadi_assert(sp.is_vector(), "DAE inputs must be vectors. "

+ dyn_in(i) + " has dimension " + str(sp.size()) + ".");

}

casadi_assert(sp.is_dense(), "DAE inputs must be dense . "

+ dyn_in(i) + " is sparse.");

}

// Consistency checks, output sparsities

for (casadi_int i = 0; i < DYN_NUM_OUT; ++i) {

const Sparsity& sp = oracle_.sparsity_out(i);

casadi_assert(sp.is_vector(), "DAE outputs must be vectors. "

+ dyn_out(i) + " has dimension " + str(sp.size()));

casadi_assert(sp.is_dense(), "DAE outputs must be dense . "

+ dyn_out(i) + " is sparse.");

}

// Get dimensions (excluding sensitivity equations), forward problem

nx1_ = oracle_.numel_in(DYN_X);

nz1_ = oracle_.numel_in(DYN_Z);

nq1_ = oracle_.numel_out(DYN_QUAD);

np1_ = oracle_.numel_in(DYN_P);

nu1_ = oracle_.numel_in(DYN_U);

ne_ = oracle_.numel_out(DYN_ZERO);

// Event support not implemented

if (ne_ > 0) {

casadi_warning("Event support is experimental");

}

// Consistency checks

casadi_assert(nx1_ > 0, "Ill-posed ODE - no state");

casadi_assert(nx1_ == oracle_.numel_out(DYN_ODE), "Dimension mismatch for 'ode'");

casadi_assert(nz1_ == oracle_.numel_out(DYN_ALG), "Dimension mismatch for 'alg'");

// Backward problem, if any

if (nadj_ > 0) {

// Generate backward DAE

rdae_ = oracle_.reverse(nadj_);

// Consistency checks

casadi_assert(rdae_.n_in() == BDYN_NUM_IN, "Backward DAE has wrong number of inputs");

casadi_assert(rdae_.n_out() == BDYN_NUM_OUT, "Backward DAE has wrong number of outputs");

casadi_assert(rdae_.numel_in(BDYN_X) == nx1_, "Dimension mismatch");

casadi_assert(rdae_.numel_in(BDYN_Z) == nz1_, "Dimension mismatch");

casadi_assert(rdae_.numel_in(BDYN_P) == np1_, "Dimension mismatch");

casadi_assert(rdae_.numel_in(BDYN_U) == nu1_, "Dimension mismatch");

casadi_assert(rdae_.numel_in(BDYN_ADJ_ODE) == nx1_ * nadj_, "Inconsistent dimensions");

casadi_assert(rdae_.numel_in(BDYN_ADJ_ALG) == nz1_ * nadj_, "Inconsistent dimensions");

casadi_assert(rdae_.numel_in(BDYN_ADJ_QUAD) == nq1_ * nadj_, "Inconsistent dimensions");

casadi_assert(rdae_.numel_out(BDYN_ADJ_P) == np1_ * nadj_, "Inconsistent dimensions");

casadi_assert(rdae_.numel_out(BDYN_ADJ_U) == nu1_ * nadj_, "Inconsistent dimensions");

// Dimensions (excluding sensitivity equations), backward problem

nrx1_ = nx1_;

nrz1_ = nz1_;

nrp1_ = nq1_;

nrq1_ = np1_;

nuq1_ = nu1_;

} else {

// No backward problem

nrx1_ = nrz1_ = nrp1_ = nrq1_ = nuq1_ = 0;

}

// Get dimensions (including sensitivity equations)

nx_ = nx1_ * (1 + nfwd_);

nz_ = nz1_ * (1 + nfwd_);

nq_ = nq1_ * (1 + nfwd_);

np_ = np1_ * (1 + nfwd_);

nu_ = nu1_ * (1 + nfwd_);

nrx_ = nrx1_ * nadj_ * (1 + nfwd_);

nrz_ = nrz1_ * nadj_ * (1 + nfwd_);

nrp_ = nrp1_ * nadj_ * (1 + nfwd_);

nrq_ = nrq1_ * nadj_ * (1 + nfwd_);

nuq_ = nuq1_ * nadj_ * (1 + nfwd_);

// Length of tmp1, tmp2 vectors

ntmp_ = nx_ + nz_;

ntmp_ = std::max(ntmp_, nrx_ + nrz_);

ntmp_ = std::max(ntmp_, ne_);

// Call the base class method

OracleFunction::init(opts);

// Instantiate functions, forward and backward problem

set_function(oracle_, "dae");

if (nadj_ > 0) set_function(rdae_, "rdae");

// Event transition function, if any

if (!transition_.is_null()) {

set_function(transition_, "transition");

if (nfwd_ > 0) create_forward("transition", nfwd_);

}

// Event detection requires linearization of the zero-crossing function in the time direction

if (ne_ > 0) {

create_forward("dae", 1);

if (nfwd_ > 0) create_forward("dae", nfwd_);

}

// Create problem functions, forward problem

create_function("daeF", dyn_in(), dae_out());

if (nq_ > 0) create_function("quadF", dyn_in(), quad_out());

if (nfwd_ > 0) {

// one direction to conserve memory, symbolic processing time

create_forward("daeF", 1);

if (nq_ > 0) create_forward("quadF", 1);

}

// Create problem functions, backward problem

if (nadj_ > 0) {

create_function(rdae_, "daeB", bdyn_in(), bdae_out());

if (nrq_ > 0 || nuq_ > 0) create_function(rdae_, "quadB", bdyn_in(), bquad_out());

if (nfwd_ > 0) {

// one direction to conserve memory, symbolic processing time

create_forward("daeB", 1);

if (nrq_ > 0 || nuq_ > 0) create_forward("quadB", 1);

}

}

// Nominal values for states

nom_x_ = oracle_.nominal_in(DYN_X);

nom_z_ = oracle_.nominal_in(DYN_Z);

// Get the sparsities of the forward and reverse DAE

sp_jac_dae_ = sp_jac_dae();

casadi_assert(!sp_jac_dae_.is_singular(),

"Jacobian of the forward problem is structurally rank-deficient. "

"sprank(J)=" + str(sprank(sp_jac_dae_)) + "<" + str(nx_+nz_));

if (nadj_ > 0) {

sp_jac_rdae_ = sp_jac_rdae();

casadi_assert(!sp_jac_rdae_.is_singular(),

"Jacobian of the backward problem is structurally rank-deficient. "

"sprank(J)=" + str(sprank(sp_jac_rdae_)) + "<" + str(nrx_+nrz_));

}

alloc_w(nq_, true); // q

alloc_w(nx_, true); // x

alloc_w(nz_, true); // z

alloc_w(np_, true); // p

alloc_w(nu_, true); // u

alloc_w(ne_, true); // e

alloc_w(ne_, true); // edot

alloc_w(ne_, true); // old_e

alloc_w(nx_, true); // xdot

alloc_w(nz_, true); // zdot

alloc_iw(ne_, true); // event_triggered

alloc_w(nrx_ + nrz_, true); // adj_x, adj_z

alloc_w(nrq_, true); // adj_p

alloc_w(nrp_, true); // adj_q

alloc_w(2 * ntmp_, true); // tmp1, tmp2

alloc_w(nx_ + nz_); // Sparsity::sp_solve

alloc_w(nrx_ + nrz_); // Sparsity::sp_solve

}

void Integrator::set_work(void* mem, const double**& arg, double**& res,

casadi_int*& iw, double*& w) const {

auto m = static_cast<IntegratorMemory*>(mem);

// Set work in base classes

OracleFunction::set_work(mem, arg, res, iw, w);

// Work vectors

m->q = w; w += nq_; // Note: q, x, z consecutive in memory

m->x = w; w += nx_;

m->z = w; w += nz_;

m->p = w; w += np_;

m->u = w; w += nu_;

m->e = w; w += ne_;

m->edot = w; w += ne_;

m->old_e = w; w += ne_;

m->xdot = w; w += nx_;

m->zdot = w; w += nz_;

m->event_triggered = iw; iw += ne_;

m->adj_x = w; w += nrx_; // doubles as adj_xz

m->adj_z = w; w += nrz_;

m->adj_p = w; w += nrq_;

m->adj_q = w; w += nrp_;

m->tmp1 = w; w += ntmp_;

m->tmp2 = w; w += ntmp_;

}

int Integrator::init_mem(void* mem) const {

if (OracleFunction::init_mem(mem)) return 1;

//auto m = static_cast<IntegratorMemory*>(mem);

return 0;

}

Function Integrator::augmented_dae() const {

// If no sensitivities, augmented oracle is the oracle itself

if (nfwd_ == 0) return oracle_;

// Name of augmented DAE

std::string aug_name = "fsens" + str(nfwd_) + "_" + oracle_.name();

// Use function in cache, if available

Function ret;

// if (incache(aug_name, ret)) return ret; // caching disabled while implementing #3047

// Create new augmented oracle

try {

if (oracle_.is_a("SXFunction")) {

ret = get_forward_dae<SX>(aug_name);

} else {

ret = get_forward_dae<MX>(aug_name);

}

} catch (std::exception& e) {

casadi_error("Failed to generate augmented DAE for " + name_ + ":\n" + e.what());

}

// Save to Function cache and return

// tocache(ret); // caching disabled while implementing #3047

return ret;

}

template<typename MatType>

Function Integrator::get_forward_dae(const std::string& name) const {

if (verbose_) casadi_message(name_ + "::get_forward_dae");

// Events not implemented

casadi_assert(ne_ == 0, "Event support not implemented for Integrator::augmented_dae");

// Get input and output expressions

std::vector<MatType> arg = MatType::get_input(oracle_);

std::vector<MatType> res = oracle_(arg);

// Symbolic expression for augmented DAE

std::vector<std::vector<MatType>> aug_in(DYN_NUM_IN);

for (casadi_int i = 0; i < DYN_NUM_IN; ++i) aug_in[i].push_back(arg.at(i));

std::vector<std::vector<MatType>> aug_out(DYN_NUM_OUT);

for (casadi_int i = 0; i < DYN_NUM_OUT; ++i) aug_out[i].push_back(res.at(i));

// Zero of time dimension

MatType zero_t = MatType::zeros(oracle_.sparsity_in(DYN_T));

// Augment aug_in with forward sensitivity seeds

std::vector<std::vector<MatType>> seed(nfwd_, std::vector<MatType>(DYN_NUM_IN));

for (casadi_int d = 0; d < nfwd_; ++d) {

// Create expressions for augmented states

std::string pref = "aug" + str(d) + "_";

for (casadi_int i = 0; i < DYN_NUM_IN; ++i) {

if (i == DYN_T) {

seed[d][i] = zero_t;

} else {

seed[d][i] = MatType::sym(pref + dyn_in(i), oracle_.sparsity_in(i));

}

}

// Save to augmented function inputs

for (casadi_int i = 0; i < DYN_NUM_IN; ++i) {

if (i != DYN_T) aug_in[i].push_back(seed[d][i]);

}

}

// Calculate directional derivatives

std::vector<std::vector<MatType>> sens;

bool always_inline = oracle_.is_a("SXFunction") || oracle_.is_a("MXFunction");

oracle_->call_forward(arg, res, seed, sens, always_inline, false);

// Augment aug_out with forward sensitivity equations

casadi_assert_dev(sens.size() == nfwd_);

for (casadi_int d = 0; d < nfwd_; ++d) {

casadi_assert_dev(sens[d].size() == DYN_NUM_OUT);

for (casadi_int i = 0; i < DYN_NUM_OUT; ++i) {

aug_out[i].push_back(project(sens[d][i], oracle_.sparsity_out(i)));

}

}

// Concatenate arrays

for (casadi_int i = 0; i < DYN_NUM_IN; ++i) arg.at(i) = vertcat(aug_in[i]);

for (casadi_int i = 0; i < DYN_NUM_OUT; ++i) res.at(i) = vertcat(aug_out[i]);

// Convert to oracle function and return

return Function(name, arg, res, dyn_in(), dyn_out());

}