"""MPC class of Impact."""
from re import X
from rockit import Ocp, rockit_pickle_context, rockit_unpickle_context
from rockit.external.method import SourceArtifact, LibraryArtifact, HeaderDirectory
from casadi import Function, MX, vcat, vvcat, veccat, GlobalOptions, vec, CodeGenerator, DaeBuilder, vertsplit, DM
from rockit.casadi_helpers import prepare_build_dir
import casadi
import yaml
import os
from collections import OrderedDict
import shutil
from zipfile import ZipFile 
from lxml import etree
from contextlib import redirect_stdout, redirect_stderr
import io
import numpy as np
import copy
from packaging import version

from .dotdict import DotDict, Structure
from .model import Model
from .diagram import Diagram
from .mask import Mask 
from .helpers import escape, write_struct, write_bus
from .artifact_ros import ROS2Generator

class Field:
  def __init__(self,name,type):
    self.name = name
    self.type = type

class Struct:
  def __init__(self,name,fields):
    self.name = name
    self.fields = fields

def modern_casadi():
  if "-" in casadi.__version__:
    return True
  return version.parse(casadi.__version__)>=version.parse("3.6.5")

CURRENT_CASADI_VERSION = casadi.__version__[:((casadi.__version__).find('-'))] if (casadi.__version__).find('-') != -1 else casadi.__version__
CURRENT_CASADI_VERSION = version.parse(CURRENT_CASADI_VERSION)

fields = [Field("sqp_stop_crit","int"),
          Field("n_sqp_iter","int"),
          Field("n_ls","int"),
          Field("n_max_ls","int"),
          Field("n_qp_iter","int"),
          Field("n_max_qp_iter","int"),
          Field("runtime","double")]

solver_stats_type = Struct("solver_stats_bus",fields)

solver_stats_c_type = Struct("solver_stats",fields)
  

dae_keys = {"x": "differential_states", "z": "algebraic_states", "p": "parameters", "u": "controls"}
dae_rockit_normal = {"x": "state", "z": "algebraic", "p": "parameter", "u": "control"}
dae_rockit_sys_id = {"x": "state", "z": "algebraic", "p": "variable", "u": "parameter"}


def fun2s_function(fun, name=None, dir=".",ignore_errors=False,build_dir_abs=None):
    fun_name = fun.name()
    if name is None:
      name = fun_name
    s_function_name = name+"_s_function_level2"
    cg_options = {"casadi_real": "real_T", "casadi_int": "int_T"}
    cg = CodeGenerator(name, cg_options)
    cg.add(fun)
    code = cg.dump()
    use_codegen = True
    if "#error" in code:
      use_codegen = False
    print(f"Use codegen for {fun_name}? {use_codegen}")
    if not use_codegen:
      code = f"""#include <casadi/casadi_c.h>
      #define casadi_int long long int
      """
    for i in range(fun.n_in()):
      if not fun.sparsity_in(i).is_dense():
        raise Exception("Sparse inputs not supported")
    for i in range(fun.n_out()):
      if not fun.sparsity_out(i).is_dense():
        raise Exception("Sparse outputs not supported")

    s_function_file_name_base = s_function_name+".c"
    s_function_file_name = os.path.join(dir,s_function_file_name_base)

    def c_api(suffix):
      if use_codegen:
        if suffix=="eval":
          return fun_name+"("
        return fun_name+"_"+suffix+"("
      else:
        return "casadi_c_"+suffix+"_id(id" + ("," if suffix in ["eval","work","sparsity_in","sparsity_out","release"] else "")

    with open(s_function_file_name,"w") as out:
      out.write(f"""
        #define S_FUNCTION_NAME {s_function_name}
        #define S_FUNCTION_LEVEL 2

        #include "simstruc.h"
        {code}

        static int_T n_in, n_out;
        static casadi_int sz_arg, sz_res, sz_iw, sz_w;
        static int id;

        static void mdlInitializeSizes(SimStruct *S) {{
          int flag;
          char casadi_file[2048];
          ssSetNumSFcnParams(S, 0);
          if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {{
              return; /* Parameter mismatch will be reported by Simulink */
          }}
        """)

      if not use_codegen:
        out.write(f"""
          id = casadi_c_id("{fun_name}");
          if (id<0) {{
            flag = casadi_c_push_file("{escape(build_dir_abs)}/{name}.casadi");
            if (flag) {{
  #ifdef MATLAB_MEX_FILE
                mexErrMsgIdAndTxt( "MATLAB:s_function:invalidParameter",
                                  "Failed to initialize.");
  #else
                return;
  #endif     
            }}
            id = casadi_c_id("{fun_name}");
            if (id<0) {{
  #ifdef MATLAB_MEX_FILE
                mexErrMsgIdAndTxt( "MATLAB:s_function:invalidParameter",
                                  "Could not locate function.");
  #else
                return;
  #endif     
            }}
          }}
        """)

      out.write(f"""
        /* Read in CasADi function dimensions */
        n_in  = {c_api("n_in")});
        n_out = {c_api("n_out")});
        {c_api("work")}&sz_arg, &sz_res, &sz_iw, &sz_w);

        /* Set up simulink input/output ports */
        int_T i;
        if (!ssSetNumInputPorts(S, n_in)) return;
        for (i=0;i<n_in;++i) {{
          const casadi_int* sp = {c_api("sparsity_in")}i);
          /* Dense inputs assumed here */
          ssSetInputPortDirectFeedThrough(S, i, 1);
          ssSetInputPortMatrixDimensions(S, i, sp[0], sp[1]);
          ssSetInputPortRequiredContiguous(S, i, 1);
        }}

        if (!ssSetNumOutputPorts(S, n_out)) return;
        for (i=0;i<n_out;++i) {{
          const casadi_int* sp = {c_api("sparsity_out")}i);
          /* Dense outputs assumed here */
          ssSetOutputPortMatrixDimensions(S, i, sp[0], sp[1]);
        }}

        ssSetNumSampleTimes(S, 1);
        
        /* Set up CasADi function work vector sizes */
        ssSetNumRWork(S, sz_w);
        ssSetNumIWork(S, 2*sz_iw);
        ssSetNumPWork(S, sz_arg+sz_res);
        ssSetNumNonsampledZCs(S, 0);

        /* specify the sim state compliance to be same as a built-in block */
        ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);

        ssSetOptions(S,
                    SS_OPTION_EXCEPTION_FREE_CODE |
                    SS_OPTION_USE_TLC_WITH_ACCELERATOR);

        /* Signal that we want to use the CasADi Function */
        {c_api("incref")});
        }}

        static void mdlInitializeSampleTimes(SimStruct *S)
        {{
            ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);
            ssSetOffsetTime(S, 0, 0.0);
            ssSetModelReferenceSampleTimeDefaultInheritance(S); 
        }}

        static void mdlOutputs(SimStruct *S, int_T tid)
        {{

          /* Set up CasADi function work vectors */
          void** p = ssGetPWork(S);
          const real_T** arg = (const real_T**) p;
          p += sz_arg;
          real_T** res = (real_T**) p;
          real_T* w = ssGetRWork(S);
          int_T* iw = ssGetIWork(S);
          
          
          /* Point to input and output buffers */
          int_T i;   
          for (i=0; i<n_in;++i) {{
            arg[i] = ssGetInputPortSignal(S,i);
          }}
          for (i=0; i<n_out;++i) {{
            res[i] = ssGetOutputPortRealSignal(S,i);
          }}

          /* Get a hold on a location to read/write persistant internal memory
          */

          int mem = {c_api("checkout")});

          /* Run the CasADi function */
          int_T ret = {c_api("eval")}arg,res,iw,w,mem);

          if (ret && {int(not ignore_errors)}) {{
              static char msg[{100+len(s_function_name)}];
              sprintf(msg, "SFunction '{s_function_name}' failed to compute (error code %d) at t=%.6fs.", ret, ssGetT(S));
              #ifdef ssSetLocalErrorStatus
                ssSetLocalErrorStatus(S, msg);
              #else
                #ifdef ssSetErrorStatus
                  ssSetErrorStatus(S, msg);
                #endif
              #endif
          }}

          /* Release hold */
          {c_api("release")}mem);

        }}

        static void mdlTerminate(SimStruct *S) {{
          {c_api("decref")});
          """)

      if not use_codegen:
          out.write(f"""
            //casadi_c_pop();
          """)

      out.write(f"""}}

        #ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
        #include "simulink.c"      /* MEX-file interface mechanism */
        #else
        #include "cg_sfun.h"       /* Code generation registration function */
        #endif
        """)
    return s_function_file_name_base



class MPC(Ocp):
  """This should be a description of the MPC class.
  It's common for programmers to give a code example inside of their
  docstring::

      from impact import MPC
      mpc = MPC(T=2.0)

  Here is a link to :py:meth:`__init__`.
  """

  def __init__(self, **kwargs):
    """Inits MPC class."""

    Ocp.__init__(self, **kwargs)
    self._expr = Model()
    self.basename = os.path.dirname(__file__)
    self._added_functions = []

  @property
  def expr(self):
      return self._expr

  def control(self,*args,**kwargs):
    """Defines control variable"""

    if len(args)>0 and isinstance(args[0],str):
        name = args[0]
        args = args[1:]
    else:
      name = "u%d" % self.nu
    
    n_rows = args[0] if len(args)>0 else 1
    n_cols = args[1] if len(args)>1 else 1
    scale = args[2] if len(args)>2 else 1
    
    order = kwargs['order'] if 'order' in kwargs else 0

    if order >= 1: 
        u = self.state(n_rows, n_cols, scale=scale)
        helper_u = self.control(n_rows=n_rows, n_cols=n_cols, order=order - 1, scale=scale)
        self.set_der(u, helper_u)
        return u
    
    u = MX.sym(name,*args)
    self.register_control(u,scale)
    self.expr._register('u', {name: u})
    return u

  def state(self,*args,**kwargs):
    if len(args)>0 and isinstance(args[0],str):
        name = args[0]
        args = args[1:]
    else:
      # if "quad" in kwargs and kwargs["quad"]:
      #   name = "xq%d" % self.nxq # This gets an error since self.nxq does not exist
      # else:
      name = "x%d" % self.nx
    x = MX.sym(name,*args)
    self.register_state(x,**kwargs)
    self.expr._register('x', {name: x})
    return x

  def parameter(self,*args,**kwargs):
    if len(args)>0 and isinstance(args[0],str):
        name = args[0]
        args = args[1:]
    else:
      name = "p%d" % self.np
    p = MX.sym(name,*args)
    self.register_parameter(p,**kwargs)
    self.expr._register('p', {name: p})
    return p

  def variable(self,*args,**kwargs):
    if len(args)>0 and isinstance(args[0],str):
        name = args[0]
        args = args[1:]
    else:
      name = "v%d" % self.nv
    v = MX.sym(name,*args)
    self.register_variable(v,**kwargs)
    self.expr._register('v', {name: v})
    return v

  def add_function(self, fun):
    self._added_functions.append(fun)

  def add_simulink_fmu(self,name,verbose=True):
    """
      Not supported: 
      * time dependence
      * delays
      Perhpas SS is better
      Caveats:
      * scaling for finite diff
    """
    from pathlib import Path
    import zipfile
    fmu_path = Path(name).resolve()
    unzipped_path =  fmu_path.parent / fmu_path.stem
    with zipfile.ZipFile(fmu_path, 'r') as zip_ref:
        zip_ref.extractall(unzipped_path)

    scalar_variables = []
    with open(unzipped_path/ 'modelDescription.xml','r') as modelDescription_file:
      tree = etree.parse(modelDescription_file)
      for var in tree.find('.//ModelVariables').findall('ScalarVariable'):
        scalar_variables.append(dict(var.items()))
    
    root = tree.find('.')
    modelName = root.attrib["modelName"]
    guid = root.attrib["guid"]
    resource = "file://" + str(unzipped_path)
    
    dll_name = unzipped_path / "binaries" / "linux64" / "vdp"

    u_i = [int(e["valueReference"]) for e in scalar_variables if e["causality"]=="input"]
    y_i = [int(e["valueReference"]) for e in scalar_variables if e["causality"]=="output"]
    x_i = [int(e["valueReference"]) for e in scalar_variables if e["causality"]=="parameter" and e["description"].startswith("x")]
    assert len(y_i)==len(x_i)
    p_i = [int(e["valueReference"]) for e in scalar_variables if e["causality"]=="parameter" and not e["description"].startswith("x")]
    print(x_i,y_i)
    
    nu = len(u_i)
    nx = len(x_i)
    np = len(p_i)
    print(nx)

    def intlist(L):
      return "{" + ", ".join(str(e) for e in L) + "}"
    with open("fmi_wrapper.cpp","w") as fmi_wrapper:
      fmi_wrapper.write(f"""
#include <math.h>
#include <vector>
#include <iostream>
#include <sstream>
#include <exception>
#include <stdarg.h>

#ifdef _WIN32 // also for 64-bit
#include <windows.h>
#else // _WIN32
#include <dlfcn.h>
#endif // _WIN32

#include <fmi2/fmi2Functions.h>

#ifndef casadi_real
#define casadi_real double
#endif

#ifndef casadi_int
#define casadi_int long long int
#endif

std::string system_infix() {{
#if defined(_WIN32)
  // Windows system
#ifdef _WIN64
  return "win64";
#else
  return "win32";
#endif
#elif defined(__APPLE__)
  // OSX
  return sizeof(void*) == 4 ? "darwin32" : "darwin64";
#else
  // Linux
  return sizeof(void*) == 4 ? "linux32" : "linux64";
#endif
}}

std::string dll_suffix() {{
#if defined(_WIN32)
  // Windows system
  return ".dll";
#elif defined(__APPLE__)
  // OSX
  return ".dylib";
#else
  // Linux
  return ".so";
#endif
}}


/* Symbol visibility in DLLs */
#ifndef CASADI_SYMBOL_EXPORT
  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
    #if defined(STATIC_LINKED)
      #define CASADI_SYMBOL_EXPORT
    #else
      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
    #endif
  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
  #else
    #define CASADI_SYMBOL_EXPORT
  #endif
#endif


std::vector<casadi_int> dense_vec(int n) {{
    std::vector<casadi_int> ret = {{n , 1, 0, n}};
    for (int i=0;i<n;++i) {{
        ret.push_back(i);
    }}
    return ret;
}}

template<typename T>
T* get_ptr(std::vector<T> &v) {{
if (v.empty())
  return nullptr;
else
  return &v.front();
}}


#if defined(_WIN32) // also for 64-bit
    typedef HINSTANCE handle_t;
#else
    typedef void* handle_t;
#endif

  /// String representation, any type
  template<typename T>
  std::string str(const T& v);

  // Implementations
  template<typename T>
  std::string str(const T& v) {{
    std::stringstream ss;
    ss << v;
    return ss.str();
  }}

  handle_t init_handle(const std::string& name) {{
    handle_t ret;
#ifdef _WIN32
    ret = LoadLibrary(TEXT(name.c_str()));
    if (ret==0) {{
      throw std::system_error(0, std::generic_category(), "CommonExternal: Cannot open \"" + name + "\". "
      "Error code (WIN32): " + str(GetLastError()));
    }}
#else // _WIN32
    ret = dlopen(name.c_str(), RTLD_LAZY);
    if (ret==nullptr) {{
      throw std::system_error(0, std::generic_category(), "CommonExternal: Cannot open \"" + name + "\". "
      "Error code: " + str(dlerror()));
    }}
    // reset error
    dlerror();
#endif // _WIN32
    return ret;
  }}

  void close_handle(handle_t h) {{
    #ifdef _WIN32
        if (h) FreeLibrary(h);
    #else // _WIN32
        if (h) dlclose(h);
    #endif // _WIN32
  }}

  typedef void (*signal_t)(void);

  signal_t get_function(handle_t h, const std::string& sym) {{
#ifdef _WIN32
    return (signal_t)GetProcAddress(h, TEXT(sym.c_str()));
#else // _WIN32
    signal_t fcnPtr = (signal_t)dlsym(h, sym.c_str());
    if (dlerror()) {{
      fcnPtr=nullptr;
      dlerror(); // Reset error flags
    }}
    return fcnPtr;
#endif // _WIN32
  }}

void logger(fmi2ComponentEnvironment componentEnvironment,
    fmi2String instanceName,
    fmi2Status status,
    fmi2String category,
    fmi2String message, ...) {{
  // Variable number of arguments
  va_list args;
  va_start(args, message);
  // Static & dynamic buffers
  char buf[256];
  size_t buf_sz = sizeof(buf);
  char* buf_dyn = nullptr;
  // Try to print with a small buffer
  int n = vsnprintf(buf, buf_sz, message, args);
  // Need a larger buffer?
  if (n > buf_sz) {{
    buf_sz = n + 1;
    buf_dyn = new char[buf_sz];
    n = vsnprintf(buf_dyn, buf_sz, message, args);
  }}
  // Print buffer content
  if (n >= 0) {{
    std::cout << "[" << instanceName << ":" << category << "] "
      << (buf_dyn ? buf_dyn : buf) << std::endl;
  }}
  // Cleanup
  delete[] buf_dyn;
  va_end(args);
  // Throw error if failure
  if (n<=0) {{
    throw std::runtime_error("Print failure while processing '" + std::string(message) + "'");
  }}
}}


void my_assert(bool v, const std::string& msg) {{
  if (!v) throw std::runtime_error(msg);
}}

class FMU {{
  public:
    FMU() {{
      h_ = init_handle("{dll_name}"+dll_suffix());
      instantiate_ = reinterpret_cast<fmi2InstantiateTYPE*>(get_function(h_,"fmi2Instantiate"));
      free_instance_ = reinterpret_cast<fmi2FreeInstanceTYPE*>(get_function(h_,"fmi2FreeInstance"));
      reset_ = reinterpret_cast<fmi2ResetTYPE*>(get_function(h_,"fmi2Reset"));
      setup_experiment_ = reinterpret_cast<fmi2SetupExperimentTYPE*>(
        get_function(h_,"fmi2SetupExperiment"));
      enter_initialization_mode_ = reinterpret_cast<fmi2EnterInitializationModeTYPE*>(
        get_function(h_,"fmi2EnterInitializationMode"));
      exit_initialization_mode_ = reinterpret_cast<fmi2ExitInitializationModeTYPE*>(
        get_function(h_,"fmi2ExitInitializationMode"));
      get_real_ = reinterpret_cast<fmi2GetRealTYPE*>(get_function(h_,"fmi2GetReal"));
      set_real_ = reinterpret_cast<fmi2SetRealTYPE*>(get_function(h_,"fmi2SetReal"));
      get_integer_ = reinterpret_cast<fmi2GetIntegerTYPE*>(get_function(h_,"fmi2GetInteger"));
      set_integer_ = reinterpret_cast<fmi2SetIntegerTYPE*>(get_function(h_,"fmi2SetInteger"));
      get_boolean_ = reinterpret_cast<fmi2GetBooleanTYPE*>(get_function(h_,"fmi2GetBoolean"));
      set_boolean_ = reinterpret_cast<fmi2SetBooleanTYPE*>(get_function(h_,"fmi2SetBoolean"));
      get_string_ = reinterpret_cast<fmi2GetStringTYPE*>(get_function(h_,"fmi2GetString"));
      set_string_ = reinterpret_cast<fmi2SetStringTYPE*>(get_function(h_,"fmi2SetString"));
      do_step_ = reinterpret_cast<fmi2DoStepTYPE*>(get_function(h_,"fmi2DoStep"));

      logging_on_ = {int(verbose)};
      c_.resize(64);

      c_[0] = instantiate();
      setup_experiment(0);

      u_i_ = {intlist(u_i)};
      x_i_ = {intlist(x_i)};
      y_i_ = {intlist(y_i)};
      p_i_ = {intlist(p_i)};

    }}
    ~FMU() {{
      close_handle(h_);
    }}
    fmi2Component instantiate() const {{
      fmi2String instanceName = "{modelName}";
      fmi2String fmuGUID = "{guid}";
      fmi2String fmuResourceLocation = "{resource}";
      fmi2Boolean visible = fmi2False;
      static fmi2CallbackFunctions cbf;
      cbf.logger = logger;
      cbf.allocateMemory = calloc;
      cbf.freeMemory = free;
      cbf.stepFinished = 0;
      cbf.componentEnvironment = 0;
      fmi2Component c = instantiate_(instanceName, fmi2CoSimulation, fmuGUID, fmuResourceLocation,
        &cbf, visible, logging_on_);
      return c;
    }}

    int setup_experiment(int mem) const {{
      // Call fmi2SetupExperiment
      fmi2Status status = setup_experiment_(c_[mem], fmi2False, 0, 0., fmi2True, 1.);
      if (status != fmi2OK) {{
        std::cerr << "fmi2SetupExperiment failed: " << status << std::endl;
        return 1;
      }}
      return 0;
    }}

    int reset(int mem) const {{
      fmi2Status status = reset_(c_[mem]);
      if (status != fmi2OK) {{
        std::cerr << "fmi2Reset failed: " << status << std::endl;
        return 1;
      }}
      return 0;
    }}

    int enter_initialization_mode(int mem) const {{
      fmi2Status status = enter_initialization_mode_(c_[mem]);
      if (status != fmi2OK) {{
        std::cerr << "fmi2EnterInitializationMode failed: " << status << std::endl;
        return 1;
      }}
      return 0;
    }}

    int exit_initialization_mode(int mem) const {{
      fmi2Status status = exit_initialization_mode_(c_[mem]);
      if (status != fmi2OK) {{
        std::cerr << "fmi2ExitInitializationMode failed: " << status << std::endl;
        return 1;
      }}
      return 0;
    }}

    int set_x(int mem, const double* x) {{
      fmi2Status status = set_real_(c_[mem], get_ptr(x_i_), x_i_.size(), x);
      if (status != fmi2OK) {{
        std::cerr << "fmi2SetReal x failed: " << status << std::endl;
        return 1;
      }}
      return 0;
    }}
    int set_u(int mem, const double* u) {{
      fmi2Status status = set_real_(c_[mem], get_ptr(u_i_), u_i_.size(), u);
      if (status != fmi2OK) {{
        std::cerr << "fmi2SetReal u failed: " << status << std::endl;
        return 1;
      }}
      return 0;
    }}
    int set_p(int mem, const double* p) {{
      fmi2Status status = set_real_(c_[mem], get_ptr(p_i_), p_i_.size(), p);
      if (status != fmi2OK) {{
        std::cerr << "fmi2SetReal p failed: " << status << std::endl;
        return 1;
      }}
      return 0;
    }}

    int get_x(int mem, double* x) {{
      fmi2Status status = get_real_(c_[mem], get_ptr(y_i_), y_i_.size(), x);
      if (status != fmi2OK) {{
        std::cerr << "fmi2GetReal x failed: " << status << std::endl;
        return 1;
      }}
      return 0;
    }}

    int do_step(int mem, double dt) {{
      fmi2Status status = do_step_(c_[mem], 0, dt, fmi2False);
      if (status != fmi2OK) {{
        std::cerr << "fmi2DosStep x failed: " << status << std::endl;
        return 1;
      }}
      return 0;
    }}
  
  private:
    handle_t h_;
    fmi2InstantiateTYPE* instantiate_;
    fmi2FreeInstanceTYPE* free_instance_;
    fmi2ResetTYPE* reset_;
    fmi2SetupExperimentTYPE* setup_experiment_;
    fmi2EnterInitializationModeTYPE* enter_initialization_mode_;
    fmi2ExitInitializationModeTYPE* exit_initialization_mode_;
    fmi2GetRealTYPE* get_real_;
    fmi2SetRealTYPE* set_real_;
    fmi2GetBooleanTYPE* get_boolean_;
    fmi2SetBooleanTYPE* set_boolean_;
    fmi2GetIntegerTYPE* get_integer_;
    fmi2SetIntegerTYPE* set_integer_;
    fmi2GetStringTYPE* get_string_;
    fmi2SetStringTYPE* set_string_;
    fmi2GetDirectionalDerivativeTYPE* get_directional_derivative_;
    fmi2DoStepTYPE* do_step_;

    std::vector<fmi2Component> c_;
    std::vector<fmi2ValueReference> x_i_;
    std::vector<fmi2ValueReference> y_i_;
    std::vector<fmi2ValueReference> u_i_;
    std::vector<fmi2ValueReference> p_i_;

    bool logging_on_;
}};

static FMU fmu;

#ifdef __cplusplus
extern "C" {{
#endif


CASADI_SYMBOL_EXPORT int F(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){{
  // do_step seems to do an univertible operation on time -> reset+setup_experiment
  if (fmu.setup_experiment(mem)) return 1;
  if (fmu.enter_initialization_mode(mem)) return 1;
  if (fmu.set_x(mem, arg[0])) return 1;
  if (fmu.set_u(mem, arg[1])) return 1;
  if (fmu.set_p(mem, arg[2])) return 1;
  if (fmu.exit_initialization_mode(mem)) return 1;
  if (fmu.do_step(mem, arg[4][0])) return 1;
  if (fmu.get_x(mem, res[0])) return 1;
  if (fmu.reset(mem)) return 1;
  return 0;
}}

CASADI_SYMBOL_EXPORT int F_alloc_mem(void) {{
  return 0;
}}

CASADI_SYMBOL_EXPORT int F_init_mem(int mem) {{
  std::cout << "test init" << std::endl;
  return 0;
}}

CASADI_SYMBOL_EXPORT void F_free_mem(int mem) {{
}}

CASADI_SYMBOL_EXPORT int F_checkout(void) {{
  return 0;
}}

CASADI_SYMBOL_EXPORT void F_release(int mem) {{
}}



CASADI_SYMBOL_EXPORT void F_incref(void) {{
}}

CASADI_SYMBOL_EXPORT void F_decref(void) {{
}}

CASADI_SYMBOL_EXPORT casadi_int F_n_in(void) {{ return 5;}}

CASADI_SYMBOL_EXPORT casadi_int F_n_out(void) {{ return 1;}}

CASADI_SYMBOL_EXPORT casadi_real F_default_in(casadi_int i) {{
  switch (i) {{
    default: return 0;
  }}
}}

CASADI_SYMBOL_EXPORT const char* F_name_in(casadi_int i) {{
  switch (i) {{
    case 0: return "x";
    case 1: return "u";
    case 2: return "p";
    case 3: return "t";
    case 4: return "dt";
    default: return 0;
  }}
}}

CASADI_SYMBOL_EXPORT const char* F_name_out(casadi_int i) {{
  switch (i) {{
    case 0: return "xp";
    default: return 0;
  }}
}}

CASADI_SYMBOL_EXPORT const casadi_int* F_sparsity_in(casadi_int i) {{
  switch (i) {{
    case 0: 
      {{
        static std::vector<casadi_int> sp_x = dense_vec({nx});
        return get_ptr(sp_x);
      }}
      break;
    case 1:
      {{
        static std::vector<casadi_int> sp_u = dense_vec({nu});
        return get_ptr(sp_u);
      }}
      break;
    case 2:
      {{
        static std::vector<casadi_int> sp_p = dense_vec({np});
        return get_ptr(sp_p);
      }}
      break;
    case 3:
    case 4:
      {{
        static std::vector<casadi_int> sp_t = dense_vec(1);
        return get_ptr(sp_t);
      }}
      break;
    default: return 0;
  }}
}}

CASADI_SYMBOL_EXPORT const casadi_int* F_sparsity_out(casadi_int i) {{
  switch (i) {{
    case 0: 
      {{
        static std::vector<casadi_int> sp_x = dense_vec({nx});
        return get_ptr(sp_x);
      }}
      break;
    default: return 0;
  }}
}}


#ifdef __cplusplus
}} /* extern "C" */
#endif
      """)

    import subprocess
    from pathlib import Path
    cmd = f"g++ fmi_wrapper.cpp -std=c++11 -I{Path(__file__).parent.parent / 'interfaces'} -shared -fPIC -o fmi_wrapper.so"
    print(cmd)
    subprocess.run(cmd, shell=True)
    F = casadi.external("F", "fmi_wrapper.so",{"enable_fd":True})
    print(F)

    x = self.state(nx)
    u = self.control(nu)
    p = self.parameter("fmu_p",np)

    self.set_next(x, F(x, u, p, self.t, 0.1))

    return F

  def add_model(self,name,file_name,mode='normal'):
    """Creates a model based on a yaml file

        :param name: Name of the model
        :type name: string

        :param file_name: Path to the yaml file
        :type name: string

        :param mode: normal or sys_id - in sys_id, parameters are promoted to variables and controls become parameters
        :type mode: string

        :return: model
        :rtype: Model

    """
    from pathlib import Path
    if not Path(file_name).is_file(): # check that the file exists
        raise Exception(f"The model file {file_name} does not exist. Please try with another file name.") 
    else:
        # Initialize model object
        m = Model(name+".")

    if mode=="sys_id":
      dae_rockit = dae_rockit_sys_id
    elif mode=="normal":
      dae_rockit = dae_rockit_normal
    else:
      raise Exception("Unknown mode: '%s' - pick 'normal' or 'sys_id'" % mode)
    
    # Read meta information from YAML file
    if file_name.endswith('.yaml'): 
      with open(file_name) as file:
          model_meta = yaml.load(file, Loader=yaml.FullLoader)

      # Add the serialized CasADi functions specified in the yaml file as equations/functions to the model.
      robot_functions = {}
      definitions = casadi.__dict__
      locals = dict(m._d)
      if "functions" in model_meta:
        functions = model_meta["functions"]
        if "external" in functions:
          external_functions = functions["external"]
          for k,v in external_functions.items():
            assert external_functions[k]["type"]=="casadi_serialized"
            robot_functions[k] = Function.load(f"{v['file_name']}")
      m._update(robot_functions)
      locals.update(robot_functions)

          
    # Read meta information from YAML file inside a compressed folder (.impz)   
    elif file_name.endswith('.impz'): 
      from tempfile import TemporaryDirectory

      # Retrieve the model file
      zip_file = ZipFile(file_name)
      
      with TemporaryDirectory() as tempdir:
          # Extract all files in model file into a temporary directory
          zip_file.extractall(tempdir)

          for file in os.listdir(tempdir):
            if file.endswith(".yaml"):
              # Open YAML file and load it as model_meta
              with open(f"{tempdir}/{file}", 'r') as yamlfile:
                model_meta = yaml.load(yamlfile, Loader=yaml.FullLoader)
              # If a YAML file has been found, stop iterating over the extracted files
              break
          
          # Add the serialized CasADi functions specified in the yaml file as equations/functions to the model.
          robot_functions = {}
          definitions = casadi.__dict__
          locals = dict(m._d)
          if "functions" in model_meta:
            functions = model_meta["functions"]
            if "external" in functions:
              external_functions = functions["external"]
              for k,v in external_functions.items():
                assert external_functions[k]["type"]=="casadi_serialized"
                robot_functions[k] = Function.load(f"{tempdir}/{v['file_name']}")
          m._update(robot_functions)
          locals.update(robot_functions)

    # Define constants
    constants = {}
    definitions = casadi.__dict__
    locals = dict(m._d)
    for k,v in m._d.items():
      if k.startswith(name+"."):
        locals[k[len(name)+1:]] = v
    if "constants" in model_meta:
      if "inline" in model_meta["constants"]:
        for k,v in model_meta["constants"]["inline"].items():
          try:
            constants[k] = eval(v,casadi.__dict__,locals)
          except:
            constants[k] = v
          locals[k] = constants[k]
    m._register("c", constants)

    # Read equations
    if not "equations" in model_meta:
      raise Exception("Model file is missing 'equations' section.")
    equations = model_meta["equations"]
    if ("external" in equations) == ("inline" in equations):
      raise Exception("Model equations must be either 'inline' or 'external'.")

    if "external" in equations:
      external = equations["external"]

      model_file_name = os.path.expanduser(external["file_name"])
      if not os.path.isabs(model_file_name):
        model_file_name = os.path.join(os.path.dirname(file_name),model_file_name)
      if external["type"]=="casadi_serialized":
        model = Function.load(model_file_name)
      elif external["type"]=="fmu":
        try:
          dae = DaeBuilder(name, model_file_name)
        except:
          unzipped_path = name+"_unzipped"
          import shutil
          #if os.path.isdir(unzipped_path): shutil.rmtree(unzipped_path)  
          # Unzip
          import zipfile
          #with zipfile.ZipFile(model_file_name, 'r') as zip_ref: zip_ref.extractall(unzipped_path)
          dae = DaeBuilder(name, unzipped_path)

        model = dae.create('f', ['x', 'u', 'p'], ['ode','y'])
      else:
        raise Exception("Unknown external type: %s" % external["type"])
      # Make sure the CasADi Function adheres to a standard
      expected_names = dae_keys.keys()
      unknowns = set(model.name_in()).difference(set(expected_names))
      if unknowns:
        raise Exception("These input names are not understood: %s. Use the following canonical names: %s." % (unknowns, expected_names))
      
      for i in range(model.n_in()):
        assert model.sparsity_in(i).is_vector()
    else:
      # Not defined yet, defer until after variables meta-data
      model = None

    # Read variables meta-data
    args = {}
    names = {}
    nd = {}

    def sanitize_name(name):
      if isinstance(name,str):
        return name.replace(".","__")
      else:
        name['name'] = sanitize_name(name['name'])
        return name

    # Loop over dae variables, e.g. key=x, long_name=differential_states
    for key, long_name in dae_keys.items():
      # Get corresponding rockit syntax name
      rockit_name = dae_rockit[key] if key in dae_rockit else None
      # If we have a model already (externally defined), retrieve variable length
      if model:
        var_len = model.numel_in(key) if key in model.name_in() else 0
      else:
        var_len = 0
      
      if var_len or not model:
        if long_name in model_meta:
          s = m._register(key,[sanitize_name(e) for e in model_meta[long_name]])
          names[key] = s._names
          locals.update(dict(zip(s._names,s._symbols)))
          v = getattr(m,key)
          if model:
            assert v.numel()==var_len
          else:
            var_len = v.numel()
          if rockit_name:
            register = getattr(self,"register_"+rockit_name)
            for e in v.primitives():
              register(e)
        else:
          if model:
            v = getattr(self,rockit_name)("dummy",var_len)
          else:
            v = MX(0, 1)
        args[key] = v
      nd[key] = var_len
    

    if model:
      if "y" in model.name_out():
        var_len = model.numel_out("y")
      else:
        var_len = 0
      if var_len>0:
        assert "outputs" in model_meta
        names["y"] = [sanitize_name(e['name']) for e in model_meta["outputs"]]
      else:
        names["y"] = []
    else:
      if "outputs" in model_meta:
        names["y"] = [sanitize_name(e['name']) for e in model_meta["outputs"]]
      else:
        names["y"] = []

    if 'parameters' in model_meta:
      # Retrieve parameter values
      for p in model_meta['parameters']:
        self.set_value(getattr(m,sanitize_name(p['name'])),p['value'])
        m._register('p_val',{sanitize_name(p['name'])+'_val': DM(p['value'])})

    # Define Outputs
    outputs = {}
    definitions = casadi.__dict__
    locals = dict(m._d)
    for k,v in m._d.items():
      if k.startswith(name+"."):
        locals[k[len(name)+1:]] = v
    locals["sq"] = lambda x: x**2

    if "external" in equations:
      model_res = model(**args)
    
    # Parse inline equations
    if "inline" in equations:
      model_res = {}
      inline = equations["inline"]

      if "outputs" in inline:
        outputs = inline["outputs"]
        outputs_ordered = []
        for local_name in names["y"]:
          if local_name not in outputs:
            raise Exception()
          output = eval(outputs[local_name],casadi.__dict__,locals)
          outputs_ordered.append(output)
          locals[local_name] = output
        model_res["y"] = vvcat(outputs_ordered)
        assert len(names["y"])==len(outputs)

      if "ode" in inline:
        ode = inline["ode"]
        rhs_ordered = []
        for local_name in names["x"]:
          if local_name not in ode:
            raise Exception()
          rhs_ordered.append(eval(ode[local_name],casadi.__dict__,locals))
        model_res["ode"] = vvcat(rhs_ordered)
        assert len(names["x"])==len(ode)

      if "alg" in inline:
        alg = inline["alg"]
        alg_ordered = []
        for e in alg:
          alg_ordered.append(eval(e,casadi.__dict__,locals))
        model_res["alg"] = vvcat(alg_ordered)

      keys = model_res.keys()
      res = casadi.cse([model_res[k] for k in keys])
      for k,v in zip(keys,res):
        model_res[k] = v

    ode = MX(0,1)
    alg = MX(0,1)
    if nd["x"]:
      assert "ode" in model_res
      ode = model_res["ode"]
      self.set_der(m.x, ode)
    if nd["z"]:
      assert "alg" in model_res
      self.add_alg(model_res["alg"])

    if "y" in model_res:
      m._register("y",dict(zip(names["y"],vertsplit(model_res["y"]))))
    m._update({"ode": ode, "alg": alg},allow_keyword=True)
    return m

  @staticmethod
  def patch_codegen(name,ocp,qp_error_on_fail=True):
    import re
    with open(name,'r') as fin:
      lines = fin.readlines()

    name_h = name[:-1]+"h"

    with open(name_h,'r') as fin:
      lines_h = fin.readlines()

    increfs = []

    qp = False

    fun_context = ""

    with open(name,'w') as fout:
      for line in lines:

        if line.startswith("/* "):
          m = re.search(r"/\* (\w+):",line)
          if m:
            fun_context = m.group(1)

        if line.startswith("}"):
          fun_context = ""

        if ("return 0;" in line or "return d->unified_return_status;" in line) and fun_context=="solver" :
          s = "\tend_t = clock();\n"
          s+= "\tCASADI_PREFIX(stats).runtime = (casadi_real)(end_t - start_t) / CLOCKS_PER_SEC;\n"
          line = s+line

        if "Add prefix to internal symbol" in line:
          line = write_struct(Struct("solver_stats", solver_stats_type.fields))+"\n"+line
        # Bugfix https://gitlab.kuleuven.be/meco/projects/sbo_dirac/dirac_mpc/-/issues/11
        if "return fmax(x, y);" not in line and "CASADI_PREFIX" not in line:
          line = re.sub(r"\bfmax\b","casadi_fmax", line)
        # Bugfix https://github.com/casadi/casadi/issues/2835

        m=re.search(r"\bcasadi_f\d+_checkout\b",line)
        if m:
          increfs.append(m.group(0))

        if "ocpfun_checkout" in line and not modern_casadi():
          fout.write(line)
          line = " ".join([e+"();" for e in increfs])
          
        if "casadi_int A_colind" in line:
          line = line.replace("casadi_int","c_int")

        if "/* Solve the QP */" in line:
          qp = True

        if "if (grampc_driver(arg, res, iw, w, mem)) return 1;" in line:
          line = "mem = grampc_driver_checkout();\n" + line + "grampc_driver_release(mem);\n"
        
        if "/* Detecting indefiniteness */" in line:
          qp = False

        if qp_error_on_fail:
          if ("d.dlam+" in line or "d->dlam+" in line) and "res" in line:
            line = line + "int flag="

          if "Detecting indefiniteness" in line:
            line = "if (flag) return -100;\n" + line

        if "#include <math.h>" in line:
          line = "#include <time.h>\n" + line

        if False and qp and "casadi_f" in line:
          indent = line[:len(line)-len(line.lstrip())]
          line = indent + "if (" + line.strip()[:-1] + ") return 1;\n"

        if "static const casadi_int casadi_s0" in line:
          if "Grampc" in str(ocp._method):
            line = """
        typedef struct {
            int sqp_stop_crit;
            int n_sqp_iter;
            int n_ls;
            int n_max_ls;
            int n_qp_iter;
            int n_max_qp_iter;
            double runtime;
        } compat_solver_stats;
        static solver_stats CASADI_PREFIX(stats);
        const compat_solver_stats* grampc_driver_get_stats(void);
        int grampc_driver_checkout(void);
        void grampc_driver_release(int);

        const solver_stats* ocpfun_stats() { 
          const compat_solver_stats* s = grampc_driver_get_stats();
          CASADI_PREFIX(stats).sqp_stop_crit = s->sqp_stop_crit;
          CASADI_PREFIX(stats).n_sqp_iter = s->n_sqp_iter;
          CASADI_PREFIX(stats).n_qp_iter = s->n_qp_iter;
          CASADI_PREFIX(stats).runtime = s->runtime;
          return &CASADI_PREFIX(stats);
        }
            """ + line
          else:
            line = "static solver_stats CASADI_PREFIX(stats);\n" + \
                  "const solver_stats* ocpfun_stats() { return &CASADI_PREFIX(stats); }\n"+ \
                  line
        
        # if "struct casadi_sqpmethod_prob p;" in line:
          # line = "clock_t start_t, end_t;\nstart_t=clock();\n" + line

        struct_strings = ["struct casadi_sqpmethod_prob p;",
                          "struct casadi_feasiblesqpmethod_prob p;",
                          "struct casadi_ipopt_prob p;",
                          "struct casadi_fatrop_prob p;"]

        if any(s in line for s in struct_strings):
          line = line + "\tclock_t start_t, end_t;\n\tstart_t=clock();\n"

        if "MAIN OPTIMIZATION LOOP" in line:
          s=""
          for f in solver_stats_type.fields:
            s += f"CASADI_PREFIX(stats).{f.name} = 0;\n"
          line = s + line
        if "Formulate the QP" in line:
          line = "CASADI_PREFIX(stats).n_sqp_iter+=1;" + "\n"+line

        if "iter_count" in line:
          m = re.search(r"if \(iter_count >= (\d+)\) break;",line)
          if m:
            line = f"if (iter_count >= {m.group(1)}) {{ CASADI_PREFIX(stats).sqp_stop_crit = 1; break; }}\n"
          
        if "Candidate accepted, update dual variables" in line:
          line = "CASADI_PREFIX(stats).n_ls += ls_iter;" + "\n"+\
          "if (ls_iter>CASADI_PREFIX(stats).n_max_ls) CASADI_PREFIX(stats).n_max_ls = ls_iter;" + "\n"+line

        if "Get solution" in line:
          line = "CASADI_PREFIX(stats).n_qp_iter += d.iter;" + "\n"+\
          "if (d.iter>CASADI_PREFIX(stats).n_max_qp_iter) CASADI_PREFIX(stats).n_max_qp_iter = d.iter;" + "\n"+line

        fout.write(line)

    with open(name_h,'w') as fout:
      for line in lines_h:
        if "ocpfun(" in line:
          line = write_struct(Struct("solver_stats", solver_stats_type.fields))+"\n"+\
          "const solver_stats* ocpfun_stats();" + "\n" + \
          "\n"+line
        fout.write(line)
      fout.write("")

  def opti_x(self):
    return self._method.opti.x

  def opti_f(self):
    return self._method.opti.f

  def opti_g(self):
    return self._method.opti.g

  def opti_lbg(self):
    return self._method.opti.lbg

  def opti_ubg(self):
    return self._method.opti.ubg

  def opti_lam_g(self):
    return self._method.opti.lam_g

  def opti_p(self):
    return self._method.opti.p

  def save(self,name):
      self._untranscribe()
      import pickle
      with rockit_pickle_context():
          pickle.dump(self,open(name,"wb"))

  def export(self, name, src_dir=".", use_codegen=None, context=None, ignore_errors=False, short_output=True, qp_error_on_fail=True, c_flags=["-O3"], add_simulator=False, compile=True, mode='standard', ros2=False, ros2_options={}):
    build_dir_rel = name+"_build_dir"
    build_dir_abs = os.path.join(os.path.abspath(src_dir),build_dir_rel)

    # optimization_compilation_flags = ['-O3', '-march=native'] if mode == 'release' else ['-g']
    if mode == 'standard':
       optimization_compilation_flags = ['-O1']
    elif mode == 'debug':
       optimization_compilation_flags = ['-g']
    elif mode == 'release':
      optimization_compilation_flags = ['-O3']
    elif mode == 'release_native':
      optimization_compilation_flags = ['-O3', '-march=native']

    optimization_compilation_flags_str = ""
    for elem in optimization_compilation_flags:
      optimization_compilation_flags_str += "'" + elem + "' "

    prepare_build_dir(build_dir_abs)

    artifacts = list(self._method.artifacts)

    for e in artifacts:
      shutil.copy(os.path.join(self._method.build_dir_abs, e.name), build_dir_abs)
    
    # print(self._stages)
    # for stage in self._stages:
    #   print(stage.x)
    #   print(stage.z)
    #   print(stage.u)
    #   print(stage.p)
    #   print(stage.parameters)

    # print("------ OCP ------")
    # print(self.x)
    # print(self.z)
    # print(self.u)
    # print(self.p)
    # print(self.parameters)

    ############################################################
    # Towards multi-stage
    ############################################################

    # for stage in self._stages:
    #   print(stage.x)
    #   print(stage.z)
    #   print(stage.u)

    #   [_,stage_states] = stage.sample(stage.x,grid='control')
    #   [_,stage_algebraics] = stage.sample(stage.z,grid='control')
    #   [_,stage_controls] = stage.sample(stage.u,grid='control-')

    #   print(stage_states.shape)
    #   exit()

    #   variables = []
    #   variables_symbols = self.variables['']+self.variables['control']+self.variables['control+']
    #   for v in self.variables['']:
    #     variables.append(self.value(v))
    #   for v in self.variables['control']:
    #     variables.append(self.sample(v,grid='control-')[1])
    #   for v in self.variables['control+']:
    #     variables.append(self.sample(v,grid='control')[1])

    # parameters_symbols = self.parameters['']+self.parameters['control']+self.parameters['control+']
    # parameters = []
    # for p in self.parameters['']:
    #   parameters.append(self.value(p))
    # for p in self.parameters['control']:
    #   parameters.append(self.sample(p,grid='control-')[1])
    # for p in self.parameters['control+']:
    #   parameters.append(self.sample(p,grid='control')[1])
    

    ############################################################
    # Original
    ############################################################
    [_,states] = self.sample(self.x,grid='control')
    [_,algebraics] = self.sample(self.z,grid='control')
    [_,controls] = self.sample(self.u,grid='control-')
    parameters_symbols = self.parameters['']+self.parameters['control']+self.parameters['control+']
    parameters = []
    for p in self.parameters['']:
      parameters.append(self.value(p))
    for p in self.parameters['control']:
      parameters.append(self.sample(p,grid='control-')[1])
    for p in self.parameters['control+']:
      parameters.append(self.sample(p,grid='control')[1])
    variables = []
    variables_symbols = self.variables['']+self.variables['control']+self.variables['control+']
    for v in self.variables['']:
      variables.append(self.value(v))
    for v in self.variables['control']:
      variables.append(self.sample(v,grid='control-')[1])
    for v in self.variables['control+']:
      variables.append(self.sample(v,grid='control')[1])
    ############################################################

    casadi_fun_name = 'ocpfun'
    is_coll = False
    if hasattr(self._method, "Xc"):
      is_coll = True

    lam_g = self._method.opti.lam_g
    parameter_names = [p.name() for p in parameters_symbols]
    variable_names = [v.name() for v in variables_symbols]

    hotstart_symbol = lam_g
    hotstart_symbol = MX(0,1)

    grid = self.sample(self.t, grid='control')[1]

    ocpfun = self.to_function(casadi_fun_name,
      [states]+(["z"] if is_coll else [MX()])+[controls]+[vvcat(variables)]+parameters+[hotstart_symbol],
      [states,algebraics,controls,vvcat(variables),hotstart_symbol, grid],
      ['x0','z0','u0','v0'] + parameter_names + ['hotstart_in'],
      ['x','z','u','v'] + ['hotstart_out']+['grid'])

    casadi_codegen_file_name_base = name+"_codegen.c"
    casadi_codegen_file_name = os.path.join(build_dir_abs,casadi_codegen_file_name_base)


    if not self._state_next:
      if self.nz>0:
        sim = self.sys_simulator(intg="collocation",intg_options={"simplify":True,"rootfinder":"fast_newton"})
      else:
        sim = self.sys_simulator(intg="rk",intg_options={"simplify":True})
      sim_args = {}
      args = []
      label_in = []
      args.append(self.x)
      label_in.append("x")
      sim_args["x"] = self.x

      args.append(self.u)
      label_in.append("u")
      sim_args["u"] = self.u
      sim_p = []
      for p,appearing in zip(parameters_symbols,self.is_parameter_appearing_in_sys()):
        if appearing:
          sim_p.append(p)
          label_in.append(p.name()) # possible solution for #9
      sim_p = vvcat(sim_p)
      if sim_p.numel()>0:
        args.append(sim_p)
        label_in.append("p")
      sim_args["p"] = sim_p

      if self.is_sys_time_varying():
        args.append(self.t)
        label_in.append("t")
        sim_args["t0"] = self.t
      dt = MX.sym("dt")
      args.append(dt)
      label_in.append("dt")
      sim_args["dt"] = dt

      if self.nz>0:
        z_initial_guess = MX.sym("z_initial_guess",self.nz)
        args.append(z_initial_guess)
        label_in.append("z_initial_guess")
        sim_args["z_initial_guess"] = z_initial_guess

      sim_out = sim(**sim_args)
      outs = [sim_out["xf"]]
      labels_out = ["xf"]
      if self.nz>0:
        outs.append(sim_out["zf"])
        labels_out.append("zf")
      
      if add_simulator:
        mysim = Function('integrate_'+name, args, outs, label_in, labels_out)
        self.add_function(mysim)

    if not self._state_next:
      sys_dae = self.sys_dae()
      sys_dae_fun = Function('dae_'+name,sys_dae,["x","u","z","p","t"],["ode","alg"])
      self.add_function(sys_dae_fun)

    gridfun_options = {}
    if modern_casadi():
      gridfun_options["allow_free"] = True
    gridfun = Function("grid_"+name,
      parameters,
      [self.sample(self.t, grid='control')[1]],
      parameter_names,
      ['grid'], gridfun_options)
    if gridfun.has_free():
      gridfun = self.to_function("grid_"+name,
        parameters,
        [self.sample(self.t, grid='control')[1]],
        parameter_names,
        ['grid'])
    if not short_output:
      self.add_function(gridfun)

    costfun_options = {}
    if modern_casadi():
      costfun_options["allow_free"] = True

    costfun = Function("cost_"+name,
      [states]+[controls]+parameters,
      [self._method.opti.f],
      ['x','u'] + parameter_names,
      ['f'],costfun_options
    )
    if costfun.has_free():
      print("Cost function has stray dependencies")
    else:
      self.add_function(costfun)

    if use_codegen is None or use_codegen:
      options = {}
      options["with_header"] = True
      cg = CodeGenerator(name+"_codegen", options)
      f = io.StringIO()
      with redirect_stderr(f):
        cg.add(ocpfun)
        cg.generate(build_dir_abs+os.sep)
        data = cg.dump()
      f.seek(0)
      if "cannot be code generated" in f.read() or "#error" in data:
        if use_codegen is None:
          use_codegen = False
        else:
          raise Exception("use_codegen argument was True, but not supported.")
      else:
        use_codegen = True
        self.patch_codegen(casadi_codegen_file_name, self,qp_error_on_fail=qp_error_on_fail)
    print("use_codegen", use_codegen)
    casadi_file_name = os.path.join(build_dir_abs,name+".casadi")
    ocpfun.save(casadi_file_name)
    prefix = "impact_"

    def format_float(e):
      return "%0.18f" % e

    def strlist(a):
      elems = []
      for e in a:
        if isinstance(e,str):
          elems.append('"'+e+'"')
        elif isinstance(e,float):
          elems.append(format_float(e))
        else:
          elems.append(str(e))
      return ",".join(elems)


    pool_names = ["x_initial_guess","z_initial_guess","u_initial_guess","v_initial_guess","p","x_opt","z_opt","u_opt","v_opt","grid"]

    p_offsets = [0]
    for p in parameters:
      p_offsets.append(p_offsets[-1]+p.numel())

    v_offsets = [0]
    for v in variables:
      v_offsets.append(v_offsets[-1]+v.numel())

    p_nominal = self.initial_value(vvcat(parameters))
    if isinstance(p_nominal,float): p_nominal = np.array([p_nominal])
    x_nominal = self.initial_value(vec(states))
    if isinstance(x_nominal,float): x_nominal = np.array([x_nominal])
    z_nominal = self.initial_value(vec(algebraics))
    if isinstance(z_nominal,float): z_nominal = np.array([z_nominal])
    u_nominal = self.initial_value(vec(controls))
    if isinstance(u_nominal,float): u_nominal = np.array([u_nominal])
    v_nominal = self.initial_value(vvcat(variables))
    if isinstance(v_nominal,float): v_nominal = np.array([v_nominal])
    grid_nominal = self.initial_value(grid)
    if isinstance(grid_nominal,float): grid_nominal = np.array([grid_nominal])

    if isinstance(p_nominal,float): p_nominal = np.array([p_nominal])
    if isinstance(x_nominal,float): x_nominal = np.array([x_nominal])
    if isinstance(z_nominal,float): z_nominal = np.array([z_nominal])
    if isinstance(u_nominal,float): u_nominal = np.array([u_nominal])
    if isinstance(v_nominal,float): v_nominal = np.array([v_nominal])
    if isinstance(grid_nominal,float): grid_nominal = np.array([grid_nominal])

    p_names = parameter_names
    x_names = [x.name() for x in self.states]
    z_names = [z.name() for z in self.algebraics]
    u_names = [u.name() for u in self.controls]
    v_names = variable_names
    grid_names = ["t"]


    hello_p_normal_name = None
    hello_p_normal = None
    hello_p_normal_nominal = None
    if self.parameters['']:
      hello_p_normal = self.parameters[''][-1]
      hello_p_normal_name = hello_p_normal.name()
      hello_p_normal_nominal = self.initial_value(self.value(hello_p_normal))
      if isinstance(hello_p_normal_nominal,float): hello_p_normal_nominal = np.array([hello_p_normal_nominal])

    i_x_current = None
    count = 0

    p_part_offset = [0]
    p_part_unit = []
    for p_symbol,p_sampled in zip(parameters_symbols,parameters):
      if p_symbol.name()=="x_current":
        i_x_current = count
      count += 1
      p_part_unit.append(p_symbol.numel())
      p_part_offset.append(p_part_offset[-1]+p_sampled.numel())


    v_part_offset = [0]
    v_part_unit = []
    for v_symbol,v_sampled in zip(variables_symbols,variables):
      v_part_unit.append(v_symbol.numel())
      v_part_offset.append(v_part_offset[-1]+v_sampled.numel())

    if i_x_current is None:
      raise Exception("You must define a parameter named 'x_current'")

    x_current_nominal = self.initial_value(parameters[i_x_current])
    if isinstance(x_current_nominal,float): x_current_nominal = np.array([x_current_nominal])

    c_file_name_base = name+".c"
    c_file_name = os.path.join(build_dir_abs,c_file_name_base)
    h_file_name_base = name+".h"
    h_file_name = os.path.join(build_dir_abs,h_file_name_base)
    hello_file_name_base = "hello_world_"+name
    hello_file_name = os.path.join(build_dir_abs,hello_file_name_base)
    hello_c_file_name_base = hello_file_name_base+".c"
    hello_c_file_name = hello_file_name + ".c"
    with open(hello_c_file_name,"w") as out:
      out.write(f"""
        #include <{name}.h>
        #include <stdio.h>
        #include <assert.h>

        int main() {{
          {prefix}struct* m = initialize(printf, 0);
          if (!m) {{
            printf("Failed to initialize\\n");
            return 1;
          }}



          int n;

          double t[2] = {{42, 43}};
          
          n = set(m, "x_initial_guess", "scara.joint_vel", IMPACT_EVERYWHERE, t, IMPACT_HREP);
          assert(n==2);
          print_problem(m);
          t[0] = 3;t[1] = 7;
          n = set(m, "x_initial_guess", "scara.joint_vel", 2, t, IMPACT_HREP);
          assert(n==2);
          print_problem(m);

          double t4[4] = {{0.1, 0.2, 0.3, 0.4}};

          n = set(m, "x_initial_guess", IMPACT_ALL, 3, t4, IMPACT_HREP);
          assert(n==4);
          print_problem(m);

          double t44[44];
          for (int i=0;i<44;++i) {{
            t44[i] = i;
          }}
          n = set(m, "x_initial_guess", IMPACT_ALL, IMPACT_EVERYWHERE, t44, IMPACT_FULL);
          assert(n==44);
          print_problem(m);

          n = set(m, "x_initial_guess", IMPACT_ALL, IMPACT_EVERYWHERE, t44, IMPACT_FULL | IMPACT_ROW_MAJOR);
          assert(n==44);
          print_problem(m);

          n = set(m, "x_initial_guess", "scara.joint_vel", IMPACT_EVERYWHERE, t44, IMPACT_FULL | IMPACT_ROW_MAJOR);
          assert(n==22);
          print_problem(m);

          n = get(m, "x_initial_guess", "scara.joint_vel", 2, t, IMPACT_HREP);
          assert(n==2);
          assert(t[0]==2.0);
          assert(t[1]==13.0);
          n = get(m, "x_initial_guess", "scara.joint_vel", 2, t, IMPACT_FULL);
          assert(n==2);
          assert(t[0]==2.0);
          assert(t[1]==13.0);

          int n_row;
          int n_col;
          {prefix}get_size(m, "u_opt", IMPACT_ALL, &n_row, &n_col);
          printf("dims %d %d\\n",n_row,n_col);

          casadi_int sz_arg, sz_res, sz_iw, sz_w;



          work(m, &sz_arg, &sz_res, &sz_iw, &sz_w);
          printf("work %lld %lld %lld %lld\\n",sz_arg,sz_res,sz_iw,sz_w);


          solve(m);

          destroy(m);
        }}
      """
      )

    with open(hello_c_file_name,"w") as out:
      out.write(f"""
        #include <{name}.h>
        #include <stdio.h>
        #include <stdlib.h>
        #include <assert.h>

        int main() {{
          int i, j, n_row, n_col, flag;
          double *u_scratch, *x_scratch;
          {prefix}struct* m = impact_initialize(printf, 0);
          if (!m) {{
            printf("Failed to initialize\\n");
            return 1;
          }}
      """)
      if hello_p_normal_name:
        out.write(f"""
          /* Example: how to set a parameter */
          double val_{hello_p_normal_name}[{len(hello_p_normal_nominal)}] = {{{strlist(hello_p_normal_nominal)}}};
          impact_set(m, "p", "{hello_p_normal_name}", IMPACT_EVERYWHERE, val_{hello_p_normal_name}, IMPACT_FULL);

        """)
      out.write(f"""
          double x0[{x_current_nominal.size}] = {{ {",".join("%0.16f" % e for e in x_current_nominal)} }};

          impact_set(m, "x_current", IMPACT_ALL, IMPACT_EVERYWHERE, x0, IMPACT_FULL);

          double num = {"%0.16f" % x_current_nominal[0]};

          impact_set(m, "x_current", "{self.states[0].name()}", IMPACT_EVERYWHERE, &num, IMPACT_FULL);

          impact_print_problem(m);


          printf("Start a solve.\\n");
          flag = impact_solve(m);
          if (flag) {{
            printf("Solve indicates a problem: return code %d.\\n", flag);
          }}
          printf("Solve finished.\\n");

          const impact_stats* stats = impact_get_stats(m);
          if (stats) {{
            printf("Number of outer iterations: %d\\n", stats->n_sqp_iter);
            printf("Stop criterion: %d\\n", stats->sqp_stop_crit);
            printf("Runtime [s]: %e\\n", stats->runtime);
          }} else {{
            printf("No stats available.\\n");
          }}

          impact_print_problem(m);

          impact_hotstart(m);

          impact_print_problem(m);

          impact_solve(m);

          /* Allocate scratch space for state and control trajectories */
          impact_get_size(m, "u_opt", IMPACT_ALL, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
          printf("u_opt dims: %d - %d\\n", n_row, n_col);
          /* u_scratch = malloc(sizeof(double)*n_row*n_col); */
          u_scratch = malloc(sizeof(double)*n_row);

          impact_get_size(m, "x_opt", IMPACT_ALL, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
          printf("x_opt dims: %d - %d\\n", n_row, n_col);
          x_scratch = malloc(sizeof(double)*n_row*n_col);

          printf("Single OCP\\n");

          impact_get(m, "x_opt", IMPACT_ALL, IMPACT_EVERYWHERE, x_scratch, IMPACT_FULL);
          for (i=0;i<n_col;++i) {{
            for (j=0;j<n_row;++j) {{
              printf("%0.3e ", x_scratch[i*n_row+j]);
            }}
            printf("\\n");
          }}

          free(x_scratch);

          x_scratch = malloc(sizeof(double)*n_row);

          printf("Simulated MPC\\n");
  
          for (i=0;i<100;++i) {{
            impact_solve(m);
            impact_get(m, "u_opt", IMPACT_ALL, 0, u_scratch, IMPACT_FULL);
            impact_get(m, "x_opt", IMPACT_ALL, 1, x_scratch, IMPACT_FULL);
            impact_set(m, "x_current", IMPACT_ALL, 0, x_scratch, IMPACT_FULL);

            for (j=0;j<n_row;++j) {{
              printf("%0.3e ", x_scratch[j]);
            }}
            printf("\\n");

          }}
          free(u_scratch);
          free(x_scratch);

          impact_destroy(m);
        }}
      """
      )
    flags = OrderedDict([("ALL",0),("EVERYWHERE",-1),("FULL",0),("HREP",1),("VREP",2),("COLUMN_MAJOR",0),("ROW_MAJOR",4)])

    with open(h_file_name,"w") as out:
      for flag_name, flag_value in flags.items():
        out.write(f"#define IMPACT_{flag_name} {flag_value}\n")

      out.write(f"""

{write_struct(Struct(prefix+"stats", solver_stats_type.fields))}

struct {prefix}_struct;
typedef struct {prefix}_struct {prefix}struct;

typedef int (*formatter)(const char * s);

/**
  \\brief Initialize and instance of an impact library

  \\note Any call to \\c initialize must be followed up with a call to \\c destroy within the same process.

  \\param[in]  fp Function pointer to a printf-like function
              Common values: 0 (null pointer: no printing), printf (C's standard printf method defined in stdio)
  \\param[in]  build_dir Location of build directory. If not supplied (null pointer), uses a hard-coded export path.


  \\return Impact instance pointer, or a null pointer upon failure

*/
{prefix}struct* {prefix}initialize(formatter fp, const char* build_dir);
/**
  \\brief Destroy impact instance

  \\note May be needed to avoid segementation faults on exit
  \\note It is safe to pass a null pointer as argument

  \\parameter[in] Impact instance pointer
*/
void {prefix}destroy({prefix}struct* m);

/**
  \\brief Compute an MPC action

  \details
  Parameters (\\b p) and initial guesses (\\b x_initial_guess, \\b u_initial_guess, \\b z_initial_guess)
  can be supplied through \\c get.
  If not supplied, defaults will be taken from the user definition.
  Outputs of the optimization are written into internal pools \\b x_opt, \\b z_opt, and \\b u_opt,
  and can be queried using \\c get. 

  \\parameter[in] Hotstart

  \\return 0 indicates success

*/
int {prefix}solve({prefix}struct* m);

/*
Prepare internal structure such that the next solve is hotstarted with the current solution
*/
void {prefix}hotstart({prefix}struct* m);

/*
Prepare internal structure such that no hotstart information is carried over
*/
void {prefix}coldstart({prefix}struct* m);

/*
Prepare internal structure such that the next solve uses the nominal trajectory from export
*/
void {prefix}freshstart({prefix}struct* m);

/**
  \\brief Print numerical data present in pools

  \\parameter[in] Impact instance pointer

  \\return 0 indicates success

*/
int {prefix}print_problem({prefix}struct* m);

/** \\brief Getting and setting numerical data

  \\parameter[in] m            Impact instance pointer
  \\parameter[in] pool_name    Any of: { strlist(pool_names) }
  \\parameter[in] id           String identifier for a specific variable name, or IMPACT_ALL
  \\parameter[in] stage        Specific time instance (0-based), or IMPACT_EVERYWHERE
  \\parameter[in] flags    Composable (using bitwise or '|' ) flags that modify behaviour
                            IMPACT_FULL: default
                            IMPACT_COLUMN_MAJOR (fortran ordering - default)
                            IMPACT_ROW_MINOR (C ordering)
                            IMPACT_HREP (repeat horizontally )

  \\return Negative number upon failure

 */
 /** @{{ */
/**
  \\brief Get numerical data
  \\parameter[in] dst          Pointer to a chunk of writable memory.
                               There must be space of at least \\b n_row* \\b n_col which can be retrieved using \\c get_size
*/
int {prefix}get({prefix}struct* m, const char* pool_name, const char* id, int stage, double* dst, int flags);
/**
  \\brief Set numerical data
  \\parameter[in] src          Pointer to a chunk of readable memory.
                               There will be \\b n_row* \\b n_col elements read which can be retrieved using \\c get_size
                               Any NaNs are ignored (i.e. the memory pool is untouched for those entries)
*/
int {prefix}set({prefix}struct* m, const char* pool_name, const char* id, int stage, const double* src, int flags);
/**
  \\brief Get shape of data
*/
int {prefix}get_size({prefix}struct* m, const char* pool_name, const char* id, int stage, int flags, int* n_row, int* n_col);
/* @}} */

/**
  \\brief Get number of (scalarized) differential states
*/
int {prefix}get_nx();
/**
  \\brief Get number of (scalarized) algebric states
*/
int {prefix}get_nz();
/**
  \\brief Get number of (scalarized) inputs
*/
int {prefix}get_nu();
/**
  \\brief Get number of (scalarized) variables
*/
int {prefix}get_nv();

int {prefix}get_id_count({prefix}struct* m, const char* pool_name);
int {prefix}get_id_from_index({prefix}struct* m, const char* pool_name, int index, const char** id);
const {prefix}stats* {prefix}get_stats(const {prefix}struct* m);

#define casadi_real double
#define casadi_int long long int

int {prefix}allocate({prefix}struct* m);
void {prefix}set_work({prefix}struct* m, const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w);
void {prefix}work({prefix}struct* m, casadi_int* sz_arg, casadi_int* sz_res, casadi_int* sz_iw, casadi_int* sz_w);

int {prefix}flag_size({prefix}struct* m);
const char* {prefix}flag_name({prefix}struct* m, int index);
int {prefix}flag_value({prefix}struct* m, int index);

      """
      )


    x_part_offset = [0]
    x_part_unit = []
    for x in self.states:
      x_part_unit.append(x.numel())
      x_part_offset.append(x_part_offset[-1]+x.numel())

    z_part_offset = [0]
    z_part_unit = []
    for z in self.algebraics:
      z_part_unit.append(z.numel())
      z_part_offset.append(z_part_offset[-1]+z.numel())

    u_part_offset = [0]
    u_part_unit = []
    for u in self.controls:
      u_part_unit.append(u.numel())
      u_part_offset.append(u_part_offset[-1]+u.numel())
    
    grid_part_offset = [0,1]
    grid_part_unit = [1]

    p_trajectory_length = [1 for p in self.parameters['']]+[self._method.N for p in self.parameters['control']]+[self._method.N+1 for p in self.parameters['control+']]
    x_trajectory_length = [self._method.N+1 for x in self.states]
    z_trajectory_length = [self._method.N+1 for x in self.algebraics]
    u_trajectory_length = [self._method.N for x in self.controls]
    x_current_trajectory_length = [1 for x in self.states]
    v_trajectory_length = [1 for v in self.variables['']]+[self._method.N for v in self.variables['control']]+[self._method.N+1 for v in self.variables['control+']]
    grid_trajectory_length = [self._method.N+1]

    p_part_stride = p_part_unit
    x_part_stride = [self.nx for x in self.states]
    z_part_stride = [self.nz for x in self.algebraics]
    u_part_stride = [self.nu for u in self.controls]
    v_part_stride = v_part_unit
    grid_part_stride = grid_part_unit

    def array_size(n):
      if n==0: return 1
      return n

    def initialize_array(name,n,init):
      s = name+f"[{array_size(n)}]"
      if n==0:
        return s
      else:
        return s + " = {" + str(init) + "}"

    def casadi_call(funname,*args):
      method = f"casadi_c_{funname}_id"
      if use_codegen:
        if funname=="eval":
          method = f"{casadi_fun_name}"
        else:
          method = f"{casadi_fun_name}_{funname}"
      if not use_codegen:
        args = ("m->id",) + args
      return method + "(" + ",".join(args) + ")"

    with open(c_file_name,"w") as out:
      out.write(f"""
          #include <stdlib.h>
          #include <string.h>          
          {"#include " + '"' + name + "_codegen.h" + '"' if use_codegen else "#include <casadi/casadi_c.h>"}
          #include "{name}.h"

          typedef struct {prefix}_pool {{
            casadi_int n;
            casadi_int size;            
            const char** names; /* length n */
            const casadi_int* trajectory_length;  /*  length n */
            const casadi_int* part_offset;  /*  length n */
            const casadi_int* part_unit;  /*  length n a non-full may span multiple parts */
            const casadi_int* part_stride;  /*  length n a non-full may span multiple parts */
            casadi_real* data;
            casadi_int stride;
          }} {prefix}pool;

          typedef void (*fatal_fp)({prefix}struct* m, const char * loc, const char * fmt, ...);
          typedef void (*info_fp)({prefix}struct* m, const char * fmt, ...);

          struct {prefix}_struct {{
            int id;
            int pop; /*  need to pop when destroyed? */
            casadi_int n_in;
            casadi_int n_out;

            casadi_int sz_arg;
            casadi_int sz_res;
            casadi_int sz_iw;
            casadi_int sz_w;

            const casadi_real** arg;
            casadi_real** res;
            casadi_int* iw;
            casadi_real* w;

            const casadi_real** arg_casadi;
            casadi_real** res_casadi;
            casadi_int* iw_casadi;
            casadi_real* w_casadi;

            {prefix}pool* x_current;

            {prefix}pool* x_initial_guess;
            {prefix}pool* z_initial_guess;
            {prefix}pool* u_initial_guess;
            {prefix}pool* v_initial_guess;
            {prefix}pool* p;


            {prefix}pool* x_opt;
            {prefix}pool* z_opt;
            {prefix}pool* u_opt;
            {prefix}pool* v_opt;
            {prefix}pool* grid;

            casadi_real* hotstart_in;
            casadi_real* hotstart_out;

            int mem;

            formatter fp;
            fatal_fp fatal;
            info_fp info;
          }};

          /* For printing */
          #include <stdio.h>
          #include <stdarg.h>
""")
      # Write all data arrays
      prim = ["x","z","u","p","v"]
      def arrays():
        yield ("p_offsets","casadi_int")
        for p in prim: yield (f"{p}_part_offset","casadi_int")
        for p in prim: yield (f"{p}_part_unit","casadi_int")
        for p in prim: yield (f"{p}_part_stride","casadi_int")
        for p in prim: yield (f"{p}_names","char*")
        for p in prim: yield (f"{p}_trajectory_length","casadi_int")
        yield ("x_current_trajectory_length","casadi_int")
        for p in prim: yield (f"{p}_nominal","casadi_real")
        yield ("x_current_nominal","casadi_real")
        yield ("grid_names","char*")
        yield ("grid_nominal","casadi_real")
        yield ("grid_part_offset","casadi_int")
        yield ("grid_part_unit","casadi_int")
        yield ("grid_part_stride","casadi_int")
        yield ("grid_trajectory_length","casadi_int")

      for array_name, data_type in arrays():
        data = eval(array_name)
        out.write(f"          static const {data_type} "+ initialize_array(prefix+array_name,len(data),strlist(data))+";\n")

      out.write(f"""
          static const char* {prefix}pool_names[{array_size(len(pool_names))}] = {{ {strlist(pool_names)} }};

          static const char* {prefix}flag_names[{len(flags)}] = {{ {",".join('"%s"' % e for e in flags.keys())} }};
          static int {prefix}flag_values[{len(flags)}] = {{ {",".join("IMPACT_" + e for e in flags.keys())} }};

          int {prefix}flag_size({prefix}struct* m) {{
            return {len(flags)};
          }}
          const char* {prefix}flag_name({prefix}struct* m, int index) {{
            if (index<0 || index>={len(flags)}) return 0;
            return {prefix}flag_names[index];
          }}
          int {prefix}flag_value({prefix}struct* m, int index) {{
            if (index<0 || index>={len(flags)}) return 0;
            return {prefix}flag_values[index];
          }}



          void {prefix}fatal({prefix}struct* m, const char* loc, const char* fmt, ...) {{
            va_list args;
            char buffer[1024];
            if (m->fp) {{
              va_start(args, fmt);
              if (loc) {{
                sprintf(buffer, "Error in %s: ", loc);
                m->fp(buffer);
              }}
              vsprintf(buffer, fmt, args);
              m->fp(buffer);
              va_end(args);
            }}
          }}
          void {prefix}info({prefix}struct* m, const char* fmt, ...) {{
            va_list args;
            char buffer[1024];
            if (m->fp) {{
              va_start(args, fmt);
              vsprintf(buffer, fmt, args);
              m->fp(buffer);
              va_end(args);
            }}
          }}


          {prefix}struct* {prefix}initialize(formatter fp, const char* build_dir) {{
            int flag;
            int i;
            {prefix}struct* m;
            char casadi_file[2048];
            m = malloc(sizeof({prefix}struct));
            m->fp = fp;
            m->fatal = {prefix}fatal;
            m->info = {prefix}info;
            m->id = -1;""")
      if not use_codegen:
        out.write(f"""
            if (casadi_c_n_loaded()) {{ 
              m->id = casadi_c_id("{casadi_fun_name}");
              m->pop = 0;
            }}
            if (m->id<0) {{
              if (build_dir==0) {{
                strcpy(casadi_file, "{escape(build_dir_abs)}");
              }} else {{
                strcpy(casadi_file, build_dir);
              }}
              strcat(casadi_file, "/{name}.casadi");
              flag = casadi_c_push_file(casadi_file);
              m->pop = 1;
              if (flag) {{
                m->fatal(m, "initialize", "Could not load file '%s'.\\n", casadi_file);
                return 0;
              }}
              m->id = casadi_c_id("{casadi_fun_name}");
              if (m->id<0) {{
                m->fatal(m, "initialize", "Could not locate function with name '{casadi_fun_name}'.\\n");
                {prefix}destroy(m);
                return 0;
              }}
            }}""")
      out.write(f"""
            /* Allocate memory (thread-safe) */
            {casadi_call("incref")};
            /* Checkout thread-local memory (not thread-safe) */
            m->mem = {casadi_call("checkout")};

            {prefix}work(m, &m->sz_arg, &m->sz_res, &m->sz_iw, &m->sz_w);
            {prefix}set_work(m,
              malloc(sizeof(const casadi_real*)*m->sz_arg),
              malloc(sizeof(casadi_real*)*m->sz_res),
              malloc(sizeof(casadi_int)*m->sz_iw),
              malloc(sizeof(casadi_real)*m->sz_w));

            m->arg_casadi = m->arg;
            m->res_casadi = m->res;
            m->iw_casadi = m->iw;
            m->w_casadi = m->w;

            m->p = malloc(sizeof({prefix}pool));
            m->p->names = {prefix}p_names;
            m->p->size = {p_nominal.size};
            m->p->data = malloc(sizeof(casadi_real)*{max(p_nominal.size,1)});
            m->p->n = {len(parameters)};
            m->p->trajectory_length = {prefix}p_trajectory_length;
            m->p->stride = -1;
            m->p->part_offset = {prefix}p_part_offset;
            m->p->part_unit = {prefix}p_part_unit;
            m->p->part_stride = {prefix}p_part_stride;

            m->x_current = malloc(sizeof({prefix}pool));
            m->x_current->names = {prefix}x_names;
            m->x_current->size = {self.nx};
            m->x_current->data = m->p->data+m->p->part_offset[{i_x_current}];
            m->x_current->n = {len(self.states)};
            m->x_current->trajectory_length = {prefix}x_current_trajectory_length;
            m->x_current->stride = {self.nx};
            m->x_current->part_offset = {prefix}x_part_offset;
            m->x_current->part_unit = {prefix}x_part_unit;
            m->x_current->part_stride = {prefix}x_part_stride;
      """)

      for sname,d in [("x",{"nominal":x_nominal,"sym":self.states,"stride":self.nx}),
                     ("z",{"nominal":z_nominal,"sym":self.algebraics,"stride":self.nz}),
                     ("u",{"nominal":u_nominal,"sym":self.controls,"stride":self.nu}),
                     ("v",{"nominal":v_nominal,"sym":variables,"stride":-1})]:
        out.write(f"""
            m->{sname}_initial_guess = malloc(sizeof({prefix}pool));
            m->{sname}_initial_guess->names = {prefix}{sname}_names;
            m->{sname}_initial_guess->size = {d["nominal"].size};
            m->{sname}_initial_guess->data = malloc(sizeof(casadi_real)*{max(d["nominal"].size,1)});
            m->{sname}_initial_guess->n = {len(d["sym"])};
            m->{sname}_initial_guess->trajectory_length = {prefix}{sname}_trajectory_length;
            m->{sname}_initial_guess->stride = {d["stride"]};
            m->{sname}_initial_guess->part_offset = {prefix}{sname}_part_offset;
            m->{sname}_initial_guess->part_unit = {prefix}{sname}_part_unit;
            m->{sname}_initial_guess->part_stride = {prefix}{sname}_part_stride;

            m->{sname}_opt = malloc(sizeof({prefix}pool));
            m->{sname}_opt->names = {prefix}{sname}_names;
            m->{sname}_opt->size = {d["nominal"].size};
            m->{sname}_opt->data = malloc(sizeof(casadi_real)*{max(d["nominal"].size,1)});
            m->{sname}_opt->n = {len(d["sym"])};
            m->{sname}_opt->trajectory_length = {prefix}{sname}_trajectory_length;
            m->{sname}_opt->stride = {d["stride"]};
            m->{sname}_opt->part_offset = {prefix}{sname}_part_offset;
            m->{sname}_opt->part_unit = {prefix}{sname}_part_unit;
            m->{sname}_opt->part_stride = {prefix}{sname}_part_stride;
            """)

      out.write(f"""
            m->grid = malloc(sizeof({prefix}pool));
            m->grid->names = {prefix}grid_names;
            m->grid->size = {grid_nominal.size};
            m->grid->data = malloc(sizeof(casadi_real)*{max(grid_nominal.size,1)});
            m->grid->n = 1;
            m->grid->trajectory_length = {prefix}grid_trajectory_length;
            m->grid->stride = 1;
            m->grid->part_offset = {prefix}grid_part_offset;
            m->grid->part_unit = {prefix}grid_part_unit;
            m->grid->part_stride = {prefix}grid_part_stride;
          """)
        
      out.write(f"""
            m->hotstart_in = malloc(sizeof(casadi_real)*{max(hotstart_symbol.numel(),1)});
            m->hotstart_out = malloc(sizeof(casadi_real)*{max(hotstart_symbol.numel(),1)});

            memcpy(m->p->data, {prefix}p_nominal, {p_nominal.size}*sizeof(casadi_real));
            memcpy(m->x_opt->data, {prefix}x_nominal, {x_nominal.size}*sizeof(casadi_real));
            memcpy(m->z_opt->data, {prefix}z_nominal, {z_nominal.size}*sizeof(casadi_real));
            memcpy(m->u_opt->data, {prefix}u_nominal, {u_nominal.size}*sizeof(casadi_real));
            memcpy(m->v_opt->data, {prefix}v_nominal, {v_nominal.size}*sizeof(casadi_real));
            memcpy(m->grid->data, {prefix}grid_nominal, {grid_nominal.size}*sizeof(casadi_real));
            for (i=0;i<{hotstart_symbol.numel()};++i) m->hotstart_out[i] = 0.0;
            {prefix}freshstart(m);
            return m;
          }}

          void {prefix}coldstart({prefix}struct* m) {{
            int i;
            for (i=0;i<{hotstart_symbol.numel()};++i) m->hotstart_in[i] = 0.0;
          }}

          void {prefix}freshstart({prefix}struct* m) {{
            int i;
            memcpy(m->x_initial_guess->data, {prefix}x_nominal, {x_nominal.size}*sizeof(casadi_real));
            memcpy(m->z_initial_guess->data, {prefix}z_nominal, {z_nominal.size}*sizeof(casadi_real));
            memcpy(m->u_initial_guess->data, {prefix}u_nominal, {u_nominal.size}*sizeof(casadi_real));
            memcpy(m->v_initial_guess->data, {prefix}v_nominal, {v_nominal.size}*sizeof(casadi_real));
            for (i=0;i<{hotstart_symbol.numel()};++i) m->hotstart_in[i] = 0.0;
          }}

          void {prefix}hotstart({prefix}struct* m) {{
            int i;
            memcpy(m->x_initial_guess->data, m->x_opt->data, {x_nominal.size}*sizeof(casadi_real));
            memcpy(m->z_initial_guess->data, m->z_opt->data, {z_nominal.size}*sizeof(casadi_real));
            memcpy(m->u_initial_guess->data, m->u_opt->data, {u_nominal.size}*sizeof(casadi_real));
            memcpy(m->v_initial_guess->data, m->v_opt->data, {v_nominal.size}*sizeof(casadi_real));
            memcpy(m->hotstart_in, m->hotstart_out, {hotstart_symbol.numel()}*sizeof(casadi_real));
          }}

          void {prefix}destroy({prefix}struct* m) {{
            if (m) {{
              /* Free memory (thread-safe) */
              {casadi_call("decref")};
              /* Release thread-local (not thread-safe) */
              {casadi_call("release","m->mem")};
              {"" if use_codegen else "if (m->pop) casadi_c_pop();"}
              /*free(m->arg);
              free(m->res);
              free(m->iw);
              free(m->w);
              free(m->p->data);
              free(m->p);
              free(m->x_current);
              free(m->x_initial_guess->data);
              free(m->x_initial_guess);
              free(m->z_initial_guess->data);
              free(m->z_initial_guess);
              free(m->u_initial_guess->data);
              free(m->u_initial_guess);
              free(m->v_initial_guess->data);
              free(m->v_initial_guess);
              free(m->hotstart_in);
              free(m->hotstart_out);
              free(m->x_opt->data);
              free(m->x_opt);
              free(m->z_opt->data);
              free(m->z_opt);
              free(m->u_opt->data);
              free(m->u_opt);
              free(m->v_opt->data);
              free(m->v_opt);
              free(m->grid->data);
              free(m->grid);*/
              free(m);
            }}
          }}

          void {prefix}set_work({prefix}struct* m, const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w) {{
            m->arg = arg;
            m->res = res;
            m->iw = iw;
            m->w = w;
          }}

          void {prefix}work({prefix}struct* m, casadi_int* sz_arg, casadi_int* sz_res, casadi_int* sz_iw, casadi_int* sz_w) {{
            {casadi_call("work","sz_arg", "sz_res", "sz_iw", "sz_w")};
            /* We might want to be adding other working memory here */
          }}

          const {prefix}pool* {prefix}get_pool_by_name({prefix}struct* m, const char* name) {{
            if (!strcmp(name,"p")) {{
              return m->p;
            }} else if (!strcmp(name,"x_current")) {{
              return m->x_current;
            }} else if (!strcmp(name,"x_initial_guess")) {{
              return m->x_initial_guess;
            }} else if (!strcmp(name,"z_initial_guess")) {{
              return m->z_initial_guess;
            }} else if (!strcmp(name,"u_initial_guess")) {{
              return  m->u_initial_guess;
            }} else if (!strcmp(name,"v_initial_guess")) {{
              return  m->v_initial_guess;
            }} else if (!strcmp(name,"x_opt")) {{
              return m->x_opt;
            }} else if (!strcmp(name,"z_opt")) {{
              return m->z_opt;
            }} else if (!strcmp(name,"u_opt")) {{
              return m->u_opt;
            }} else if (!strcmp(name,"v_opt")) {{
              return m->v_opt;
            }} else if (!strcmp(name,"grid")) {{
              return m->grid;
            }} else {{
              m->fatal(m, "get_pool_by_name", "Pool with name '%s' not recognized. \
                                       Use one of: {pool_names}. \\n", name);
              return 0;
            }}
          }}


          int {prefix}get_id_count({prefix}struct* m, const char* pool) {{
            const {prefix}pool* p;
            if (!pool) {{
              m->fatal(m, "get_id_count (ret code -1)", "You may not pass a null pointer as pool. \\n");
              return -1;
            }}
            p = {prefix}get_pool_by_name(m, pool);
            if (!p) {{
              m->fatal(m, "get_id_count (ret code -2)", "Failed to find a pool named '%s'. \\n", pool);
              return -2;
            }}
            return p->n;
          }}

          int {prefix}get_id_from_index({prefix}struct* m, const char* pool, int index, const char** id) {{
            int i;
            const {prefix}pool* p;
            if (!pool) {{
              m->fatal(m, "get_id_from_index (ret code -1)", "You may not pass a null pointer as pool. \\n");
              return -1;
            }}
            p = {prefix}get_pool_by_name(m, pool);
            if (!p) {{
              m->fatal(m, "get_id_from_index (ret code -2)", "Failed to find a pool named '%s'. \\n", pool);
              return -2;
            }}
            if (index<0 || index>= p->n) {{
              m->fatal(m, "get_id_from_index (ret code -3)", "Index %d is out of bounds for pool %s: need [0,%d[. \\n", index, pool, p->n);
              return -3;
            }}
            *id = p->names[index];

            return 0;
          }}


          int {prefix}get_size({prefix}struct* m, const char* pool, const char* id, int stage, int flags, int* n_row, int* n_col) {{
            int index, i;
            const {prefix}pool* p;
            if (!pool) {{
              m->fatal(m, "get_size (ret code -1)", "You may not pass a null pointer as pool. \\n");
              return -1;
            }}
            p = {prefix}get_pool_by_name(m, pool);
            if (!p) {{
              m->fatal(m, "get_size (ret code -2)", "Failed to find a pool named '%s'. \\n", pool);
              return -2;
            }}

            /* Determine index */
            index = -1;
            if (id!=IMPACT_ALL) {{
              for (i=0;i<p->n;++i) {{
                if (!strcmp(id, p->names[i])) break;
              }}
              if (i==p->n) {{
                m->fatal(m, "get_id_from_index (ret code -4)", "Id '%s' not found for pool '%s'. Use one of these options: \\n", id, pool);
                for (i=0;i<p->n;++i) {{
                  m->fatal(m, 0, " - %s\\n", p->names[i]);
                }}
                return -4;
              }}
              index = i;
            }}

            if (index==-1) {{
              if (p->stride>=0) {{
                *n_row = p->stride;
                *n_col = p->trajectory_length[0];
              }} else {{
                *n_row = p->size;
                *n_col = 1;
              }}
            }} else {{
              *n_row = p->part_unit[index];
              *n_col = p->trajectory_length[index];
            }}

            if (stage!=IMPACT_EVERYWHERE) *n_col = 1;

            return 0;
          }}

          int {prefix}get_nx() {{ return {self.nx}; }}
          int {prefix}get_nz() {{ return {self.nz}; }}
          int {prefix}get_nu() {{ return {self.nu}; }}
          int {prefix}get_nv() {{ return {self.nv}; }}

          int {prefix}set_get({prefix}struct* m, const char* pool, const char* id, int stage, double* data, int data_flags, char mode) {{
            int i, j, k, index, i_start, i_stop, k_start, k_stop, offset, row, col, stride, data_i;
            const {prefix}pool* p;
            if (!pool) {{
              m->fatal(m, "set_get (ret code -1)", "You may not pass a null pointer as pool. \\n");
              return -1;
            }}
            if (!data) {{
              m->fatal(m, "set_get (ret code -2)", "You may not pass a null pointer for src/dst. \\n");
              return -2;
            }}
            p = {prefix}get_pool_by_name(m, pool);
            if (!p) {{
              m->fatal(m, "set_get (ret code -3)", "Failed to find a pool named '%s'. \\n", pool);
              return -3;
            }}

            /* Determine index */
            index = -1;
            if (id!=IMPACT_ALL) {{
              for (i=0;i<p->n;++i) {{
                if (!strcmp(id, p->names[i])) break;
              }}
              if (i==p->n) {{
                m->fatal(m, "set_get (ret code -4)", "Id '%s' not found for pool '%s'. Use one of these options: \\n", id, pool);
                for (i=0;i<p->n;++i) {{
                  m->fatal(m, 0, " - %s\\n", p->names[i]);
                }}
                return -4;
              }}
              index = i;
            }}

            i_start = index==-1? 0 : index;
            i_stop  = index==-1? p->n : index+1;
            k_start = stage==IMPACT_EVERYWHERE? 0 : stage;
            offset = 0;
            data_i = -1;
            for (i=i_start;i<i_stop;++i) {{
              row = id==IMPACT_ALL ? p->part_offset[i] : 0;
              if (data_flags & IMPACT_ROW_MAJOR) {{
                stride = p->trajectory_length[i];
              }} else {{
                stride = id==IMPACT_ALL ? p->part_stride[i] : p->part_unit[i];
              }}
              for (j=0;j<p->part_unit[i];++j) {{
                k_stop  = stage==IMPACT_EVERYWHERE? p->trajectory_length[i] : stage+1;
                for (k=k_start;k<k_stop;++k) {{
                  col = (data_flags & IMPACT_HREP || stage!=IMPACT_EVERYWHERE) ? 0 : k;
                  data_i = data_flags & IMPACT_ROW_MAJOR ? stride*row + col : row + stride*col;
                  if (mode) {{
                    data[data_i] = p->data[j+ p->part_offset[i] + p->part_stride[i]*k];
                  }} else {{
                    /* skip nan */
                    if (data[data_i]==data[data_i]) p->data[j+ p->part_offset[i] + p->part_stride[i]*k] = data[data_i];
                  }}
                }}
                row++;
              }}
            }}
            return data_i+1;
          }}

          int {prefix}set({prefix}struct* m, const char* pool, const char* id, int stage, const double* src, int src_flags) {{
            return {prefix}set_get(m, pool, id, stage, (double*) src, src_flags, 0);
          }}

          int {prefix}get({prefix}struct* m, const char* pool, const char* id, int stage, double* dst, int dst_flags) {{
            return {prefix}set_get(m, pool, id, stage, dst, dst_flags, 1);
          }}

          int {prefix}get_pool(const {prefix}pool* p, casadi_real* value) {{
            if (!value) return 1;
            memcpy(value, p->data, p->size*sizeof(casadi_real));
            return 0;
          }}

          int {prefix}print_problem({prefix}struct* m) {{
            int i,j,k,l,max_len;
            char formatbuffer[10];
            const {prefix}pool* p;
            if (!m->fp) return 1;
            max_len=0;
            for (l=0;l<5;++l) {{
              p = {prefix}get_pool_by_name(m, {prefix}pool_names[l]);
              for (i=0;i<p->n;++i) {{
                max_len = strlen(p->names[i])>max_len? strlen(p->names[i]) : max_len;
              }}
            }}

            if (m->fp) {{
              for (l=0;l<5;++l) {{
                const {prefix}pool* p = {prefix}get_pool_by_name(m, {prefix}pool_names[l]);
                m->info(m, "=== %s ===\\n", {prefix}pool_names[l]);
                sprintf(formatbuffer, "%%-%ds", max_len);
                for (i=0;i<p->n;++i) {{
                  m->info(m, formatbuffer, p->names[i]);
                  m->info(m, ":");
                  for (j=0;j<p->part_unit[i];++j) {{
                    if (j>0) {{
                        m->info(m, formatbuffer, "");
                        m->info(m, " ");
                    }}
                    for (k=0;k<p->trajectory_length[i];++k) {{
                      m->info(m, " %.4e", p->data[j+ p->part_offset[i] + p->part_stride[i]*k]);
                    }}
                    m->info(m, "\\n");
                  }}
                }}
              }}
            }}
            return 0;
          }}

          int {prefix}solve({prefix}struct* m) {{
            int i;
            m->arg_casadi[0] = m->x_initial_guess->data;
            m->arg_casadi[1] = m->z_initial_guess->data;
            m->arg_casadi[2] = m->u_initial_guess->data;
            m->arg_casadi[3] = m->v_initial_guess->data;
            for (i=0;i<{len(parameters)};i++) {{
              m->arg_casadi[4+i] = m->p->data + {prefix}p_offsets[i];
            }}
            m->arg_casadi[4+{len(parameters)}] = m->hotstart_in;
            m->res_casadi[0] = m->x_opt->data;
            m->res_casadi[1] = m->z_opt->data;
            m->res_casadi[2] = m->u_opt->data;
            m->res_casadi[3] = m->v_opt->data;
            m->res_casadi[4] = m->hotstart_out;
            m->res_casadi[5] = m->grid->data;
            return {casadi_call("eval","m->arg_casadi","m->res_casadi","m->iw_casadi","m->w_casadi","m->mem")};
          }}

          int {prefix}get_p_by_id({prefix}struct* m, const char* id, const casadi_real* value) {{
            return 0;
          }}

          void {prefix}get_u({prefix}struct* m, double* value) {{

          }}

          const {prefix}stats* {prefix}get_stats(const {prefix}struct* m) {{
            return {casadi_call("stats") if use_codegen else 0};
          }}

        """)

    lib_name = name
    lib_file_name = os.path.join(build_dir_abs,"lib" + lib_name + ".so")

    s_function_name = name+"_s_function_level2"

    s_function_file_name_base = s_function_name+".c"
    s_function_file_name = os.path.join(build_dir_abs,s_function_file_name_base)

    make_cfg_name = s_function_name+"_makecfg"

    make_cfg_file_name_base = make_cfg_name+".m"
    make_cfg_file_name = os.path.join(build_dir_abs,make_cfg_file_name_base)
    

    with open(make_cfg_file_name,"w") as out:
      out.write(f"""
      function {make_cfg_name}(objBuildinfo)
      """)
      for e in artifacts:
        if isinstance(e, SourceArtifact):
          if e.reldir=="":
            reldir = "$(START_DIR)"
          else:
            reldir = f'$(START_DIR)/{e.reldir}'
          out.write(f"""
          addSourceFiles(objBuildinfo,'{e.name}','{reldir}');
          """)
        if isinstance(e, HeaderDirectory):
          out.write(f"""
          addIncludePaths(objBuildinfo,['$(START_DIR)/{e.dir}'])
          """)

      out.write(f"""
      addIncludePaths(objBuildinfo,casadi.GlobalOptions.getCasadiIncludePath());
      addDefines(objBuildinfo,'-DLA_HIGH_PERFORMANCE=ON','OPTS');
      addDefines(objBuildinfo,'-DBLASFEO_REF_API=ON','OPTS');
      addDefines(objBuildinfo,'-DMF_PANELMAJ=ON','OPTS');  
      addDefines(objBuildinfo,'-DK_MAX_STACK=300','OPTS');       
      addDefines(objBuildinfo,'-DUSE_C99_MATH=ON','OPTS');
      addDefines(objBuildinfo,'-DEXT_DEP=ON','OPTS');
      addDefines(objBuildinfo,'-DOS_LINUX=ON','OPTS');
      addDefines(objBuildinfo,'-DEXTERNAL_BLAS_NONE=ON','OPTS');
      addDefines(objBuildinfo,'-DTARGET_X64_INTEL_CORE=ON','OPTS');
      addDefines(objBuildinfo,'-DRELEASE=ON','OPTS');
      addDefines(objBuildinfo,'-DLEVEL1_DCACHE_LINE_SIZE=','OPTS');
      addDefines(objBuildinfo,'-DPANEL_SIZE=','OPTS');    
      """)
                
      out.write("end")

    with open(s_function_file_name,"w") as out:
      out.write(f"""
        #define S_FUNCTION_NAME {s_function_name}
        #define S_FUNCTION_LEVEL 2

        #include <{name}.h>

        #include "simstruc.h"

        static {prefix}struct* m;
        struct {{
          int sz_arg;
          int n_p;
        }} config;

        {write_struct(solver_stats_type,target="matlab_c")}

        void cleanup() {{
          if (m) {{
            {prefix}destroy(m);
          }}
        }}

        static void mdlInitializeSizes(SimStruct *S) {{
#ifdef MATLAB_MEX_FILE
            mexPrintf("mdlInitializeSizes\\n");
#endif
            int i;
            int n_row, n_col;
            const char* id;
            casadi_int sz_arg, sz_res, sz_iw, sz_w;
            m = 0;

            /* Simulink does not provide a cleanup-hook when parameters are changed */
            cleanup();

#ifdef MATLAB_MEX_FILE
            m = {prefix}initialize(mexPrintf, 0);
#else
            m = {prefix}initialize(0, 0);
#endif
            if (!m) {{
              cleanup();
#ifdef MATLAB_MEX_FILE
              mexErrMsgIdAndTxt( "MATLAB:s_function:invalidParameter",
                                "Failed to initialize.");
#else
              return;
#endif           
            }}
            {prefix}print_problem(m);

            /* Read work vector requirements */
            {prefix}work(m, &sz_arg, &sz_res, &sz_iw, &sz_w);

            /* Set up CasADi function work vector sizes */
            ssSetNumRWork(S, sz_w);
            ssSetNumIWork(S, sz_iw*sizeof(casadi_int)/sizeof(int_T));
            ssSetNumPWork(S, sz_arg+sz_res);

            int n_p = {prefix}get_id_count(m, "p");
            if (!ssSetNumInputPorts(S, n_p+{2+(self.nz>0)+(self.nv>0)}+1)) return;
            if (n_p<0) {{
              cleanup();
#ifdef MATLAB_MEX_FILE
              mexErrMsgIdAndTxt( "MATLAB:s_function:invalidParameter",
                                "Failure.");
#else
              return;
#endif            
            }}
            for (i=0;i<n_p;++i) {{
              {prefix}get_id_from_index(m, "p", i, &id);
              {prefix}get_size(m, "p", id, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
              ssSetInputPortDirectFeedThrough(S, i, 1);
              ssSetInputPortMatrixDimensions(S, i, n_row, n_col);
              ssSetInputPortRequiredContiguous(S, i, 1);
            }}

            {prefix}get_size(m, "x_initial_guess", IMPACT_ALL, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
            ssSetInputPortDirectFeedThrough(S, i, 1);
            ssSetInputPortMatrixDimensions(S, i, n_row, n_col);
            ssSetInputPortRequiredContiguous(S, i, 1);
            i++;""")

      if self.nz>0:
        out.write(f"""
            {prefix}get_size(m, "z_initial_guess", IMPACT_ALL, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
            ssSetInputPortDirectFeedThrough(S, i, 1);
            ssSetInputPortMatrixDimensions(S, i, n_row, n_col);
            ssSetInputPortRequiredContiguous(S, i, 1);
            i++;""")

      out.write(f"""
            {prefix}get_size(m, "u_initial_guess", IMPACT_ALL, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
            ssSetInputPortDirectFeedThrough(S, i, 1);
            ssSetInputPortMatrixDimensions(S, i, n_row, n_col);
            ssSetInputPortRequiredContiguous(S, i, 1);
            i++;
            """)

      if self.nv>0:
        out.write(f"""
            {prefix}get_size(m, "v_initial_guess", IMPACT_ALL, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
            ssSetInputPortDirectFeedThrough(S, i, 1);
            ssSetInputPortMatrixDimensions(S, i, n_row, n_col);
            ssSetInputPortRequiredContiguous(S, i, 1);
            i++;""")

      out.write(f"""
            // Port
            ssSetInputPortDirectFeedThrough(S, i, 1);
            ssSetInputPortMatrixDimensions(S, i, 1, 1);
            ssSetInputPortRequiredContiguous(S, i, 1);
            i++;
        """)
      

      
      def ports():
        yield (f"""{prefix}get_size(m, "x_opt", IMPACT_ALL, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
            ssSetOutputPortMatrixDimensions(S, i, n_row, {'1' if short_output else 'n_col'});""",
            f"""{prefix}get(m, "x_opt", IMPACT_ALL, {'1' if short_output else 'IMPACT_EVERYWHERE'}, ssGetOutputPortRealSignal(S, i++), IMPACT_FULL);""")
        if self.nz>0:
            yield (f"""
          {prefix}get_size(m, "z_opt", IMPACT_ALL, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
          ssSetOutputPortMatrixDimensions(S, i, n_row, n_col);""",
            f"""{prefix}get(m, "z_opt", IMPACT_ALL, IMPACT_EVERYWHERE, ssGetOutputPortRealSignal(S, i++), IMPACT_FULL);"""
            )
        yield (f"""
            {prefix}get_size(m, "u_opt", IMPACT_ALL, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
            ssSetOutputPortMatrixDimensions(S, i, n_row, {'1' if short_output else 'n_col'});
            """,
            f"""{prefix}get(m, "u_opt", IMPACT_ALL, {'0' if short_output else 'IMPACT_EVERYWHERE'}, ssGetOutputPortRealSignal(S, i++), IMPACT_FULL);""")
        
        if self.nv>0:
          yield (f"""
            {prefix}get_size(m, "v_opt", IMPACT_ALL, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
          ssSetOutputPortMatrixDimensions(S, i, n_row, n_col);
          """,
          f"""{prefix}get(m, "v_opt", IMPACT_ALL, IMPACT_EVERYWHERE, ssGetOutputPortRealSignal(S, i++), IMPACT_FULL);"""
            )

        if ignore_errors:
          yield (f"""
              ssSetOutputPortMatrixDimensions(S, i, 1, 1);
              """,
              """ssGetOutputPortRealSignal(S, i++)[0] = ret;"""
              )
          
        yield (f"""
#if MATLAB_MEX_FILE
            if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {{
                DTypeId tid;
                ssRegisterTypeFromNamedObject(S, "solver_stats_bus", &tid)
                if (tid==INVALID_DTYPE_ID) {{
                  mexErrMsgIdAndTxt( "MATLAB:s_function:invalidParameter",
                                    "Could not find solver_stats_bus data_type.");
                }}
                ssSetOutputPortDataType(S, i, tid);
            }}
#endif  
            ssSetOutputPortWidth(S, i, 1);
            ssSetOutputPortBusMode(S, i, SL_BUS_MODE);
            ssSetBusOutputObjectName(S, i, (void *) "{solver_stats_type.name}");
            ssSetBusOutputAsStruct(S, i, 1);
            """,
            f"""{solver_stats_type.name}* stats = ({solver_stats_type.name}*) ssGetOutputPortRealSignal(S, i++);
          const {prefix}stats* s = {prefix}get_stats(m);"""
            )
        if not short_output:
          yield (f"""
            {prefix}get_size(m, "grid", IMPACT_ALL, IMPACT_EVERYWHERE, IMPACT_FULL, &n_row, &n_col);
            ssSetOutputPortMatrixDimensions(S, i, n_row, n_col);
          """,
          f"""{prefix}get(m, "grid", IMPACT_ALL, IMPACT_EVERYWHERE, ssGetOutputPortRealSignal(S, i++), IMPACT_FULL);"""
          )
        
      out.write(f"""
            if (!ssSetNumOutputPorts(S, {len(list(ports()))})) return;
            i = 0;
            """)
      for init,_ in ports():
        out.write(f"""
            {init}
            i++;""")


      out.write(f"""
            ssSetNumSampleTimes(S, 1);
            
            ssSetNumNonsampledZCs(S, 0);

            /* specify the sim state compliance to be same as a built-in block */
            ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);

            /* Make sure mdlTerminate is called on error */
            ssSetOptions(S,
                        SS_OPTION_EXCEPTION_FREE_CODE |
                        SS_OPTION_USE_TLC_WITH_ACCELERATOR);
        }}

        /* Function: mdlInitializeSampleTimes =========================================
        * Abstract:
        *    Specifiy that we inherit our sample time from the driving block.
        */
        static void mdlInitializeSampleTimes(SimStruct *S)
        {{
#ifdef MATLAB_MEX_FILE
            mexPrintf("mdlInitializeSampleTimes\\n");
#endif
            ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);
            ssSetOffsetTime(S, 0, 0.0);
            ssSetModelReferenceSampleTimeDefaultInheritance(S); 
        }}

        static void mdlOutputs(SimStruct *S, int_T tid)
        {{
            if (!m) {{
#ifdef MATLAB_MEX_FILE
              mexPrintf("mdlOutputs (nullptr)\\n");
#endif
              return;
            }}
            void** p;
            const real_T** arg;
            double* w;
            casadi_int* iw;
            int_T i;
            const char* id;
            
#ifdef MATLAB_MEX_FILE
            mexPrintf("mdlOutputs %p\\n", m);
#endif
            /* Set up CasADi function work vectors */
            p = ssGetPWork(S);
            arg = (const real_T**) p;
            p += config.sz_arg;
            real_T** res = (real_T**) p;
            w = ssGetRWork(S);
            iw = (casadi_int*) ssGetIWork(S);

            {prefix}set_work(m, arg, res, iw, w);
            
            /* Point to input and output buffers */
            int n_p = {prefix}get_id_count(m, "p"); 
            for (i=0; i<n_p;++i) {{
              {prefix}get_id_from_index(m, "p", i, &id);
              {prefix}set(m, "p", id, IMPACT_EVERYWHERE, ssGetInputPortSignal(S,i), IMPACT_FULL);
            }}
            i = n_p;
            {prefix}set(m, "x_initial_guess", IMPACT_ALL, IMPACT_EVERYWHERE, ssGetInputPortSignal(S,i++), IMPACT_FULL);""")
      if self.nz>0:
        out.write(f"""
            {prefix}set(m, "z_initial_guess", IMPACT_ALL, IMPACT_EVERYWHERE, ssGetInputPortSignal(S,i++), IMPACT_FULL);""")

      out.write(f"""
            {prefix}set(m, "u_initial_guess", IMPACT_ALL, IMPACT_EVERYWHERE, ssGetInputPortSignal(S,i++), IMPACT_FULL);
            """)
      if self.nv>0:
        out.write(f"""
            {prefix}set(m, "v_initial_guess", IMPACT_ALL, IMPACT_EVERYWHERE, ssGetInputPortSignal(S,i++), IMPACT_FULL);""")

      out.write(f"""
            const real_T* hotstart_ptr = ssGetInputPortSignal(S,i++);
            real_T hotstart = *hotstart_ptr;

            if (hotstart==1.0) {{
              {prefix}hotstart(m);
            }} else if (hotstart==-1.0) {{
              {prefix}freshstart(m);
            }} else if (hotstart==0.0) {{
              {prefix}coldstart(m);
            }}

            int_T ret = {prefix}solve(m);
            {prefix}print_problem(m);
            if (ret && {int(not ignore_errors)}) {{
                static char msg[{200+len(s_function_name)}];
                sprintf(msg, "SFunction '{s_function_name}' failed to compute (error code %d) at t=%.6fs. "
                "Export with ('ignore_errors', true) if you want the simulation to continue anyway.", ret, ssGetT(S));
                #ifdef ssSetLocalErrorStatus
                  ssSetLocalErrorStatus(S, msg);
                #else
                  #ifdef ssSetErrorStatus
                    ssSetErrorStatus(S, msg);
                  #endif
                #endif
            }}

            i = 0;""")
      for _,update in ports():
        out.write(f"""
          {update}
        """)
          
      for f in solver_stats_type.fields:
        out.write(f"          stats->{f.name} = s ? s->{f.name} : 0;\n")

      out.write(f"""

        }}

        static void mdlStart(SimStruct *S) {{
#ifdef  MATLAB_MEX_FILE
          mexPrintf("mdlStart\\n");
#endif
        }}

        static void mdlTerminate(SimStruct *S) {{
#ifdef  MATLAB_MEX_FILE
          mexPrintf("mdlTerminate\\n");
#endif
          cleanup();
        }}


        #ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
        #include "simulink.c"      /* MEX-file interface mechanism */
        #else
        #include "cg_sfun.h"       /* Code generation registration function */
        #endif
        """)

    # Export helper functions
    added_functions = []
    for fun in self._added_functions:
      fun.save(os.path.join(build_dir_abs, fun.name()+".casadi"))
      added_functions.append(fun2s_function(fun, dir=build_dir_abs,ignore_errors=ignore_errors,build_dir_abs=build_dir_abs))

    m_build_file_name = os.path.join(build_dir_abs,"build.m")

    # If it were not for CASADI_PRINTF=mexPrintf (and perphaps PATH things on Windows), weould go for something much simpler:
    with open(m_build_file_name,"w") as out:
      out.write(f"""
        s_function_file_name_base = '{s_function_file_name_base}';
        [build_dir,~,~] = fileparts(mfilename('fullpath'));
        flags={{['-I' build_dir] ['-L' build_dir] '-DCASADI_PRINTF=mexPrintf' '-l{name}'}};
        files = {{[build_dir filesep s_function_file_name_base]}};
        """)

      if use_codegen:
        ipopt = ''
        fatrop = ''

        if CURRENT_CASADI_VERSION>=version.parse("3.6.3"):          
          ipopt = "'-lipopt'"
          if casadi.has_nlpsol("fatrop") and casadi.has_nlpsol("blasfeo"):
            fatrop = "'-lfatrop -lblasfeo'"

        out.write(f"""
          files=[files {{ [build_dir filesep casadi_codegen_file_name_base]}}];
          flags=[flags {{['-I' casadi.GlobalOptions.getCasadiIncludePath()] ['-I' casadi.GlobalOptions.getCasadiPath()] '-losqp' {ipopt} {fatrop}}}];
          """)
      else:
        out.write(f"""
          flags=[flags {{'-lcasadi'}}];
          """)
      out.write(f"""
      if ispc
      else
        flags=[flags {{{optimization_compilation_flags_str} ['LDFLAGS=\"\$LDFLAGS -Wl,-rpath,' casadi.GlobalOptions.getCasadiPath() ' -Wl,-rpath,' build_dir '\"']}}];
      end
      mex(flags{{:}},files{{:}});\n""")
      for f_name in added_functions:
        out.write(f"mex(flags{{:}}, [build_dir filesep '{f_name}']);\n")

    if False:
      with open(m_build_file_name,"w") as out:
        out.write("""
        try
          casadi.MX;
        catch
          error('This block depends on CasADi. Obtain it from install.casadi.org and make sure to have the casadi directory added to the matlab path.');
        end

        """)
        out.write(f"""
          s_function_file_name_base = '{s_function_file_name_base}';
          c_file_name_base = '{c_file_name_base}';
          casadi_codegen_file_name_base = '{casadi_codegen_file_name_base}';
          [build_dir,~,~] = fileparts(mfilename('fullpath'));
          flags={{['-I' build_dir] ['-L' build_dir] '-DCASADI_PRINTF=mexPrintf' ['-I' casadi.GlobalOptions.getCasadiIncludePath()] ['-L' casadi.GlobalOptions.getCasadiPath()]}};
          files = {{[build_dir filesep s_function_file_name_base], [build_dir filesep c_file_name_base]}};
          """)
        for e in artifacts:
          if isinstance(e, HeaderDirectory):
            out.write(f"""           flags = [flags {{['-I' build_dir filesep '{e.dir}']}}];\n""")
          if isinstance(e, LibraryArtifact):
            out.write(f"""           flags = [flags {{['-l{e.basename}']}}];\n""")
        for e in artifacts:
          if isinstance(e, SourceArtifact):
            out.write(f"""           files = [files {{[build_dir filesep '{e.relative}']}}];\n""")
        if use_codegen:

          ipopt = ''
          if modern_casadi():
            ipopt = "'-lipopt' '-lfatrop' '-lblasfeo'"
          out.write(f"""
            files=[files {{ [build_dir filesep casadi_codegen_file_name_base]}}];
            flags=[flags {{['-I' casadi.GlobalOptions.getCasadiIncludePath()] ['-I' casadi.GlobalOptions.getCasadiPath()] '-losqp' {ipopt}}}];
            """)
        else:
          out.write(f"""
            flags=[flags {{'-lcasadi'}}];
            """)
        out.write(f"""
        if ispc
        else
          flags=[flags {{'-g' ['LDFLAGS=\"\$LDFLAGS -Wl,-rpath,' casadi.GlobalOptions.getCasadiPath() ' -Wl,-rpath,' build_dir '\"']}}];
        end
        mex(flags{{:}},files{{:}});\n""")
        for f_name in added_functions:
          out.write(f"mex(flags{{:}}, [build_dir filesep '{f_name}']);\n")

    shutil.copy(os.path.join(self.basename,"templates","python","impact.py"), os.path.join(build_dir_abs,"impact.py"))
    py_file_name = os.path.join(build_dir_abs,"hello_world_" + name+".py")

      
    with open(py_file_name,"w") as out:
      out.write(f"""

import pylab as plt
import numpy as np

from impact import Impact

impact = Impact("{name}",src_dir="..")
""")

      if hello_p_normal_name:
        out.write(f"""
# Example: how to set a parameter
val_{hello_p_normal_name} = [{strlist(hello_p_normal_nominal)}]
impact.set("p", "{hello_p_normal_name}", impact.EVERYWHERE, impact.FULL, val_{hello_p_normal_name})
        """)
      out.write(f"""
print("Solve a single OCP (default parameters)")
impact.solve()

# Get {self.states[0].name()} solution trajectory
print(impact.get("x_opt", "{self.states[0].name()}", impact.EVERYWHERE, impact.FULL))

# Get solution trajectory
x_opt = impact.get("x_opt", impact.ALL, impact.EVERYWHERE, impact.FULL)

# Plotting


_, ax = plt.subplots(2,1,sharex=True)
ax[0].plot(x_opt.T)
ax[0].set_title('Single OCP')
ax[0].set_xlabel('Sample')

print("Running MPC simulation loop")

history = []
for i in range(100):
  impact.solve()

  # Optimal input at k=0
  u = impact.get("u_opt", impact.ALL, 0, impact.FULL)

  # Simulate 1 step forward in time
  # (TODO: use simulation model other than MPC model)
  x_sim = impact.get("x_opt", impact.ALL, 1, impact.FULL)

  # Update current state
  impact.set("x_current", impact.ALL, 0, impact.FULL, x_sim)
  history.append(x_sim)

# More plotting
ax[1].plot(np.hstack(history).T)
ax[1].set_title('Simulated MPC')
ax[1].set_xlabel('Sample')
plt.show()



      """)

    simulink_library_name = "library_"+name
    simulink_library_dirname = os.path.join(build_dir_abs,simulink_library_name)
    simulink_library_filename = simulink_library_dirname+".slx"

    template = os.path.join(self.basename,"templates","simulink","lib2016b.slx")
    diag = Diagram(template,simulink_library_filename,simulink_library_dirname=simulink_library_dirname)

    port_labels_in = []
    for p in parameters_symbols:
      port_labels_in.append(p.name())
    port_labels_in.append('x_initial_guess')

    if self.nz>0:
      port_labels_in.append('z_initial_guess')
    port_labels_in.append('u_initial_guess')
    if self.nv>0:
      port_labels_in.append('v_initial_guess')
    port_labels_in.append('hotstart')
    mask_name = f"IMPACT MPC\\n{name}"
    port_labels_out = []
    port_labels_out.append("x_opt @ k=1" if short_output else "x_opt")
    if self.nz>0:
      port_labels_out.append("z_opt")
    port_labels_out.append("u_opt @ k=0" if short_output else "u_opt")
    if self.nv>0:
      port_labels_out.append("v_opt")
    if ignore_errors:
      port_labels_out.append("status code (0=good)")
    port_in = []
    for p in parameters_symbols:
      port_in.append(f"NaN({p.shape[0]},{p.shape[1]})")

    port_in.append(f"NaN({self.nx},{self._method.N+1})")
    if self.nz>0:
      port_in.append(f"NaN({len(self.algebraics)},{self._method.N+1})")
    port_in.append(f"NaN({self.nu},{self._method.N})")
    if self.nv>0:
      port_in.append(f"NaN({v_nominal.size},1)")
    port_in.append(f"0")


    port_labels_out.append("solver stats")
    dependencies = f"""
    try
      casadi.load_nlpsol('ipopt')
      casadi.load_nlpsol('fatrop')
      casadi.load_conic('osqp')
    catch
      error('This block depends on CasADi >= 3.6.7 - Obtain it from install.casadi.org and make sure to have the casadi directory added to the matlab path.');
    end

    """

    if not short_output:
      port_labels_out.append("grid")
    mask = Mask(port_labels_in=port_labels_in, port_labels_out=port_labels_out,port_in=port_in,block_name=name,name=mask_name,s_function=s_function_name,dependencies=[name+"_codegen", name],init_code=dependencies+write_bus(solver_stats_type))

    diag.add(mask)
    for fname,fun in zip(added_functions,self._added_functions):
      mask = Mask(port_labels_in=fun.name_in(), port_labels_out=fun.name_out(), block_name=fun.name(), name=fun.name(), s_function=fname[:-2])
      diag.add(mask)
    diag.write()


    make_file_name = os.path.join(build_dir_abs,"Makefile")

    source_files = [casadi_codegen_file_name]

    for e in artifacts:
      if isinstance(e, SourceArtifact):
        source_files.append(os.path.join(build_dir_abs, e.relative))

    for e in artifacts:
      e.copy_to_build_dir(build_dir_abs)

    import rockit
    try:
      cmake_flags = rockit.GlobalOptions.get_cmake_flags()
    except:
      cmake_flags = []

    def cmake_path(e,build_dir_abs):
      try:
        use_relative = os.path.commonpath([e,build_dir_abs])==build_dir_abs
      except:
        use_relative = False

      if use_relative:
        if os.path.relpath(e,build_dir_abs)==".":
          return "${CMAKE_CURRENT_SOURCE_DIR}"
        else:
          return "${CMAKE_CURRENT_SOURCE_DIR}/"+os.path.relpath(e,build_dir_abs)
      return e
    
    def format_files_or_dirs(v,build_dir_abs,target="cmake"):
      if not isinstance(v,list):
        return format_files_or_dirs([v],build_dir_abs,target)
      return  " ".join([f"\"{escape(cmake_path(e, build_dir_abs))}\"" for e in v])

  
    cmake_file_name = os.path.join(build_dir_abs,"CMakeLists.txt")
    with open(cmake_file_name,"w") as out:
      out.write(f"""
        cmake_minimum_required(VERSION 3.0.0)
        project({name} VERSION 0.1.0)

        #message("If you get a complaint about missing libcasadi.lib, please copy the casadi.lib file to libcasadi.lib")
        #set(CMAKE_PREFIX_PATH "{escape(casadi.GlobalOptions.getCasadiPath())}" ${{CMAKE_PREFIX_PATH}})
        #find_package(casadi REQUIRED)

        set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
        set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
        if(APPLE)
          set(CMAKE_MACOSX_RPATH ON)
          set(CMAKE_INSTALL_RPATH "@loader_path/;@loader_path/lib/")
        else()
          set(CMAKE_INSTALL_RPATH "$ORIGIN;$ORIGIN/lib")
        endif()""")

      include_dirs = [GlobalOptions.getCasadiIncludePath()]
      for e in artifacts:
        if isinstance(e, HeaderDirectory):
          include_dirs.append(e.resolve_dir(build_dir_abs))

      libraries = set()
      if use_codegen:
        with open(casadi_codegen_file_name,"r") as f:
          for l in f.readlines():
            if "osqp" in l:
              libraries.add("osqp")
            if "highs" in l:
              libraries.add("highs")
            if "fatrop" in l:
              libraries.add("fatrop")
              libraries.add("blasfeo")
            if "ipopt" in l:
              libraries.add("ipopt")
      else:
        libraries.add("casadi")

      for e in artifacts:
        if isinstance(e, LibraryArtifact):
          libraries.add(e.basename)

      libraries = sorted(list(libraries))

      link_dirs = [GlobalOptions.getCasadiPath(),build_dir_abs]

      if use_codegen:
        out.write(f"""
                  
          add_library({name}_codegen SHARED {format_files_or_dirs(source_files, build_dir_abs,target="cmake")})
        
          target_include_directories({name}_codegen PRIVATE {format_files_or_dirs(include_dirs, build_dir_abs,target="cmake")})
          target_link_directories({name}_codegen PUBLIC {format_files_or_dirs(link_dirs, build_dir_abs,target="cmake")})
          target_include_directories({name}_codegen PUBLIC {format_files_or_dirs(build_dir_abs, build_dir_abs,target="cmake")})

          target_link_libraries({name}_codegen PRIVATE {" ".join(libraries)})
          
          add_library({name} SHARED {name}.c)
          target_link_libraries({name} {name}_codegen)


          #add_executable({hello_file_name_base} {format_files_or_dirs(hello_c_file_name, build_dir_abs,target="cmake")})
          #target_link_libraries({hello_file_name_base} {name})

          INSTALL(TARGETS {name} {name}_codegen
          LIBRARY DESTINATION .
          ARCHIVE DESTINATION .
          INCLUDES DESTINATION .
          RUNTIME DESTINATION .
          )
          
        """)

      else:
        out.write(f"""
          add_library({name} SHARED {name}.c)
        
          target_include_directories({name} PRIVATE {format_files_or_dirs(include_dirs, build_dir_abs,target="cmake")})
          target_link_directories({name} PUBLIC {format_files_or_dirs(link_dirs, build_dir_abs,target="cmake")})
          target_include_directories({name} PUBLIC {format_files_or_dirs(build_dir_abs, build_dir_abs,target="cmake")})
          target_link_libraries({name} PRIVATE {" ".join(libraries)})

          #add_executable({hello_file_name_base} {format_files_or_dirs(hello_c_file_name, build_dir_abs,target="cmake")})
          #target_link_libraries({hello_file_name_base} {name})

          INSTALL(TARGETS {name}
          LIBRARY DESTINATION .
          ARCHIVE DESTINATION .
          INCLUDES DESTINATION .
          RUNTIME DESTINATION .
          )
          
        """)

    import subprocess
    assert subprocess.run(["cmake","-S", "."] + cmake_flags+["-DCMAKE_BUILD_TYPE="+rockit.GlobalOptions.get_cmake_build_type(),"-B", "build"], cwd=build_dir_abs).returncode==0
    assert subprocess.run(["cmake","--build","build","--config",rockit.GlobalOptions.get_cmake_build_type(),"--verbose"], cwd=build_dir_abs).returncode==0
    assert subprocess.run(["cmake","--install","build","--prefix","."], cwd=build_dir_abs).returncode==0
  
    with open(make_file_name,"w") as out:

      flags = ["-pedantic","-Wall","-Wextra","-Wno-unknown-pragmas","-Wno-long-long","-Wno-unused-parameter","-Wno-unused-const-variable","-Wno-sign-compare","-Wno-unused-but-set-variable","-Wno-unused-variable","-Wno-endif-labels"]
      deps = ["-I"+build_dir_abs,"-L"+build_dir_abs,"-Wl,-rpath,"+build_dir_abs,"-L"+GlobalOptions.getCasadiPath(),"-I"+GlobalOptions.getCasadiIncludePath(),"-Wl,-rpath,"+GlobalOptions.getCasadiPath()]
      for e in artifacts:
        if isinstance(e, HeaderDirectory):
          deps+=e.get_flags(build_dir_abs)


      if use_codegen:
        lib_codegen_file_name = os.path.join(build_dir_abs,"lib" + name + "_codegen.so")
        lib_codegen_compile_commands = ["gcc"] + c_flags + optimization_compilation_flags + ["-fPIC","-shared"]+source_files+ ["-lm","-o"+lib_codegen_file_name]+deps
        deps += ["-l"+name+"_codegen","-losqp"]


        if CURRENT_CASADI_VERSION>=version.parse("3.6.3"):
          deps += ["-lipopt"]

          if casadi.has_nlpsol("fatrop"): deps += ["-lfatrop"]
          if casadi.has_nlpsol("blasfeo"): deps += ["-lblasfeo"]
          
        fatrop_driver = os.path.join(os.path.abspath(src_dir),"foobar","lib","libfatrop_driver.so")
        if os.path.exists(fatrop_driver):
          shutil.copy(fatrop_driver, build_dir_abs)
          shutil.copy(os.path.join(os.path.abspath(src_dir),"foobar","lib","libfatrop.so"), build_dir_abs)
          deps += ["-lfatrop_driver"]

      else:
        deps += ["-lcasadi"]

      for e in artifacts:
        if isinstance(e, LibraryArtifact):
          deps += ["-l" + e.basename]
      lib_compile_commands = ["gcc"] + c_flags + optimization_compilation_flags + ["-fPIC","-shared",c_file_name,"-o"+lib_file_name]+deps+flags

      hello_compile_commands = ["gcc"] + c_flags + optimization_compilation_flags + [hello_c_file_name,"-I"+build_dir_abs,"-l"+lib_name,"-o",hello_file_name]+deps+flags

      if compile and os.name!="nt":
        print("Compiling")
        import subprocess
        if use_codegen:
          p = subprocess.run(lib_codegen_compile_commands, capture_output=True, text=True)
          print(" ".join(p.args))
          if p.returncode!=0:
            raise Exception("Failed to compile:\n{args}\n{stdout}\n{stderr}".format(args=" ".join(p.args),stderr=p.stderr,stdout=p.stdout))

        p = subprocess.run(lib_compile_commands, capture_output=True, text=True)
        print(" ".join(p.args))
        if p.returncode!=0:
          raise Exception("Failed to compile:\n{args}\n{stdout}\n{stderr}".format(args=" ".join(p.args),stderr=p.stderr,stdout=p.stdout))
        # #breaks matlab
        if context!="matlab":
          p = subprocess.run(hello_compile_commands, capture_output=True, text=True)
          if p.returncode!=0:
            raise Exception("Failed to compile:\n{args}\n{stdout}\n{stderr}".format(args=" ".join(p.args),stderr=p.stderr,stdout=p.stdout))
        print(hello_compile_commands)
      else:
        print(f"Not compiling Impact artifacts. compile {compile}")

      """
      if use_codegen:
        out.write(os.path.basename(lib_codegen_file_name) + ": " + " ".join([os.path.basename(e) for e in source_files]) + "\n")
        out.write("\t"+" ".join(lib_codegen_compile_commands)+"\n\n")

      out.write(os.path.basename(lib_file_name) + ": " + os.path.basename(c_file_name) + "\n")
      out.write("\t"+" ".join(lib_compile_commands)+"\n\n")

      out.write(os.path.basename(hello_file_name) + ": " + os.path.basename(hello_c_file_name) + "\n")
      out.write("\t"+" ".join(hello_compile_commands)+"\n\n")"""

    cmake_file_name = os.path.join(build_dir_abs,"build.bat")
    with open(cmake_file_name,"w") as out:
      out.write(f"""
      echo "Should be ran in 'x64 Native Tools Command Prompt for VS'"
      cmake -G "Visual Studio 16 2019" -A x64 -B build
      cmake --build build --config Release
      cmake --install build --prefix .
      """)
    cmake_file_name = os.path.join(build_dir_abs,"build.sh")
    with open(cmake_file_name,"w") as out:
      out.write(f"""
      cmake -DCMAKE_INSTALL_PREFIX=. -B build
      cmake --build build --config Release
      cmake --install build --prefix .
      """)


    #  -- Discrete model code generation (extra node) ---
    try:
        import casadi as cs
        # Rockit exposes a discrete map builder on the method:
        if hasattr(self._method, "discrete_system"):
            fdisc = self._method.discrete_system(self)  # (x0,u,T) -> {"xf": ...}

            # Symbols with the right sizes
            x0 = cs.MX.sym('x0', self.nx)
            u  = cs.MX.sym('u', self.nu if isinstance(self.nu, int) else int(self.nu))
            T  = cs.MX.sym('T')

            xf = fdisc(x0=x0, u=u, T=T)["xf"]

            # Name the C function <name>_step and the unit (file) <name>_model
            step_fun = cs.Function(f"{name}_step", [x0, u, T], [xf],
                                  ["x0","u","T"], ["xf"])

            cg_model = cs.CodeGenerator(f"{name}_model", {"with_header": True})
            cg_model.add(step_fun)
            cg_model.generate(build_dir_abs + os.sep)
            print(f"Generated discrete model C at {build_dir_rel}/{name}_model.c")
        else:
            print("Warning: method has no discrete_system(...); skipping model codegen.")
    except Exception as e:
        print(f"Warning: discrete model codegen failed: {e}")
    # -- Discrete model code generation (extra node) ---



    if ros2: 
      ROS2gen = ROS2Generator(
          mpc_specification = self,
          package_name = name,
          ros2_options= ros2_options,
      )
      print(f"\nExporting ROS 2 package {name} ...")

      ROS2gen.export_package()
      print(f"\nROS 2 package {name} exported")


    print("\nsuccess")



  @staticmethod
  def load(name):
      import pickle
      with rockit_unpickle_context():
          return pickle.load(open(name,"rb"))