openpilot_comma/pyextra/acados_template/acados_ocp_solver.py

# -*- coding: future_fstrings -*-
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
#
# This file is part of acados.
#
# The 2-Clause BSD License
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.;
#

import sys
import os
import json
import numpy as np
from datetime import datetime
import ctypes
from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref

from copy import deepcopy

from .generate_c_code_explicit_ode import generate_c_code_explicit_ode
from .generate_c_code_implicit_ode import generate_c_code_implicit_ode
from .generate_c_code_gnsf import generate_c_code_gnsf
from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics
from .generate_c_code_constraint import generate_c_code_constraint
from .generate_c_code_nls_cost import generate_c_code_nls_cost
from .generate_c_code_external_cost import generate_c_code_external_cost
from .acados_ocp import AcadosOcp
from .acados_model import acados_model_strip_casadi_symbolics
from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
     format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
     set_up_imported_gnsf_model, get_acados_path, get_ocp_nlp_layout, get_python_interface_path


def make_ocp_dims_consistent(acados_ocp):
    dims = acados_ocp.dims
    cost = acados_ocp.cost
    constraints = acados_ocp.constraints
    model = acados_ocp.model
    opts = acados_ocp.solver_options

    # nx
    if is_column(model.x):
        dims.nx = casadi_length(model.x)
    else:
        raise Exception('model.x should be column vector!')

    # nu
    if is_empty(model.u):
        dims.nu = 0
    else:
        dims.nu = casadi_length(model.u)

    # nz
    if is_empty(model.z):
        dims.nz = 0
    else:
        dims.nz = casadi_length(model.z)

    # np
    if is_empty(model.p):
        dims.np = 0
    else:
        dims.np = casadi_length(model.p)
    if acados_ocp.parameter_values.shape[0] != dims.np:
        raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \
            f'\nGot np = {dims.np}, acados_ocp.parameter_values.shape = {acados_ocp.parameter_values.shape[0]}\n')

    # cost
    # initial stage - if not set, copy fields from path constraints
    if cost.cost_type_0 is None:
        cost.cost_type_0 = cost.cost_type
        cost.W_0 = cost.W
        cost.Vx_0 = cost.Vx
        cost.Vu_0 = cost.Vu
        cost.Vz_0 = cost.Vz
        cost.yref_0 = cost.yref
        cost.cost_ext_fun_type_0 = cost.cost_ext_fun_type
        model.cost_y_expr_0 = model.cost_y_expr
        model.cost_expr_ext_cost_0 = model.cost_expr_ext_cost
        model.cost_expr_ext_cost_custom_hess_0 = model.cost_expr_ext_cost_custom_hess

    if cost.cost_type_0 == 'LINEAR_LS':
        ny_0 = cost.W_0.shape[0]
        if cost.Vx_0.shape[0] != ny_0 or cost.Vu_0.shape[0] != ny_0:
            raise Exception('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0.' + \
                            f'\nGot W_0[{cost.W_0.shape}], Vx_0[{cost.Vx_0.shape}], Vu_0[{cost.Vu_0.shape}]\n')
        if dims.nz != 0 and cost.Vz_0.shape[0] != ny_0:
            raise Exception('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0, Vz_0.' + \
                            f'\nGot W_0[{cost.W_0.shape}], Vx_0[{cost.Vx_0.shape}], Vu_0[{cost.Vu_0.shape}], Vz_0[{cost.Vz_0.shape}]\n')
        if cost.Vx_0.shape[1] != dims.nx and ny_0 != 0:
            raise Exception('inconsistent dimension: Vx_0 should have nx columns.')
        if cost.Vu_0.shape[1] != dims.nu and ny_0 != 0:
            raise Exception('inconsistent dimension: Vu_0 should have nu columns.')
        if cost.yref_0.shape[0] != ny_0:
            raise Exception('inconsistent dimension: regarding W_0, yref_0.' + \
                            f'\nGot W_0[{cost.W_0.shape}], yref_0[{cost.yref_0.shape}]\n')
        dims.ny_0 = ny_0

    elif cost.cost_type_0 == 'NONLINEAR_LS':
        ny_0 = cost.W_0.shape[0]
        if is_empty(model.cost_y_expr_0) and ny_0 != 0:
            raise Exception('inconsistent dimension ny_0: regarding W_0, cost_y_expr.')
        elif casadi_length(model.cost_y_expr_0) != ny_0:
            raise Exception('inconsistent dimension ny_0: regarding W_0, cost_y_expr.')
        if cost.yref_0.shape[0] != ny_0:
            raise Exception('inconsistent dimension: regarding W_0, yref_0.' + \
                            f'\nGot W_0[{cost.W.shape}], yref_0[{cost.yref_0.shape}]\n')
        dims.ny_0 = ny_0

    # path
    if cost.cost_type == 'LINEAR_LS':
        ny = cost.W.shape[0]
        if cost.Vx.shape[0] != ny or cost.Vu.shape[0] != ny:
            raise Exception('inconsistent dimension ny, regarding W, Vx, Vu.' + \
                            f'\nGot W[{cost.W.shape}], Vx[{cost.Vx.shape}], Vu[{cost.Vu.shape}]\n')
        if dims.nz != 0 and cost.Vz.shape[0] != ny:
            raise Exception('inconsistent dimension ny, regarding W, Vx, Vu, Vz.' + \
                            f'\nGot W[{cost.W.shape}], Vx[{cost.Vx.shape}], Vu[{cost.Vu.shape}], Vz[{cost.Vz.shape}]\n')
        if cost.Vx.shape[1] != dims.nx and ny != 0:
            raise Exception('inconsistent dimension: Vx should have nx columns.')
        if cost.Vu.shape[1] != dims.nu and ny != 0:
            raise Exception('inconsistent dimension: Vu should have nu columns.')
        if cost.yref.shape[0] != ny:
            raise Exception('inconsistent dimension: regarding W, yref.' + \
                            f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n')
        dims.ny = ny

    elif cost.cost_type == 'NONLINEAR_LS':
        ny = cost.W.shape[0]
        if is_empty(model.cost_y_expr) and ny != 0:
            raise Exception('inconsistent dimension ny: regarding W, cost_y_expr.')
        elif casadi_length(model.cost_y_expr) != ny:
            raise Exception('inconsistent dimension ny: regarding W, cost_y_expr.')
        if cost.yref.shape[0] != ny:
            raise Exception('inconsistent dimension: regarding W, yref.' + \
                            f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n')
        dims.ny = ny

    # terminal
    if cost.cost_type_e == 'LINEAR_LS':
        ny_e = cost.W_e.shape[0]
        if cost.Vx_e.shape[0] != ny_e:
            raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \
                f'\nGot W_e[{cost.W_e.shape}], Vx_e[{cost.Vx_e.shape}]')
        if cost.Vx_e.shape[1] != dims.nx and ny_e != 0:
            raise Exception('inconsistent dimension: Vx_e should have nx columns.')
        if cost.yref_e.shape[0] != ny_e:
            raise Exception('inconsistent dimension: regarding W_e, yref_e.')
        dims.ny_e = ny_e

    elif cost.cost_type_e == 'NONLINEAR_LS':
        ny_e = cost.W_e.shape[0]
        if is_empty(model.cost_y_expr_e) and ny_e != 0:
            raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.')
        elif casadi_length(model.cost_y_expr_e) != ny_e:
            raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.')
        if cost.yref_e.shape[0] != ny_e:
            raise Exception('inconsistent dimension: regarding W_e, yref_e.')
        dims.ny_e = ny_e


    ## constraints
    # initial
    if (constraints.lbx_0 == [] and constraints.ubx_0 == []):
        dims.nbx_0 = 0
    else:
        this_shape = constraints.lbx_0.shape
        other_shape = constraints.ubx_0.shape
        if not this_shape == other_shape:
            raise Exception('lbx_0, ubx_0 have different shapes!')
        if not is_column(constraints.lbx_0):
            raise Exception('lbx_0, ubx_0 must be column vectors!')
        dims.nbx_0 = constraints.lbx_0.size

    if all(constraints.lbx_0 == constraints.ubx_0) and dims.nbx_0 == dims.nx \
        and dims.nbxe_0 is None \
        and (constraints.idxbxe_0.shape == constraints.idxbx_0.shape)\
            and all(constraints.idxbxe_0 == constraints.idxbx_0):
        # case: x0 was set: nbx0 are all equlities.
        dims.nbxe_0 = dims.nbx_0
    elif dims.nbxe_0 is None:
        # case: x0 was not set -> dont assume nbx0 to be equality constraints.
        dims.nbxe_0 = 0

    # path
    nbx = constraints.idxbx.shape[0]
    if constraints.ubx.shape[0] != nbx or constraints.lbx.shape[0] != nbx:
        raise Exception('inconsistent dimension nbx, regarding idxbx, ubx, lbx.')
    else:
        dims.nbx = nbx

    nbu = constraints.idxbu.shape[0]
    if constraints.ubu.shape[0] != nbu or constraints.lbu.shape[0] != nbu:
        raise Exception('inconsistent dimension nbu, regarding idxbu, ubu, lbu.')
    else:
        dims.nbu = nbu

    ng = constraints.lg.shape[0]
    if constraints.ug.shape[0] != ng or constraints.C.shape[0] != ng \
       or constraints.D.shape[0] != ng:
        raise Exception('inconsistent dimension ng, regarding lg, ug, C, D.')
    else:
        dims.ng = ng

    if not is_empty(model.con_h_expr):
        nh = casadi_length(model.con_h_expr)
    else:
        nh = 0

    if constraints.uh.shape[0] != nh or constraints.lh.shape[0] != nh:
        raise Exception('inconsistent dimension nh, regarding lh, uh, con_h_expr.')
    else:
        dims.nh = nh

    if is_empty(model.con_phi_expr):
        dims.nphi = 0
        dims.nr = 0
    else:
        dims.nphi = casadi_length(model.con_phi_expr)
        if is_empty(model.con_r_expr):
            raise Exception('convex over nonlinear constraints: con_r_expr but con_phi_expr is nonempty')
        else:
            dims.nr = casadi_length(model.con_r_expr)

    # terminal
    nbx_e = constraints.idxbx_e.shape[0]
    if constraints.ubx_e.shape[0] != nbx_e or constraints.lbx_e.shape[0] != nbx_e:
        raise Exception('inconsistent dimension nbx_e, regarding idxbx_e, ubx_e, lbx_e.')
    else:
        dims.nbx_e = nbx_e

    ng_e = constraints.lg_e.shape[0]
    if constraints.ug_e.shape[0] != ng_e or constraints.C_e.shape[0] != ng_e:
        raise Exception('inconsistent dimension ng_e, regarding_e lg_e, ug_e, C_e.')
    else:
        dims.ng_e = ng_e

    if not is_empty(model.con_h_expr_e):
        nh_e = casadi_length(model.con_h_expr_e)
    else:
        nh_e = 0

    if constraints.uh_e.shape[0] != nh_e or constraints.lh_e.shape[0] != nh_e:
        raise Exception('inconsistent dimension nh_e, regarding lh_e, uh_e, con_h_expr_e.')
    else:
        dims.nh_e = nh_e

    if is_empty(model.con_phi_expr_e):
        dims.nphi_e = 0
        dims.nr_e = 0
    else:
        dims.nphi_e = casadi_length(model.con_phi_expr_e)
        if is_empty(model.con_r_expr_e):
            raise Exception('convex over nonlinear constraints: con_r_expr_e but con_phi_expr_e is nonempty')
        else:
            dims.nr_e = casadi_length(model.con_r_expr_e)

    # Slack dimensions
    nsbx = constraints.idxsbx.shape[0]
    if is_empty(constraints.lsbx):
        constraints.lsbx = np.zeros((nsbx,))
    elif constraints.lsbx.shape[0] != nsbx:
        raise Exception('inconsistent dimension nsbx, regarding idxsbx, lsbx.')
    if is_empty(constraints.usbx):
        constraints.usbx = np.zeros((nsbx,))
    elif constraints.usbx.shape[0] != nsbx:
        raise Exception('inconsistent dimension nsbx, regarding idxsbx, usbx.')
    dims.nsbx = nsbx

    nsbu = constraints.idxsbu.shape[0]
    if is_empty(constraints.lsbu):
        constraints.lsbu = np.zeros((nsbu,))
    elif constraints.lsbu.shape[0] != nsbu:
        raise Exception('inconsistent dimension nsbu, regarding idxsbu, lsbu.')
    if is_empty(constraints.usbu):
        constraints.usbu = np.zeros((nsbu,))
    elif constraints.usbu.shape[0] != nsbu:
        raise Exception('inconsistent dimension nsbu, regarding idxsbu, usbu.')
    dims.nsbu = nsbu

    nsh = constraints.idxsh.shape[0]
    if is_empty(constraints.lsh):
        constraints.lsh = np.zeros((nsh,))
    elif constraints.lsh.shape[0] != nsh:
        raise Exception('inconsistent dimension nsh, regarding idxsh, lsh.')
    if is_empty(constraints.ush):
        constraints.ush = np.zeros((nsh,))
    elif constraints.ush.shape[0] != nsh:
        raise Exception('inconsistent dimension nsh, regarding idxsh, ush.')
    dims.nsh = nsh

    nsphi = constraints.idxsphi.shape[0]
    if is_empty(constraints.lsphi):
        constraints.lsphi = np.zeros((nsphi,))
    elif constraints.lsphi.shape[0] != nsphi:
        raise Exception('inconsistent dimension nsphi, regarding idxsphi, lsphi.')
    if is_empty(constraints.usphi):
        constraints.usphi = np.zeros((nsphi,))
    elif constraints.usphi.shape[0] != nsphi:
        raise Exception('inconsistent dimension nsphi, regarding idxsphi, usphi.')
    dims.nsphi = nsphi

    nsg = constraints.idxsg.shape[0]
    if is_empty(constraints.lsg):
        constraints.lsg = np.zeros((nsg,))
    elif constraints.lsg.shape[0] != nsg:
        raise Exception('inconsistent dimension nsg, regarding idxsg, lsg.')
    if is_empty(constraints.usg):
        constraints.usg = np.zeros((nsg,))
    elif constraints.usg.shape[0] != nsg:
        raise Exception('inconsistent dimension nsg, regarding idxsg, usg.')
    dims.nsg = nsg

    ns = nsbx + nsbu + nsh + nsg + nsphi
    wrong_field = ""
    if cost.Zl.shape[0] != ns:
        wrong_field = "Zl"
        dim = cost.Zl.shape[0]
    elif cost.Zu.shape[0] != ns:
        wrong_field = "Zu"
        dim = cost.Zu.shape[0]
    elif cost.zl.shape[0] != ns:
        wrong_field = "zl"
        dim = cost.zl.shape[0]
    elif cost.zu.shape[0] != ns:
        wrong_field = "zu"
        dim = cost.zu.shape[0]

    if wrong_field != "":
        raise Exception(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t'\
            + f'Detected ns = {ns} = nsbx + nsbu + nsg + nsh + nsphi.\n\t'\
            + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}')

    dims.ns = ns

    nsbx_e = constraints.idxsbx_e.shape[0]
    if is_empty(constraints.lsbx_e):
        constraints.lsbx_e = np.zeros((nsbx_e,))
    elif constraints.lsbx_e.shape[0] != nsbx_e:
        raise Exception('inconsistent dimension nsbx_e, regarding idxsbx_e, lsbx_e.')
    if is_empty(constraints.usbx_e):
        constraints.usbx_e = np.zeros((nsbx_e,))
    elif constraints.usbx_e.shape[0] != nsbx_e:
        raise Exception('inconsistent dimension nsbx_e, regarding idxsbx_e, usbx_e.')
    dims.nsbx_e = nsbx_e

    nsh_e = constraints.idxsh_e.shape[0]
    if is_empty(constraints.lsh_e):
        constraints.lsh_e = np.zeros((nsh_e,))
    elif constraints.lsh_e.shape[0] != nsh_e:
        raise Exception('inconsistent dimension nsh_e, regarding idxsh_e, lsh_e.')
    if is_empty(constraints.ush_e):
        constraints.ush_e = np.zeros((nsh_e,))
    elif constraints.ush_e.shape[0] != nsh_e:
        raise Exception('inconsistent dimension nsh_e, regarding idxsh_e, ush_e.')
    dims.nsh_e = nsh_e

    nsg_e = constraints.idxsg_e.shape[0]
    if is_empty(constraints.lsg_e):
        constraints.lsg_e = np.zeros((nsg_e,))
    elif constraints.lsg_e.shape[0] != nsg_e:
        raise Exception('inconsistent dimension nsg_e, regarding idxsg_e, lsg_e.')
    if is_empty(constraints.usg_e):
        constraints.usg_e = np.zeros((nsg_e,))
    elif constraints.usg_e.shape[0] != nsg_e:
        raise Exception('inconsistent dimension nsg_e, regarding idxsg_e, usg_e.')
    dims.nsg_e = nsg_e

    nsphi_e = constraints.idxsphi_e.shape[0]
    if is_empty(constraints.lsphi_e):
        constraints.lsphi_e = np.zeros((nsphi_e,))
    elif constraints.lsphi_e.shape[0] != nsphi_e:
        raise Exception('inconsistent dimension nsphi_e, regarding idxsphi_e, lsphi_e.')
    if is_empty(constraints.usphi_e):
        constraints.usphi_e = np.zeros((nsphi_e,))
    elif constraints.usphi_e.shape[0] != nsphi_e:
        raise Exception('inconsistent dimension nsphi_e, regarding idxsphi_e, usphi_e.')
    dims.nsphi_e = nsphi_e

    # terminal
    ns_e = nsbx_e + nsh_e + nsg_e + nsphi_e
    wrong_field = ""
    if cost.Zl_e.shape[0] != ns_e:
        wrong_field = "Zl_e"
        dim = cost.Zl_e.shape[0]
    elif cost.Zu_e.shape[0] != ns_e:
        wrong_field = "Zu_e"
        dim = cost.Zu_e.shape[0]
    elif cost.zl_e.shape[0] != ns_e:
        wrong_field = "zl_e"
        dim = cost.zl_e.shape[0]
    elif cost.zu_e.shape[0] != ns_e:
        wrong_field = "zu_e"
        dim = cost.zu_e.shape[0]

    if wrong_field != "":
        raise Exception(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t'\
            + f'Detected ns_e = {ns_e} = nsbx_e + nsg_e + nsh_e + nsphi_e.\n\t'\
            + f'With nsbx_e = {nsbx_e}, nsg_e = {nsg_e}, nsh_e = {nsh_e}, nsphi_e = {nsphi_e}')

    dims.ns_e = ns_e

    # discretization
    if is_empty(opts.time_steps) and is_empty(opts.shooting_nodes):
        # uniform discretization
        opts.time_steps = opts.tf / dims.N * np.ones((dims.N,))

    elif not is_empty(opts.shooting_nodes):
        if np.shape(opts.shooting_nodes)[0] != dims.N+1:
            raise Exception('inconsistent dimension N, regarding shooting_nodes.')

        # time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
        # # identify constant time-steps: due to numerical reasons the content of time_steps might vary a bit
        # delta_time_steps = time_steps[1:] - time_steps[0:-1]
        # avg_time_steps = np.average(time_steps)
        # # criterion for constant time-step detection: the min/max difference in values normalized by the average
        # check_const_time_step = np.max(delta_time_steps)-np.min(delta_time_steps) / avg_time_steps
        # # if the criterion is small, we have a constant time-step
        # if check_const_time_step < 1e-9:
        #     time_steps[:] = avg_time_steps  # if we have a constant time-step: apply the average time-step
        time_steps = np.zeros((dims.N,))
        for i in range(dims.N):
            time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i] # TODO use commented code above

        opts.time_steps = time_steps

    elif (not is_empty(opts.time_steps)) and (not is_empty(opts.shooting_nodes)):
        Exception('Please provide either time_steps or shooting_nodes for nonuniform discretization')

    tf = np.sum(opts.time_steps)
    if (tf - opts.tf) / tf > 1e-15:
        raise Exception(f'Inconsistent discretization: {opts.tf}'\
            f' = tf != sum(opts.time_steps) = {tf}.')

    # num_steps
    if isinstance(opts.sim_method_num_steps, np.ndarray) and opts.sim_method_num_steps.size == 1:
        opts.sim_method_num_steps = opts.sim_method_num_steps.item()

    if isinstance(opts.sim_method_num_steps, (int, float)) and opts.sim_method_num_steps % 1 == 0:
        opts.sim_method_num_steps = opts.sim_method_num_steps * np.ones((dims.N,), dtype=np.int64)
    elif isinstance(opts.sim_method_num_steps, np.ndarray) and opts.sim_method_num_steps.size == dims.N \
           and np.all(np.equal(np.mod(opts.sim_method_num_steps, 1), 0)):
        opts.sim_method_num_steps = np.reshape(opts.sim_method_num_steps, (dims.N,)).astype(np.int64)
    else:
        raise Exception("Wrong value for sim_method_num_steps. Should be either int or array of ints of shape (N,).")

    # num_stages
    if isinstance(opts.sim_method_num_stages, np.ndarray) and opts.sim_method_num_stages.size == 1:
        opts.sim_method_num_stages = opts.sim_method_num_stages.item()

    if isinstance(opts.sim_method_num_stages, (int, float)) and opts.sim_method_num_stages % 1 == 0:
        opts.sim_method_num_stages = opts.sim_method_num_stages * np.ones((dims.N,), dtype=np.int64)
    elif isinstance(opts.sim_method_num_stages, np.ndarray) and opts.sim_method_num_stages.size == dims.N \
           and np.all(np.equal(np.mod(opts.sim_method_num_stages, 1), 0)):
        opts.sim_method_num_stages = np.reshape(opts.sim_method_num_stages, (dims.N,)).astype(np.int64)
    else:
        raise Exception("Wrong value for sim_method_num_stages. Should be either int or array of ints of shape (N,).")

    # jac_reuse
    if isinstance(opts.sim_method_jac_reuse, np.ndarray) and opts.sim_method_jac_reuse.size == 1:
        opts.sim_method_jac_reuse = opts.sim_method_jac_reuse.item()

    if isinstance(opts.sim_method_jac_reuse, (int, float)) and opts.sim_method_jac_reuse % 1 == 0:
        opts.sim_method_jac_reuse = opts.sim_method_jac_reuse * np.ones((dims.N,), dtype=np.int64)
    elif isinstance(opts.sim_method_jac_reuse, np.ndarray) and opts.sim_method_jac_reuse.size == dims.N \
           and np.all(np.equal(np.mod(opts.sim_method_jac_reuse, 1), 0)):
        opts.sim_method_jac_reuse = np.reshape(opts.sim_method_jac_reuse, (dims.N,)).astype(np.int64)
    else:
        raise Exception("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).")


def get_simulink_default_opts():
    python_interface_path = get_python_interface_path()
    abs_path = os.path.join(python_interface_path, 'simulink_default_opts.json')
    with open(abs_path , 'r') as f:
        simulink_default_opts = json.load(f)
    return simulink_default_opts


def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_nlp.json'):
    # Load acados_ocp_nlp structure description
    ocp_layout = get_ocp_nlp_layout()

    # Copy input ocp object dictionary
    ocp_nlp_dict = dict(deepcopy(acados_ocp).__dict__)
    # TODO: maybe make one function with formatting

    for acados_struct, v in ocp_layout.items():
        # skip non dict attributes
        if not isinstance(v, dict):
            continue
        #  setattr(ocp_nlp, acados_struct, dict(getattr(acados_ocp, acados_struct).__dict__))
        # Copy ocp object attributes dictionaries
        ocp_nlp_dict[acados_struct]=dict(getattr(acados_ocp, acados_struct).__dict__)

    ocp_nlp_dict = format_class_dict(ocp_nlp_dict)

    # strip symbolics
    ocp_nlp_dict['model'] = acados_model_strip_casadi_symbolics(ocp_nlp_dict['model'])

    # strip shooting_nodes
    ocp_nlp_dict['solver_options'].pop('shooting_nodes', None)

    dims_dict = acados_class2dict(acados_ocp.dims)

    ocp_check_against_layout(ocp_nlp_dict, dims_dict)

    # add simulink options
    ocp_nlp_dict['simulink_opts'] = simulink_opts

    with open(json_file, 'w') as f:
        json.dump(ocp_nlp_dict, f, default=np_array_to_list, indent=4, sort_keys=True)



def ocp_formulation_json_load(json_file='acados_ocp_nlp.json'):
    # Load acados_ocp_nlp structure description
    ocp_layout = get_ocp_nlp_layout()

    with open(json_file, 'r') as f:
        ocp_nlp_json = json.load(f)

    ocp_nlp_dict = json2dict(ocp_nlp_json, ocp_nlp_json['dims'])

    # Instantiate AcadosOcp object
    acados_ocp = AcadosOcp()

    # load class dict
    acados_ocp.__dict__ = ocp_nlp_dict

    # load class attributes dict, dims, constraints, etc
    for acados_struct, v in ocp_layout.items():
        # skip non dict attributes
        if not isinstance(v, dict):
            continue
        acados_attribute = getattr(acados_ocp, acados_struct)
        acados_attribute.__dict__ = ocp_nlp_dict[acados_struct]
        setattr(acados_ocp, acados_struct, acados_attribute)

    return acados_ocp


def ocp_generate_external_functions(acados_ocp, model):

    model = make_model_consistent(model)

    if acados_ocp.solver_options.hessian_approx == 'EXACT':
        opts = dict(generate_hess=1)
    else:
        opts = dict(generate_hess=0)
    code_export_dir = acados_ocp.code_export_directory
    opts['code_export_directory'] = code_export_dir

    if acados_ocp.model.dyn_ext_fun_type != 'casadi':
        raise Exception("ocp_generate_external_functions: dyn_ext_fun_type only supports 'casadi' for now.\
            Extending the Python interface with generic function support is welcome.")

    if acados_ocp.solver_options.integrator_type == 'ERK':
        # explicit model -- generate C code
        generate_c_code_explicit_ode(model, opts)
    elif acados_ocp.solver_options.integrator_type == 'IRK':
        # implicit model -- generate C code
        generate_c_code_implicit_ode(model, opts)
    elif acados_ocp.solver_options.integrator_type == 'LIFTED_IRK':
        generate_c_code_implicit_ode(model, opts)
    elif acados_ocp.solver_options.integrator_type == 'GNSF':
        generate_c_code_gnsf(model, opts)
    elif acados_ocp.solver_options.integrator_type == 'DISCRETE':
        generate_c_code_discrete_dynamics(model, opts)
    else:
        raise Exception("ocp_generate_external_functions: unknown integrator type.")

    if acados_ocp.dims.nphi > 0 or acados_ocp.dims.nh > 0:
        generate_c_code_constraint(model, model.name, False, opts)

    if acados_ocp.dims.nphi_e > 0 or acados_ocp.dims.nh_e > 0:
        generate_c_code_constraint(model, model.name, True, opts)

    # dummy matrices
    if not acados_ocp.cost.cost_type_0 == 'LINEAR_LS':
        acados_ocp.cost.Vx_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nx))
        acados_ocp.cost.Vu_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nu))
    if not acados_ocp.cost.cost_type == 'LINEAR_LS':
        acados_ocp.cost.Vx = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nx))
        acados_ocp.cost.Vu = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nu))
    if not acados_ocp.cost.cost_type_e == 'LINEAR_LS':
        acados_ocp.cost.Vx_e = np.zeros((acados_ocp.dims.ny_e, acados_ocp.dims.nx))

    if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS':
        generate_c_code_nls_cost(model, model.name, 'initial', opts)
    elif acados_ocp.cost.cost_type_0 == 'EXTERNAL':
        generate_c_code_external_cost(model, 'initial', opts)

    if acados_ocp.cost.cost_type == 'NONLINEAR_LS':
        generate_c_code_nls_cost(model, model.name, 'path', opts)
    elif acados_ocp.cost.cost_type == 'EXTERNAL':
        generate_c_code_external_cost(model, 'path', opts)

    if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS':
        generate_c_code_nls_cost(model, model.name, 'terminal', opts)
    elif acados_ocp.cost.cost_type_e == 'EXTERNAL':
        generate_c_code_external_cost(model, 'terminal', opts)


def ocp_render_templates(acados_ocp, json_file):

    name = acados_ocp.model.name

    # setting up loader and environment
    json_path = os.path.join(os.getcwd(), json_file)

    if not os.path.exists(json_path):
        raise Exception('{} not found!'.format(json_path))

    code_export_dir = acados_ocp.code_export_directory
    template_dir = code_export_dir

    ## Render templates
    in_file = 'main.in.c'
    out_file = f'main_{name}.c'
    render_template(in_file, out_file, template_dir, json_path)

    in_file = 'acados_solver.in.c'
    out_file = f'acados_solver_{name}.c'
    render_template(in_file, out_file, template_dir, json_path)

    in_file = 'acados_solver.in.h'
    out_file = f'acados_solver_{name}.h'
    render_template(in_file, out_file, template_dir, json_path)

    in_file = 'acados_solver.in.pxd'
    out_file = f'acados_solver.pxd'
    render_template(in_file, out_file, template_dir, json_path)

    in_file = 'Makefile.in'
    out_file = 'Makefile'
    render_template(in_file, out_file, template_dir, json_path)

    in_file = 'acados_solver_sfun.in.c'
    out_file = f'acados_solver_sfunction_{name}.c'
    render_template(in_file, out_file, template_dir, json_path)

    in_file = 'make_sfun.in.m'
    out_file = f'make_sfun_{name}.m'
    render_template(in_file, out_file, template_dir, json_path)

    # sim
    in_file = 'acados_sim_solver.in.c'
    out_file = f'acados_sim_solver_{name}.c'
    render_template(in_file, out_file, template_dir, json_path)

    in_file = 'acados_sim_solver.in.h'
    out_file = f'acados_sim_solver_{name}.h'
    render_template(in_file, out_file, template_dir, json_path)

    in_file = 'main_sim.in.c'
    out_file = f'main_sim_{name}.c'
    render_template(in_file, out_file, template_dir, json_path)

    ## folder model
    template_dir = os.path.join(code_export_dir, name + '_model')
    in_file = 'model.in.h'
    out_file = f'{name}_model.h'
    render_template(in_file, out_file, template_dir, json_path)

    # constraints on convex over nonlinear function
    if acados_ocp.constraints.constr_type == 'BGP' and acados_ocp.dims.nphi > 0:
        # constraints on outer function
        template_dir = os.path.join(code_export_dir, name + '_constraints')
        in_file = 'phi_constraint.in.h'
        out_file = f'{name}_phi_constraint.h'
        render_template(in_file, out_file, template_dir, json_path)

    # terminal constraints on convex over nonlinear function
    if acados_ocp.constraints.constr_type_e == 'BGP' and acados_ocp.dims.nphi_e > 0:
        # terminal constraints on outer function
        template_dir = os.path.join(code_export_dir, name + '_constraints')
        in_file = 'phi_e_constraint.in.h'
        out_file = f'{name}_phi_e_constraint.h'
        render_template(in_file, out_file, template_dir, json_path)

    # nonlinear constraints
    if acados_ocp.constraints.constr_type == 'BGH' and acados_ocp.dims.nh > 0:
        template_dir = os.path.join(code_export_dir, name + '_constraints')
        in_file = 'h_constraint.in.h'
        out_file = f'{name}_h_constraint.h'
        render_template(in_file, out_file, template_dir, json_path)

    # terminal nonlinear constraints
    if acados_ocp.constraints.constr_type_e == 'BGH' and acados_ocp.dims.nh_e > 0:
        template_dir = os.path.join(code_export_dir, name + '_constraints')
        in_file = 'h_e_constraint.in.h'
        out_file = f'{name}_h_e_constraint.h'
        render_template(in_file, out_file, template_dir, json_path)

    # initial stage Nonlinear LS cost function
    if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS':
        template_dir = os.path.join(code_export_dir, name + '_cost')
        in_file = 'cost_y_0_fun.in.h'
        out_file = f'{name}_cost_y_0_fun.h'
        render_template(in_file, out_file, template_dir, json_path)
    # external cost - terminal
    elif acados_ocp.cost.cost_type_0 == 'EXTERNAL':
        template_dir = os.path.join(code_export_dir, name + '_cost')
        in_file = 'external_cost_0.in.h'
        out_file = f'{name}_external_cost_0.h'
        render_template(in_file, out_file, template_dir, json_path)

    # path Nonlinear LS cost function
    if acados_ocp.cost.cost_type == 'NONLINEAR_LS':
        template_dir = os.path.join(code_export_dir, name + '_cost')
        in_file = 'cost_y_fun.in.h'
        out_file = f'{name}_cost_y_fun.h'
        render_template(in_file, out_file, template_dir, json_path)

    # terminal Nonlinear LS cost function
    if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS':
        template_dir = os.path.join(code_export_dir, name + '_cost')
        in_file = 'cost_y_e_fun.in.h'
        out_file = f'{name}_cost_y_e_fun.h'
        render_template(in_file, out_file, template_dir, json_path)

    # external cost
    if acados_ocp.cost.cost_type == 'EXTERNAL':
        template_dir = os.path.join(code_export_dir, name + '_cost')
        in_file = 'external_cost.in.h'
        out_file = f'{name}_external_cost.h'
        render_template(in_file, out_file, template_dir, json_path)

    # external cost - terminal
    if acados_ocp.cost.cost_type_e == 'EXTERNAL':
        template_dir = os.path.join(code_export_dir, name + '_cost')
        in_file = 'external_cost_e.in.h'
        out_file = f'{name}_external_cost_e.h'
        render_template(in_file, out_file, template_dir, json_path)


def remove_x0_elimination(acados_ocp):
    acados_ocp.constraints.idxbxe_0 = np.zeros((0,))
    acados_ocp.dims.nbxe_0 = 0


class AcadosOcpSolver:
    """
    Class to interact with the acados ocp solver C object.

        :param acados_ocp: type AcadosOcp - description of the OCP for acados
        :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
        :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
    """
    if sys.platform=="win32":
        from ctypes import wintypes
        dlclose = ctypes.WinDLL('kernel32', use_last_error=True).FreeLibrary
        dlclose.argtypes = [wintypes.HMODULE]
    else:
        dlclose = CDLL(None).dlclose
        dlclose.argtypes = [c_void_p]

    @classmethod
    def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True):
        model = acados_ocp.model

        if simulink_opts is None:
            simulink_opts = get_simulink_default_opts()

        # make dims consistent
        make_ocp_dims_consistent(acados_ocp)

        # module dependent post processing
        if acados_ocp.solver_options.integrator_type == 'GNSF':
            set_up_imported_gnsf_model(acados_ocp)

        if acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_QPDUNES':
            remove_x0_elimination(acados_ocp)

        # set integrator time automatically
        acados_ocp.solver_options.Tsim = acados_ocp.solver_options.time_steps[0]

        # generate external functions
        ocp_generate_external_functions(acados_ocp, model)

        # dump to json
        ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file)

        code_export_dir = acados_ocp.code_export_directory
        # render templates
        ocp_render_templates(acados_ocp, json_file)

        if build:
          ## Compile solver
          cwd=os.getcwd()
          os.chdir(code_export_dir)
          os.system('make clean_ocp_shared_lib')
          os.system('make ocp_shared_lib')
          os.chdir(cwd)

    def __init__(self, model_name, N, code_export_dir):
        self.model_name = model_name
        self.N = N

        self.solver_created = False
        self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{self.model_name}.so'

        # get shared_lib
        self.shared_lib = CDLL(self.shared_lib_name)

        # create capsule
        getattr(self.shared_lib, f"{self.model_name}_acados_create_capsule").restype = c_void_p
        self.capsule = getattr(self.shared_lib, f"{self.model_name}_acados_create_capsule")()

        # create solver
        getattr(self.shared_lib, f"{self.model_name}_acados_create").argtypes = [c_void_p]
        getattr(self.shared_lib, f"{self.model_name}_acados_create").restype = c_int
        assert getattr(self.shared_lib, f"{self.model_name}_acados_create")(self.capsule)==0
        self.solver_created = True

        # get pointers solver
        self.__get_pointers_solver()


    def __get_pointers_solver(self):
        """
        Private function to get the pointers for solver
        """
        # get pointers solver
        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").argtypes = [c_void_p]
        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").restype = c_void_p
        self.nlp_opts = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts")(self.capsule)

        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims").argtypes = [c_void_p]
        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims").restype = c_void_p
        self.nlp_dims = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims")(self.capsule)

        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config").argtypes = [c_void_p]
        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config").restype = c_void_p
        self.nlp_config = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config")(self.capsule)

        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").argtypes = [c_void_p]
        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").restype = c_void_p
        self.nlp_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out")(self.capsule)

        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").argtypes = [c_void_p]
        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").restype = c_void_p
        self.nlp_in = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in")(self.capsule)

        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").argtypes = [c_void_p]
        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").restype = c_void_p
        self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule)

        # treat parameters separately
        getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
        getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int
        self._set_param = getattr(self.shared_lib, f"{self.model_name}_acados_update_params")

        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int

        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int

        self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
        self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
        self.shared_lib.ocp_nlp_out_set.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
        self.shared_lib.ocp_nlp_set.argtypes = \
            [c_void_p, c_void_p, c_int, c_char_p, c_void_p]

    def solve(self):
        """
        Solve the ocp with current input.
        """

        getattr(self.shared_lib, f"{self.model_name}_acados_solve").argtypes = [c_void_p]
        getattr(self.shared_lib, f"{self.model_name}_acados_solve").restype = c_int
        status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
        return status


    def set_new_time_steps(self, new_time_steps):
        """
        Set new time steps before solving. Only reload library without code generation but with new time steps.

            :param new_time_steps: vector of new time steps for the solver

            .. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
                      the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
                      do not require a new code export and compilation.
        """

        # unlikely but still possible
        if not self.solver_created:
            raise Exception('Solver was not yet created!')

        # check if time steps really changed in value
        if np.array_equal(self.acados_ocp.solver_options.time_steps, new_time_steps):
            return

        N = new_time_steps.size
        model = self.acados_ocp.model
        new_time_steps_data = cast(new_time_steps.ctypes.data, POINTER(c_double))

        # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
        if self.acados_ocp.solver_options.time_steps.size == N:
            getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").argtypes = [c_void_p, c_int, c_void_p]
            getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").restype = c_int
            assert getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps")(self.capsule, N, new_time_steps_data) == 0
        else:  # recreate the solver with the new time steps
            self.solver_created = False

            # delete old memory (analog to __del__)
            getattr(self.shared_lib, f"{self.model_name}_acados_free").argtypes = [c_void_p]
            getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int
            getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule)

            # store N and new time steps
            self.N = self.acados_ocp.dims.N = N
            self.acados_ocp.solver_options.time_steps = new_time_steps
            self.acados_ocp.solver_options.Tsim = self.acados_ocp.solver_options.time_steps[0]

            # create solver with new time steps
            getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").argtypes = [c_void_p, c_int, c_void_p]
            getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").restype = c_int
            assert getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization")(self.capsule, N, new_time_steps_data) == 0

            self.solver_created = True

            # get pointers solver
            self.__get_pointers_solver()


    def get(self, stage_, field_):
        """
        Get the last solution of the solver:

            :param stage: integer corresponding to shooting node
            :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',]

            .. note:: regarding lam, t: \n
                    the inequalities are internally organized in the following order: \n
                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]

            .. note:: pi: multipliers for dynamics equality constraints \n
                      lam: multipliers for inequalities \n
                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
                      sl: slack variables of soft lower inequality constraints \n
                      su: slack variables of soft upper inequality constraints \n
        """

        out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su']
        # mem_fields = ['sl', 'su']
        field = field_
        field = field.encode('utf-8')

        if (field_ not in out_fields):
            raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
                    \n Possible values are {}. Exiting.'.format(field_, out_fields))

        if not isinstance(stage_, int):
            raise Exception('AcadosOcpSolver.get(): stage index must be Integer.')

        if stage_ < 0 or stage_ > self.N:
            raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))

        if stage_ == self.N and field_ == 'pi':
            raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
                .format(field_, stage_))

        self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
        self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int

        dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
            self.nlp_dims, self.nlp_out, stage_, field)

        out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64)
        out_data = cast(out.ctypes.data, POINTER(c_double))

        if (field_ in out_fields):
            self.shared_lib.ocp_nlp_out_get.argtypes = \
                [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
            self.shared_lib.ocp_nlp_out_get(self.nlp_config, \
                self.nlp_dims, self.nlp_out, stage_, field, out_data)
        # elif field_ in mem_fields:
        #     self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
        #         [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
        #     self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
        #         self.nlp_dims, self.nlp_solver, stage_, field, out_data)

        return out


    def print_statistics(self):
        """
        prints statistics of previous solver run as a table:
            - iter: iteration number
            - res_stat: stationarity residual
            - res_eq: residual wrt equality constraints (dynamics)
            - res_ineq: residual wrt inequality constraints (constraints)
            - res_comp: residual wrt complementarity conditions
            - qp_stat: status of QP solver
            - qp_iter: number of QP iterations
            - qp_res_stat: stationarity residual of the last QP solution
            - qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
            - qp_res_ineq: residual wrt inequality constraints (constraints)  of the last QP solution
            - qp_res_comp: residual wrt complementarity conditions of the last QP solution
        """
        stat = self.get_stats("statistics")

        if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
            print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter')
            if stat.shape[0]>7:
                print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
            for jj in range(stat.shape[1]):
                print('{:d}\t{:e}\t{:e}\t{:e}\t{:e}\t{:d}\t{:d}'.format( \
                     int(stat[0][jj]), stat[1][jj], stat[2][jj], \
                     stat[3][jj], stat[4][jj], int(stat[5][jj]), int(stat[6][jj])))
                if stat.shape[0]>7:
                    print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \
                        stat[7][jj], stat[8][jj], stat[9][jj], stat[10][jj]))
            print('\n')
        elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
            print('\niter\tqp_stat\tqp_iter')
            if stat.shape[0]>3:
                print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
            for jj in range(stat.shape[1]):
                print('{:d}\t{:d}\t{:d}'.format( int(stat[0][jj]), int(stat[1][jj]), int(stat[2][jj])))
                if stat.shape[0]>3:
                    print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \
                         stat[3][jj], stat[4][jj], stat[5][jj], stat[6][jj]))
            print('\n')

        return


    def store_iterate(self, filename='', overwrite=False):
        """
        Stores the current iterate of the ocp solver in a json file.

            :param filename: if not set, use model_name + timestamp + '.json'
            :param overwrite: if false and filename exists add timestamp to filename
        """
        if filename == '':
            filename += self.model_name + '_' + 'iterate' + '.json'

        if not overwrite:
            # append timestamp
            if os.path.isfile(filename):
                filename = filename[:-5]
                filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json'

        # get iterate:
        solution = dict()

        for i in range(self.N+1):
            solution['x_'+str(i)] = self.get(i,'x')
            solution['u_'+str(i)] = self.get(i,'u')
            solution['z_'+str(i)] = self.get(i,'z')
            solution['lam_'+str(i)] = self.get(i,'lam')
            solution['t_'+str(i)] = self.get(i, 't')
            solution['sl_'+str(i)] = self.get(i, 'sl')
            solution['su_'+str(i)] = self.get(i, 'su')
        for i in range(self.N):
            solution['pi_'+str(i)] = self.get(i,'pi')

        # save
        with open(filename, 'w') as f:
            json.dump(solution, f, default=np_array_to_list, indent=4, sort_keys=True)
        print("stored current iterate in ", os.path.join(os.getcwd(), filename))


    def load_iterate(self, filename):
        """
        Loads the iterate stored in json file with filename into the ocp solver.
        """
        if not os.path.isfile(filename):
            raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename))

        with open(filename, 'r') as f:
            solution = json.load(f)

        for key in solution.keys():
            (field, stage) = key.split('_')
            self.set(int(stage), field, np.array(solution[key]))


    def get_stats(self, field_):
        """
        Get the information of the last solver call.

            :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
        """

        fields = ['time_tot',  # total cpu time previous call
                  'time_lin',  # cpu time for linearization
                  'time_sim',  # cpu time for integrator
                  'time_sim_ad',  # cpu time for integrator contribution of external function calls
                  'time_sim_la',  # cpu time for integrator contribution of linear algebra
                  'time_qp',   # cpu time qp solution
                  'time_qp_solver_call',  # cpu time inside qp solver (without converting the QP)
                  'time_qp_xcond',
                  'time_glob',  # cpu time globalization
                  'time_reg',  # cpu time regularization
                  'sqp_iter',  # number of SQP iterations
                  'qp_iter',  # vector of QP iterations for last SQP call
                  'statistics',  # table with info about last iteration
                  'stat_m',
                  'stat_n',]

        field = field_
        field = field.encode('utf-8')
        if (field_ not in fields):
            raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\
                    \n Possible values are {}. Exiting.'.format(fields, fields))

        if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
            out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64)
            out_data = cast(out.ctypes.data, POINTER(c_int64))

        elif field_ == 'statistics':
            sqp_iter = self.get_stats("sqp_iter")
            stat_m = self.get_stats("stat_m")
            stat_n = self.get_stats("stat_n")

            min_size = min([stat_m, sqp_iter+1])

            out = np.ascontiguousarray(
                        np.zeros((stat_n[0]+1, min_size[0])), dtype=np.float64)
            out_data = cast(out.ctypes.data, POINTER(c_double))

        elif field_ == 'qp_iter':
            full_stats = self.get_stats('statistics')
            if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
                out = full_stats[6, :]
            elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
                out = full_stats[2, :]

        else:
            out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
            out_data = cast(out.ctypes.data, POINTER(c_double))

        if not field_ == 'qp_iter':
            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)

        return out


    def get_cost(self):
        """
        Returns the cost value of the current solution.
        """
        # compute cost internally
        self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p]
        self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)

        # create output array
        out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
        out_data = cast(out.ctypes.data, POINTER(c_double))

        # call getter
        self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]

        field = "cost_value".encode('utf-8')
        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)

        return out[0]


    def get_residuals(self):
        """
        Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
        """
        # compute residuals if RTI
        if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
            self.shared_lib.ocp_nlp_eval_residuals.argtypes = [c_void_p, c_void_p, c_void_p]
            self.shared_lib.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)

        # create output array
        out = np.ascontiguousarray(np.zeros((4, 1)), dtype=np.float64)
        out_data = cast(out.ctypes.data, POINTER(c_double))

        # call getters
        self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]

        field = "res_stat".encode('utf-8')
        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)

        out_data = cast(out[1].ctypes.data, POINTER(c_double))
        field = "res_eq".encode('utf-8')
        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)

        out_data = cast(out[2].ctypes.data, POINTER(c_double))
        field = "res_ineq".encode('utf-8')
        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)

        out_data = cast(out[3].ctypes.data, POINTER(c_double))
        field = "res_comp".encode('utf-8')
        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
        return out.flatten()


    # Note: this function should not be used anymore, better use cost_set, constraints_set

    def set(self, stage_, field_, value_):

        """
        Set numerical data inside the solver.

            :param stage: integer corresponding to shooting node
            :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']

            .. note:: regarding lam, t: \n
                    the inequalities are internally organized in the following order: \n
                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]

            .. note:: pi: multipliers for dynamics equality constraints \n
                      lam: multipliers for inequalities \n
                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
                      sl: slack variables of soft lower inequality constraints \n
                      su: slack variables of soft upper inequality constraints \n
        """
        cost_fields = ['y_ref', 'yref']
        constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
        out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']

        # cast value_ to avoid conversion issues
        if isinstance(value_, (float, int)):
            value_ = np.array([value_])
        value_ = value_.astype(float)

        field = field_
        field = field.encode('utf-8')

        stage = c_int(stage_)

        # treat parameters separately
        if field_ == 'p':
            getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
            getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int

            value_data = cast(value_.ctypes.data, POINTER(c_double))

            assert getattr(self.shared_lib, f"{self.model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0
        else:
            if field_ not in constraints_fields + cost_fields + out_fields:
                raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
                    \nPossible values are {}. Exiting.".format(field, \
                    constraints_fields + cost_fields + out_fields + ['p']))

            self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
                [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
            self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int

            dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
                self.nlp_dims, self.nlp_out, stage_, field)

            if value_.shape[0] != dims:
                msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
                msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
                raise Exception(msg)

            value_data = cast(value_.ctypes.data, POINTER(c_double))
            value_data_p = cast((value_data), c_void_p)

            if field_ in constraints_fields:
                self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
                    self.nlp_dims, self.nlp_in, stage, field, value_data_p)
            elif field_ in cost_fields:
                self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
                    self.nlp_dims, self.nlp_in, stage, field, value_data_p)
            elif field_ in out_fields:
                self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
                    self.nlp_dims, self.nlp_out, stage, field, value_data_p)
            # elif field_ in mem_fields:
            #     self.shared_lib.ocp_nlp_set(self.nlp_config, \
            #         self.nlp_solver, stage, field, value_data_p)

        return


    def cost_set(self, stage_, field_, value_, api='warn'):
        """
        Set numerical data in the cost module of the solver.

            :param stage: integer corresponding to shooting node
            :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
            :param value: of appropriate size
        """
        # cast value_ to avoid conversion issues
        if isinstance(value_, (float, int)):
            value_ = np.array([value_])
        value_ = value_.astype(float)

        field = field_
        field = field.encode('utf-8')

        stage = c_int(stage_)
        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int

        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
        dims_data = cast(dims.ctypes.data, POINTER(c_int))

        self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \
            self.nlp_dims, self.nlp_out, stage_, field, dims_data)

        value_shape = value_.shape
        if len(value_shape) == 1:
            value_shape = (value_shape[0], 0)

        elif len(value_shape) == 2:
            if api=='old':
                pass
            elif api=='warn':
                if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
                    raise Exception("Ambiguity in API detected.\n"
                                    "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n"
                                    "Are you seeing this error suddenly in previously running code? Read on.\n"
                                    "  You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) +
                                    "  acados_template now correctly passes on any matrices to acados in column major format.\n" +
                                    "  Two options to fix this error: \n" +
                                    "   * Add api='old' to cost_set to restore old incorrect behaviour\n" +
                                    "   * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " +
                                    "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
                                    "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
                # Get elements in column major order
                value_ = np.ravel(value_, order='F')
            elif api=='new':
                # Get elements in column major order
                value_ = np.ravel(value_, order='F')
            else:
                raise Exception("Unknown api: '{}'".format(api))

        if value_shape != tuple(dims):
            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
                ' for field "{}" with dimension {} (you have {})'.format( \
                field_, tuple(dims), value_shape))

        value_data = cast(value_.ctypes.data, POINTER(c_double))
        value_data_p = cast((value_data), c_void_p)

        self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
        self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
            self.nlp_dims, self.nlp_in, stage, field, value_data_p)

        return


    def constraints_set(self, stage_, field_, value_, api='warn'):
        """
        Set numerical data in the constraint module of the solver.

            :param stage: integer corresponding to shooting node
            :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D']
            :param value: of appropriate size
        """
        # cast value_ to avoid conversion issues
        if isinstance(value_, (float, int)):
            value_ = np.array([value_])
        value_ = value_.astype(float)

        field = field_
        field = field.encode('utf-8')

        stage = c_int(stage_)
        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int

        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
        dims_data = cast(dims.ctypes.data, POINTER(c_int))

        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
            self.nlp_dims, self.nlp_out, stage_, field, dims_data)

        value_shape = value_.shape
        if len(value_shape) == 1:
            value_shape = (value_shape[0], 0)
        elif len(value_shape) == 2:
            if api=='old':
                pass
            elif api=='warn':
                if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
                    raise Exception("Ambiguity in API detected.\n"
                                    "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n"
                                    "Are you seeing this error suddenly in previously running code? Read on.\n"
                                    "  You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) +
                                    "  acados_template now correctly passes on any matrices to acados in column major format.\n" +
                                    "  Two options to fix this error: \n" +
                                    "   * Add api='old' to constraints_set to restore old incorrect behaviour\n" +
                                    "   * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " +
                                    "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
                                    "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
                # Get elements in column major order
                value_ = np.ravel(value_, order='F')
            elif api=='new':
                # Get elements in column major order
                value_ = np.ravel(value_, order='F')
            else:
                raise Exception("Unknown api: '{}'".format(api))

        if value_shape != tuple(dims):
            raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
                ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))

        value_data = cast(value_.ctypes.data, POINTER(c_double))
        value_data_p = cast((value_data), c_void_p)

        self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
        self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
            self.nlp_dims, self.nlp_in, stage, field, value_data_p)

        return


    def dynamics_get(self, stage_, field_):
        """
        Get numerical data from the dynamics module of the solver:

            :param stage: integer corresponding to shooting node
            :param field: string, e.g. 'A'
        """

        field = field_
        field = field.encode('utf-8')
        stage = c_int(stage_)

        # get dims
        self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
        self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.restype = c_int

        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
        dims_data = cast(dims.ctypes.data, POINTER(c_int))

        self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, \
            self.nlp_dims, self.nlp_out, stage_, field, dims_data)

        # create output data
        out = np.ascontiguousarray(np.zeros((np.prod(dims),)), dtype=np.float64)
        out = out.reshape(dims[0], dims[1], order='F')

        out_data = cast(out.ctypes.data, POINTER(c_double))
        out_data_p = cast((out_data), c_void_p)

        # call getter
        self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
        self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
            self.nlp_dims, self.nlp_solver, stage, field, out_data_p)

        return out


    def options_set(self, field_, value_):
        """
        Set options of the solver.

            :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
            :param value: of type int, float
        """
        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
        string_fields = ['globalization']

        # check field availability and type
        if field_ in int_fields:
            if not isinstance(value_, int):
                raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_)))
            else:
                value_ctypes = c_int(value_)

        elif field_ in double_fields:
            if not isinstance(value_, float):
                raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_)))
            else:
                value_ctypes = c_double(value_)

        elif field_ in string_fields:
            if not isinstance(value_, str):
                raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_)))
            else:
                value_ctypes = value_.encode('utf-8')
        else:
            raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
                '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))


        if field_ == 'rti_phase':
            if value_ < 0 or value_ > 2:
                raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
                    'take only values 0, 1, 2 for SQP-RTI-type solvers')
            if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
                raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
                    'take only value 0 for SQP-type solvers')

        # encode
        field = field_
        field = field.encode('utf-8')

        # call C interface
        if field_ in string_fields:
            self.shared_lib.ocp_nlp_solver_opts_set.argtypes = \
                [c_void_p, c_void_p, c_char_p, c_char_p]
            self.shared_lib.ocp_nlp_solver_opts_set(self.nlp_config, \
                self.nlp_opts, field, value_ctypes)
        else:
            self.shared_lib.ocp_nlp_solver_opts_set.argtypes = \
                [c_void_p, c_void_p, c_char_p, c_void_p]
            self.shared_lib.ocp_nlp_solver_opts_set(self.nlp_config, \
                self.nlp_opts, field, byref(value_ctypes))
        return


    def __del__(self):
        if self.solver_created:
            getattr(self.shared_lib, f"{self.model_name}_acados_free").argtypes = [c_void_p]
            getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int
            getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule)

            getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule").argtypes = [c_void_p]
            getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule").restype = c_int
            getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule")(self.capsule)

            try:
                self.dlclose(self.shared_lib._handle)
            except:
                pass