diff --git a/+ocl/+acados/Solver.m b/+ocl/+acados/Solver.m
new file mode 100644
index 00000000..2ff5c065
--- /dev/null
+++ b/+ocl/+acados/Solver.m
@@ -0,0 +1,337 @@
+classdef Solver < handle
+  
+  properties
+    acados_ocp_p
+    
+    x_struct_p
+    z_struct_p
+    u_struct_p
+    p_struct_p
+    
+    x0_bounds_p
+    
+    N_p
+    T_p
+    
+    x_guess_p
+    u_guess_p
+    
+    x_traj_p
+    u_traj_p
+    
+    sol_out_p
+    times_out_p
+    
+    verbose_p
+  end
+  
+  methods
+    function self = Solver(varargin)
+      
+      ocl.acados.setup();
+      
+      wsp = ocl.utils.workspacePath();
+      acados_build_dir = fullfile(wsp, 'export');
+      
+      p = ocl.utils.ArgumentParser;
+      
+      p.addRequired('T', @(el) isnumeric(el) );
+      p.addKeyword('vars', @ocl.utils.emptyfun, @ocl.utils.isFunHandle);
+      p.addKeyword('dae', @ocl.utils.emptyfun, @ocl.utils.isFunHandle);
+      p.addKeyword('pathcosts', @ocl.utils.zerofun, @ocl.utils.isFunHandle);
+      p.addKeyword('gridcosts', @ocl.utils.zerofun, @ocl.utils.isFunHandle);
+      p.addKeyword('gridconstraints', @ocl.utils.emptyfun, @ocl.utils.isFunHandle);
+      p.addKeyword('terminalcost', @ocl.utils.emptyfun, @ocl.utils.isFunHandle);
+      
+      p.addParameter('N', 20, @isnumeric);
+      p.addParameter('d', 3, @isnumeric);
+      
+      p.addParameter('verbose', true, @islogical);
+      
+      r = p.parse(varargin{:});
+      
+      T = r.T;
+      N = r.N;
+      varsfh = r.vars;
+      daefh = r.dae;
+      pathcostsfh = r.pathcosts;
+      gridcostsfh = r.gridcosts;
+      gridconstraintsfh = r.gridconstraints;
+      terminalcostfh = r.terminalcost;
+      verbose = r.verbose;
+      
+      model_changed = ocl.acados.hasModelChanged({varsfh, daefh, pathcostsfh, ...
+        gridcostsfh, gridconstraintsfh, terminalcostfh}, N);
+      
+      [x_struct, z_struct, u_struct, p_struct, ...
+        x_bounds, ~, u_bounds, ~, ...
+        x_order] = ocl.model.vars(varsfh);
+      
+      nx = length(x_struct);
+      nz = length(z_struct);
+      nu = length(u_struct);
+      np = length(p_struct);
+      
+      ocl.utils.assert(nz==0, 'Algebraic variable are currently not support in the acados interface.');
+      ocl.utils.assert(np==0, 'Parameters are currently not support in the acados interface.');
+      
+      daefun = @(x,z,u,p) ocl.model.dae( ...
+        daefh, ...
+        x_struct, ...
+        z_struct, ...
+        u_struct, ...
+        p_struct, ...
+        x_order, ...
+        x, z, u, p);
+      
+      gridcostfun = @(k,K,x,p) ocl.model.gridcosts( ...
+        gridcostsfh, ...
+        x_struct, ...
+        p_struct, ...
+        k, K, x, p);
+      
+      pathcostfun = @(x,z,u,p) ocl.model.pathcosts( ...
+        pathcostsfh, ...
+        x_struct, ...
+        z_struct, ...
+        u_struct, ...
+        p_struct, ...
+        x, z, u, p);
+      
+      gridconstraintfun = @(k,K,x,p) ocl.model.gridconstraints( ...
+        gridconstraintsfh, ...
+        x_struct, ...
+        p_struct, ...
+        k, K, x, p);
+      
+      terminalcostfun = @(x,p) ocl.model.terminalcost( ...
+        terminalcostfh, ...
+        x_struct, ...
+        p_struct, ...
+        x, p);
+    
+      [x_lb, x_ub] = ocl.model.bounds(x_struct, x_bounds);
+      [lbx, ubx, Jbx] = ocl.acados.bounds(x_lb, x_ub);
+      
+      [u_lb, u_ub] = ocl.model.bounds(u_struct, u_bounds);
+      [lbu, ubu, Jbu] = ocl.acados.bounds(u_lb, u_ub);
+      
+      ocp = ocl.acados.construct( ...
+        nx, nu, ...
+        T, N, ...
+        daefun, gridcostfun, pathcostfun, gridconstraintfun, ...
+        terminalcostfun, ...
+        lbx, ubx, Jbx, lbu, ubu, Jbu, ...
+        acados_build_dir, ...
+        model_changed);
+      
+      % ig variables
+      x_traj_structure = ocl.types.Structure();
+      for k=1:N+1
+        x_traj_structure.add('x', x_struct);
+      end
+      x_traj = ocl.Variable.create(x_traj_structure, 0);
+      x_traj = x_traj.x;
+      
+      u_traj_structure = ocl.types.Structure();
+      for k=1:N
+        u_traj_structure.add('u', u_struct);
+      end
+      u_traj = ocl.Variable.create(u_traj_structure, 0);
+      u_traj = u_traj.u;
+      
+      % solution variables
+      sol_struct = ocl.types.Structure();
+      times_struct = ocl.types.Structure();
+
+      sol_struct.add('states', x_struct);
+      times_struct.add('states', [1,1]);
+      
+      for j=1:N
+        sol_struct.add('states', x_struct);
+        times_struct.add('states', [1,1]);
+      end
+
+      for j=1:N
+        sol_struct.add('controls', u_struct);
+        times_struct.add('controls', [1,1]);
+      end
+
+      sol_out = ocl.Variable.create(sol_struct, 0);
+      times_out = ocl.Variable.create(times_struct, 0);
+      
+      self.x_traj_p = x_traj;
+      self.u_traj_p = u_traj;
+      
+      self.sol_out_p = sol_out;
+      self.times_out_p = times_out;
+      
+      self.acados_ocp_p = ocp;
+      self.x_struct_p = x_struct;
+      self.z_struct_p = z_struct;
+      self.u_struct_p = u_struct;
+      self.p_struct_p = p_struct;
+      
+      self.x0_bounds_p = ocl.types.Bounds();
+      self.N_p = N;
+      self.T_p = T;
+      
+      self.x_guess_p = ocl.types.InitialGuess(x_struct);
+      self.u_guess_p = ocl.types.InitialGuess(u_struct);
+      
+      self.verbose_p = verbose;
+    end
+    
+    function initialize(self, id, points, values, T)
+      
+      points = ocl.Variable.getValue(points);
+      values = ocl.Variable.getValue(values);
+      
+      if nargin==5
+        points = points / T;
+      end
+      
+      x_ids = self.x_struct_p.getNames();
+      u_ids = self.u_struct_p.getNames();
+      
+      if ocl.utils.fieldnamesContain(x_ids, id)
+        self.x_guess_p.set(id, points, values);
+      elseif ocl.utils.fieldnamesContain(u_ids, id)
+        self.u_guess_p.set(id, points, values);
+      else
+        ocl.utils.error(['Unknown id: ' , id, ' (not a state and not a control)']);
+      end
+    end
+    
+    function [sol_out,times_out] = solve(self)
+      
+      ocp = self.acados_ocp_p;
+      x_struct = self.x_struct_p;
+      u_struct = self.u_struct_p;
+      N = self.N_p;
+      T = self.T_p;
+      x0_bounds = self.x0_bounds_p;
+      x_guess = self.x_guess_p.data;
+      u_guess = self.u_guess_p.data;
+      verbose = self.verbose_p;
+      x_traj = self.x_traj_p;
+      u_traj = self.u_traj_p;
+      sol_out = self.sol_out_p;
+      times_out = self.times_out_p;
+      
+      % x0
+      [x0_lb, x0_ub] = ocl.model.bounds(x_struct, x0_bounds);
+      ocl.utils.assert(all(x0_lb == x0_ub), 'Initial state must be a fixed value (not a box constraint) in the acados interface.');
+      ocp.set('constr_x0', x0_lb);
+      
+      % init x
+      x_traj.set(ocp.get('x'));
+      
+      times_target = linspace(0,1,N+1);
+      names = fieldnames(x_guess);
+      for k=1:length(names)
+        id = names{k};
+        xdata = x_guess.(id).x;
+        ydata = x_guess.(id).y;
+        
+        ytarget = interp1(xdata, ydata, times_target,'linear','extrap');
+        
+        x_traj.get(id).set(ytarget);
+      end
+      ocp.set('init_x', x_traj.value);
+      
+      % init u
+      u_traj.set(ocp.get('u'));
+      
+      times_target = linspace(0,1,N+1);
+      times_target = times_target(1:end-1);
+      names = fieldnames(u_guess);
+      for k=1:length(names)
+        id = names{k};
+        xdata = u_guess.(id).x;
+        ydata = u_guess.(id).y;
+        
+        ytarget = interp1(xdata, ydata, times_target,'linear','extrap');
+        
+        u_traj.get(id).set(ytarget);
+      end
+      ocp.set('init_u', u_traj.value);
+      
+      % solve
+      ocl.acados.solve(ocp);
+      
+      x_traj = ocp.get('x');
+      u_traj = ocp.get('u');
+      
+      sol_out.states.set(x_traj);
+      sol_out.controls.set(u_traj);
+      
+      x_times = linspace(0,T,N+1);
+      u_times = x_times(1:end-1);
+   
+      times_out.states.set(x_times);
+      times_out.controls.set(u_times);
+      
+      % clear initial guess
+      self.x_guess_p = ocl.types.InitialGuess(x_struct);
+      self.u_guess_p = ocl.types.InitialGuess(u_struct);
+      
+      if verbose
+        disp(self.stats())
+      end
+
+    end
+    
+    function setMaxIterations(self, N)
+      self.acados_ocp_p.opts_struct.nlp_solver_max_iter = N;
+    end
+
+    function r = stats(self)
+      
+      r = '';
+      
+      acados_ocp = self.acados_ocp_p;
+      
+      status = acados_ocp.get('status');
+      sqp_iter = acados_ocp.get('sqp_iter');
+      time_tot = acados_ocp.get('time_tot');
+      time_lin = acados_ocp.get('time_lin');
+      time_reg = acados_ocp.get('time_reg');
+      time_qp_sol = acados_ocp.get('time_qp_sol');
+      
+      r = [r, sprintf('\nstatus = %d, sqp_iter = %d, time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3)];
+
+      stat = acados_ocp.get('stat');
+      r = [r, sprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_stat\tqp_iter')];
+      if size(stat,2)>7
+        r = [r, sprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m')];
+      end
+      r = [r, newline];
+      for ii=1:size(stat,1)
+        r = [r, sprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6), stat(ii,7))];
+        if size(stat,2)>7
+          r = [r, sprintf('\t%e\t%e\t%e\t%e', stat(ii,8), stat(ii,9), stat(ii,10), stat(ii,11))];
+        end
+        r = [r, newline];
+      end
+      r = [r, newline];
+
+      if status==0
+        r = [r, sprintf('\nsuccess!\n\n')];
+      else
+        r = [r, sprintf('\nsolution failed!\n\n')];
+      end
+    end
+    
+    function setInitialStateBounds(self, id, varargin)
+
+      x0_bounds = self.x0_bounds_p;
+      x0_bounds.set(id, varargin{:});
+    end
+    
+    function setInitialState(self, id, value)
+      self.setInitialStateBounds(id, value);
+    end
+    
+  end
+end
diff --git a/+ocl/+acados/bounds.m b/+ocl/+acados/bounds.m
new file mode 100644
index 00000000..e85ed589
--- /dev/null
+++ b/+ocl/+acados/bounds.m
@@ -0,0 +1,10 @@
+function [lb_out, ub_out, Jb] = bounds(lb_in, ub_in)
+
+bounds_select = ~isinf(lb_in) | ~isinf(ub_in);
+
+Jb = diag(bounds_select);
+Jb = Jb(any(Jb,2),:);
+Jb = real(Jb);
+
+lb_out = lb_in(bounds_select);
+ub_out = ub_in(bounds_select);
diff --git a/+ocl/+acados/construct.m b/+ocl/+acados/construct.m
new file mode 100644
index 00000000..99db4c97
--- /dev/null
+++ b/+ocl/+acados/construct.m
@@ -0,0 +1,142 @@
+function ocp = construct( ...
+    nx, nu, ...
+    T, N, ...
+    daefun, gridcostfun, pathcostfun, gridconstraintfun, ...
+    terminalcostfun, ...
+    lbx, ubx, Jbx, lbu, ubu, Jbu, ...
+    acados_build_dir, ...
+    build_model)
+
+casadi_sym = @casadi.SX.sym;
+
+x_sym = casadi_sym('x', nx);
+u_sym = casadi_sym('u', nu);
+xd_sym = casadi_sym('xd', nx);
+
+% system equations
+[ode,~] = daefun(x_sym, [], u_sym, []);
+f_expl = ode;
+
+lagrange_cost = pathcostfun(x_sym, [], u_sym, []);
+mayer_cost = gridcostfun(N+1, N+1, x_sym, []);
+
+% end constraints
+[endconstraints, endconstraints_lb, endconstraints_ub] = ...
+    gridconstraintfun(N+1, N+1, x_sym, []);
+
+% interface compatibility checks
+ocl.utils.assert(~isempty(T), 'Free endtime is not supported in the acados interface. In most cases it is possible to reformulate a time-optimal control problem by doing a coordinate transformation.')
+
+for k=1:N
+  gridcost = gridcostfun(k, N+1, x_sym, []);
+  ocl.utils.assert(gridcost == 0, 'In the gridcosts only terminal cost (mayer cost) are supported in the acados interface.');
+end
+
+for k=1:N
+  [gridconstraint,~,~] = gridconstraintfun(k, N+1, x_sym, []);
+  ocl.utils.assert(isempty(gridconstraint), 'In the gridconstraints only terminal constraints are supported in the acados interface.');
+end
+
+mayer_cost = mayer_cost + terminalcostfun(x_sym, []);
+
+n_endconstraints = length(endconstraints);
+
+ocp_model = acados_ocp_model();
+
+% dims
+ocp_model.set('T', T);
+ocp_model.set('dim_nx', nx);
+ocp_model.set('dim_nu', nu);
+ocp_model.set('dim_nbx', length(lbx));
+ocp_model.set('dim_nbu', length(lbu));
+ocp_model.set('dim_ng', 0);
+ocp_model.set('dim_ng_e', 0);
+ocp_model.set('dim_nh', 0);
+ocp_model.set('dim_nh_e', n_endconstraints);
+
+% symbolics
+ocp_model.set('sym_x', x_sym);
+ocp_model.set('sym_u', u_sym);
+ocp_model.set('sym_xdot', xd_sym);
+
+% cost
+ocp_model.set('cost_type', 'ext_cost');
+ocp_model.set('cost_type_e', 'ext_cost'); % mayer
+ocp_model.set('cost_expr_ext_cost', lagrange_cost);
+ocp_model.set('cost_expr_ext_cost_e', mayer_cost);
+
+% dynamics
+ocp_model.set('dyn_type', 'explicit');
+ocp_model.set('dyn_expr_f', f_expl);
+
+% bounds
+ocp_model.set('constr_lbx', lbx);
+ocp_model.set('constr_ubx', ubx);
+ocp_model.set('constr_Jbx', Jbx);
+
+ocp_model.set('constr_lbu', lbu);
+ocp_model.set('constr_ubu', ubu);
+ocp_model.set('constr_Jbu', Jbu);
+
+% constraints (non-linear terminal)
+if ~isempty(endconstraints)
+  ocp_model.set('constr_expr_h_e', endconstraints);
+  ocp_model.set('constr_lh_e', endconstraints_lb);
+  ocp_model.set('constr_uh_e', endconstraints_ub);
+end
+
+%% acados ocp opts
+nlp_solver_ext_qp_res = 1;
+nlp_solver_max_iter = 100;
+nlp_solver_tol_stat = 1e-8;
+nlp_solver_tol_eq   = 1e-8;
+nlp_solver_tol_ineq = 1e-8;
+nlp_solver_tol_comp = 1e-8;
+qp_solver_cond_N = 5;
+qp_solver_cond_ric_alg = 0;
+qp_solver_ric_alg = 0;
+qp_solver_warm_start = 2;
+sim_method_num_stages = 4;
+sim_method_num_steps = 3;
+
+if build_model
+  codgen_model = 'true';
+  ocl.utils.info('Compiling model...')
+else
+  codgen_model = 'false';
+end
+
+ocp_opts = acados_ocp_opts();
+nlp_solver = 'sqp';
+ocp_opts.set('compile_mex', 'false');
+ocp_opts.set('codgen_model', codgen_model);
+ocp_opts.set('param_scheme', 'multiple_shooting_unif_grid');
+ocp_opts.set('param_scheme_N', N);
+ocp_opts.set('nlp_solver', nlp_solver);
+ocp_opts.set('nlp_solver_exact_hessian', 'false');
+ocp_opts.set('regularize_method', 'no_regularize');
+ocp_opts.set('nlp_solver_ext_qp_res', nlp_solver_ext_qp_res);
+ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter);
+ocp_opts.set('nlp_solver_tol_stat', nlp_solver_tol_stat);
+ocp_opts.set('nlp_solver_tol_eq', nlp_solver_tol_eq);
+ocp_opts.set('nlp_solver_tol_ineq', nlp_solver_tol_ineq);
+ocp_opts.set('nlp_solver_tol_comp', nlp_solver_tol_comp);
+ocp_opts.set('qp_solver', 'partial_condensing_hpipm');
+ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N);
+ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg);
+ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg);
+ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start);
+ocp_opts.set('sim_method', 'erk');
+ocp_opts.set('sim_method_num_stages', sim_method_num_stages);
+ocp_opts.set('sim_method_num_steps', sim_method_num_steps);
+
+ocp_opts.set('output_dir', acados_build_dir);
+
+ocp = acados_ocp(ocp_model, ocp_opts);
+setenv('OCL_MODEL_DATENUM', num2str(now));
+setenv('OCL_MODEL_N', num2str(N));
+
+x_traj_init = zeros(nx, N+1);
+u_traj_init = zeros(nu, N);
+ocp.set('init_x', x_traj_init);
+ocp.set('init_u', u_traj_init);
diff --git a/+ocl/+acados/hasModelChanged.m b/+ocl/+acados/hasModelChanged.m
new file mode 100644
index 00000000..856bb14b
--- /dev/null
+++ b/+ocl/+acados/hasModelChanged.m
@@ -0,0 +1,33 @@
+function r = hasModelChanged(fh_list, N)
+
+cur_model_datenum = getenv('OCL_MODEL_DATENUM');
+cur_model_N = getenv('OCL_MODEL_N');
+
+if isempty(cur_model_datenum) || isempty(cur_model_N) || ...
+    str2double(cur_model_N) ~= N
+  r = true;
+else
+  cur_model_datenum = str2double(cur_model_datenum);
+  r = false;
+
+  for k=1:length(fh_list)
+    info = functions(fh_list{k});
+
+    if strcmp(info.type, 'simple')
+
+      % check function file
+      fun_str = info.function;
+      file_str = which(fun_str);
+
+      file_info = dir(file_str);
+
+      if file_info.datenum > cur_model_datenum
+        r = true;
+        break;
+      end
+    elseif ~strcmp(info.type, 'classsimple')
+      r = true;
+      break;
+    end
+  end
+end
\ No newline at end of file
diff --git a/+ocl/+acados/setup.m b/+ocl/+acados/setup.m
new file mode 100644
index 00000000..6acba961
--- /dev/null
+++ b/+ocl/+acados/setup.m
@@ -0,0 +1,117 @@
+function setup()
+
+ocl.utils.checkStartup;
+
+latest_acados_version = 'n3_7a83d3';
+
+% check if MinGW compiler is setup
+c_compiler = mex.getCompilerConfigurations('C','Selected').ShortName;
+ocl.utils.assert(strcmp(c_compiler, 'mingw64') || strcmp(c_compiler, 'gcc'), ...
+  'Please setup gcc (Linux) or MinGW (Windows) as your c compiler using `mex -setup C`.');
+
+acados_dir = getenv('ACADOS_INSTALL_DIR');
+
+ocl_dir  = fileparts(which('ocl'));
+
+if isempty(acados_dir) 
+  
+  % install acados if not on the system
+  if ~acados_installed(ocl_dir, latest_acados_version)
+    install_acados(ocl_dir, latest_acados_version)
+  end
+  
+  setenv('ACADOS_INSTALL_DIR', fullfile(ocl_dir,'Lib','acados'));
+  acados_dir = fullfile(ocl_dir,'Lib','acados');
+end
+
+addpath(fullfile(acados_dir, 'interfaces', 'acados_matlab'));
+
+setenv('ENV_RUN','true')
+
+% compile acados mex interface
+export_dir = fullfile(ocl.utils.workspacePath(), 'export');
+if ~exist(fullfile(export_dir, 'ACADOS_MEX_INSTALLED'), 'file')
+  ocl.utils.info('Compiling acados mex interface...')
+
+  opts = struct;
+  opts.output_dir = export_dir;
+  opts.qp_solver = 'partial_condensing_hpipm';
+  ocp_compile_mex(opts);
+  
+  fid = fopen(fullfile(export_dir,'ACADOS_MEX_INSTALLED'), 'wt' );
+  fclose(fid);
+end
+
+ocl.utils.info('Acados setup procedure finished. ')
+
+end
+
+function r = acados_installed(ocl_dir, version)
+r = exist(fullfile(ocl_dir,'Lib','acados','ACADOS_INSTALLED'), 'file');
+
+if r
+  
+  fid = fopen(fullfile(ocl_dir,'Lib','acados','ACADOS_INSTALLED'), 'r' );
+  installed_version = fscanf(fid, '%s');
+  fclose(fid);
+  
+  if ~strcmp(installed_version, version)
+    [~] = input(['Deleting old acados version ', installed_version, newline, ...
+      'to replace with the newer version ', version, newline, ...
+      'Press [enter] to proceed.'], 's');
+    
+    acados_dir = fullfile(ocl_dir,'Lib','acados');
+    addpath(fullfile(acados_dir, 'interfaces', 'acados_matlab'));
+    rmpath(fullfile(acados_dir, 'interfaces', 'acados_matlab'));
+    s = rmdir(acados_dir, 's');
+    if ~s
+      ocl.utils.error(['Could not remove acados. Restart Matlab and try ', ...
+        'again or remove the Lib/acados directory manually.']);
+    end
+    
+    export_dir = fullfile(ocl.utils.workspacePath(), 'export');
+    delete(fullfile(export_dir, 'ACADOS_MEX_INSTALLED'));
+    r = false;
+  end
+end
+
+end
+
+function install_acados(ocl_dir, latest_acados_version)
+
+ocl.utils.info(['We are now downloading binaries of acados for you. ', ...
+  'This takes a while! If you ', ...
+  'would like to setup an individual acados installation, set the ', ...
+  'ACADOS_INSTALL_DIR environment variable. ']);
+
+if ispc
+  download_url = 'https://github.com/jkoendev/acados-deployment/releases/download/n3_7a83d3/acados-7a83d3_win.zip';
+elseif isunix && ~ismac
+  download_url = 'https://github.com/jkoendev/acados-deployment/releases/download/n3_7a83d3/acados-7a83d3_linux.zip';
+else
+  ocl.utils.error(['Your system is not supported to setup acados automatically. ', ...
+    'Please tell us if you want your configuration to be supported. You can also ', ...
+    'download and compile acados yourself, and set the ACADOS_INSTALL_DIR ', ...
+    'environment variable.']);
+end
+
+% download and unzip acados
+downlad_destination = fullfile(ocl_dir, 'Workspace', 'acados-download.zip');
+ocl.utils.info('Downloading acados (~32MB) ...');
+websave(downlad_destination, download_url);
+ocl.utils.info('Unpacking acados ...');
+unzip(downlad_destination, fullfile(ocl_dir,'Lib','acados'))
+
+% copy binaries to Workspace/export
+export_dir = fullfile(ocl_dir, 'Workspace', 'export');
+copyfile(fullfile(ocl_dir,'Lib','acados','lib','*'), export_dir);
+
+fid = fopen(fullfile(ocl_dir,'Lib','acados','ACADOS_INSTALLED'), 'wt' );
+fprintf(fid,'%s\n', latest_acados_version);
+fclose(fid);
+
+end
+
+
+
+
diff --git a/+ocl/+acados/solve.m b/+ocl/+acados/solve.m
new file mode 100644
index 00000000..3a5ff09f
--- /dev/null
+++ b/+ocl/+acados/solve.m
@@ -0,0 +1,4 @@
+function solve(acados_ocp)
+
+% solve
+acados_ocp.solve();
\ No newline at end of file
diff --git a/CasadiLibrary/CasadiIntegrator.m b/+ocl/+casadi/CasadiIntegrator.m
old mode 100755
new mode 100644
similarity index 72%
rename from CasadiLibrary/CasadiIntegrator.m
rename to +ocl/+casadi/CasadiIntegrator.m
index 6c9d7221..cac96af4
--- a/CasadiLibrary/CasadiIntegrator.m
+++ b/+ocl/+casadi/CasadiIntegrator.m
@@ -8,21 +8,18 @@
   
   properties
     casadiIntegrator
-    system
   end
   
   methods
-    function self = CasadiIntegrator(system)
+    function self = CasadiIntegrator(nx, nz, nu, np, daefun)
       
-      self.system = system;
+      states = casadi.SX.sym('x', nx);
+      algVars = casadi.SX.sym('z', nz);
+      controls = casadi.SX.sym('u', nu);
+      h = casadi.SX.sym('h');
+      parameters = casadi.SX.sym('p', np);
       
-      states = casadi.SX.sym('x',system.nx,1);
-      algVars = casadi.SX.sym('z',system.nz,1);
-      controls = casadi.SX.sym('u',system.nu,1);
-      h = casadi.SX.sym('h',1,1);
-      parameters = casadi.SX.sym('p',system.np,1);
-      
-      [ode,alg] = system.daefun(states,algVars,controls,parameters);
+      [ode,alg] = daefun(states,algVars,controls,parameters);
 
       dae = struct;
       dae.x = states;
diff --git a/CasadiLibrary/CasadiOptions.m b/+ocl/+casadi/CasadiOptions.m
similarity index 100%
rename from CasadiLibrary/CasadiOptions.m
rename to +ocl/+casadi/CasadiOptions.m
diff --git a/+ocl/+casadi/CasadiSolver.m b/+ocl/+casadi/CasadiSolver.m
new file mode 100644
index 00000000..977ef2c0
--- /dev/null
+++ b/+ocl/+casadi/CasadiSolver.m
@@ -0,0 +1,423 @@
+classdef CasadiSolver < handle
+  
+  properties
+    timeMeasures
+    stageList
+    collocationList
+    nlpData
+  end
+  
+  properties (Access = private)
+    controls_regularization
+    controls_regularization_value
+  end
+  
+  methods
+    
+    function self = CasadiSolver(stageList, transitionList, ...
+                                 nlp_casadi_mx, ...
+                                 controls_regularization, controls_regularization_value, ...
+                                 casadi_options)
+      
+      ocl.utils.assert(length(stageList)==length(transitionList)+1, ...
+                'You need to specify Ns-1 transitions for Ns stages.');
+              
+      self.controls_regularization = controls_regularization;
+      self.controls_regularization_value = controls_regularization_value;
+      
+      constructTotalTic = tic;
+      
+      % create variables as casadi symbolics
+      if nlp_casadi_mx
+        expr = @casadi.MX.sym;
+      else
+        expr = @casadi.SX.sym;
+      end
+      
+      vars = cell(length(stageList), 1);
+      costs = cell(length(stageList), 1);
+      constraints = cell(length(stageList), 1);
+      constraints_LB = cell(length(stageList), 1);
+      constraints_UB = cell(length(stageList), 1);
+      
+      v_stage = [];
+      
+      collocationList = cell(length(stageList), 1);
+      
+      for k=1:length(stageList)
+        stage = stageList{k};
+        
+        x_struct = stage.x_struct;
+        z_struct = stage.z_struct;
+        u_struct = stage.u_struct;
+        p_struct = stage.p_struct;
+        x_order = stage.x_order;
+        
+        daefh = stage.daefh;
+        pathcostsfh = stage.pathcostsfh;
+        gridcostsfh = stage.gridcostsfh;
+        gridconstraintsfh = stage.gridconstraintsfh;
+        terminalcostfh = stage.terminalcostfh;
+        
+        nx = stage.nx;
+        nu = stage.nu;
+        np = stage.np;
+        d = stage.d;
+        H_norm = stage.H_norm;
+        T = stage.T;
+        N = stage.N;
+        
+        collocation = ocl.Collocation(x_struct, z_struct, u_struct, p_struct, x_order, daefh, pathcostsfh, d);
+        
+        ni = collocation.num_i;
+        collocationfun = @(x0,vars,u,h,p) ocl.collocation.equations(collocation, x0, vars, u, h, p);
+        
+        x = expr(['x','_s',mat2str(k)], nx);
+        vi = expr(['vi','_s',mat2str(k)], ni);
+        u = expr(['u','_s',mat2str(k)], nu);
+        h = expr(['h','_s',mat2str(k)]);
+        p = expr(['p','_s',mat2str(k)], np);
+        
+        [xF, cost_integr, equations] = collocationfun(x, vi, u, h, p);
+        integrator_fun = casadi.Function('sys', {x,vi,u,h,p}, {xF, cost_integr, equations});
+        
+        integratormap = integrator_fun.map(N, 'serial');
+        
+        nv_stage = ocl.simultaneous.nvars(N, nx, ni, nu, np);
+        v_last_stage = v_stage;
+        v_stage = expr(['v','_s',mat2str(k)], nv_stage);
+        
+        gridcostfun = @(k,K,x,p) ocl.model.gridcosts(gridcostsfh, x_struct, p_struct, k, K, x, p);
+        gridconstraintfun = @(k,K,x,p) ocl.model.gridconstraints(gridconstraintsfh, x_struct, p_struct, k, K, x, p);
+        terminalcostfun = @(x,p) ocl.model.terminalcost(terminalcostfh, x_struct, p_struct, x, p);
+        
+        [costs_stage, constraints_stage, ...
+         constraints_LB_stage, constraints_UB_stage] = ...
+            ocl.simultaneous.equations(H_norm, T, nx, ni, nu, np, ...
+                                       gridcostfun, gridconstraintfun, ...
+                                       terminalcostfun, ...
+                                       integratormap, v_stage, ...
+                                       controls_regularization, controls_regularization_value);
+        
+        collocationList{k} = collocation;
+                                     
+        transition_eq = [];
+        transition_lb = [];
+        transition_ub = [];
+        if k >= 2
+          x0s = ocl.simultaneous.getFirstState(stageList{k}, collocationList{k}, v_stage);
+          xfs = ocl.simultaneous.getLastState(stageList{k-1}, collocationList{k-1}, v_last_stage);
+          transition_fun = transitionList{k-1};
+          
+          tansition_handler = ocl.Constraint();
+          
+          x0 = ocl.Variable.create(stageList{k}.x_struct, x0s);
+          xf = ocl.Variable.create(stageList{k-1}.x_struct, xfs);
+          
+          transition_fun(tansition_handler,x0,xf);
+          
+          transition_eq = tansition_handler.values;
+          transition_lb = tansition_handler.lowerBounds;
+          transition_ub = tansition_handler.upperBounds;
+        end
+        
+        vars{k} = v_stage;
+        costs{k} = costs_stage;
+        constraints{k} = vertcat(transition_eq, constraints_stage);
+        constraints_LB{k} = vertcat(transition_lb, constraints_LB_stage);
+        constraints_UB{k} = vertcat(transition_ub, constraints_UB_stage);
+        
+      end
+      
+      v = vertcat(vars{:});
+      costs = sum([costs{:}]);
+      constraints = vertcat(constraints{:});
+      constraints_LB = vertcat(constraints_LB{:});
+      constraints_UB = vertcat(constraints_UB{:});
+      
+      % get struct with nlp for casadi
+      casadiNLP = struct;
+      casadiNLP.x = v;
+      casadiNLP.f = costs;
+      casadiNLP.g = constraints;
+      casadiNLP.p = [];
+      
+      constructSolverTic = tic;
+      casadiSolver = casadi.nlpsol('my_solver', 'ipopt', casadiNLP, casadi_options);
+      constructSolverTime = toc(constructSolverTic);
+
+      nlpData = struct;
+      nlpData.casadiNLP = casadiNLP;
+      nlpData.constraints_LB = constraints_LB;
+      nlpData.constraints_UB = constraints_UB;
+      nlpData.solver = casadiSolver;
+      
+      timeMeasures.constructTotal = toc(constructTotalTic);
+      timeMeasures.constructSolver = constructSolverTime;
+      
+      self.stageList = stageList;
+      self.nlpData = nlpData;
+      self.timeMeasures = timeMeasures;
+      self.collocationList = collocationList;
+    end
+    
+    function igMerged = getInitialGuessWithUserData(self)
+      
+      stage_list = self.stageList;
+      colloc_list = self.collocationList;
+      
+      igMerged = cell(length(stage_list), 1);
+      
+      ig = self.getInitialGuess();
+      for k=1:length(stage_list)
+        stage = stage_list{k};
+        colloc = colloc_list{k};
+        
+        igMerged{k} = ocl.simultaneous.getInitialGuessWithUserData(ig{k}, stage, colloc);
+      end
+    end
+    
+    function igList = getInitialGuess(self)
+      stage_list = self.stageList;
+
+      igList = cell(length(stage_list),1);
+      for k=1:length(stage_list)
+        stage = stage_list{k};
+        
+        colloc = self.collocationList{k};
+        
+        N = stage.N;
+        x_struct = stage.x_struct;
+        z_struct = stage.z_struct;
+        u_struct = stage.u_struct;
+        p_struct = stage.p_struct;
+        
+        nx = stage.nx;
+        nu = stage.nu;
+        np = stage.np;
+        H_norm = stage.H_norm;
+        T = stage.T;
+        
+        vi_struct = colloc.vars;
+        ni = colloc.num_i;
+        
+        x_bounds = stage.x_bounds;
+        x0_bounds = stage.x0_bounds;
+        xF_bounds = stage.xF_bounds;
+        z_bounds = stage.z_bounds;
+        u_bounds = stage.u_bounds;
+        p_bounds = stage.p_bounds;
+        
+        [x_lb, x_ub] = ocl.model.bounds(x_struct, x_bounds);
+        [x0_lb, x0_ub] = ocl.model.bounds(x_struct, x0_bounds);
+        [xF_lb, xF_ub] = ocl.model.bounds(x_struct, xF_bounds);
+        
+        [z_lb, z_ub] = ocl.model.bounds(z_struct, z_bounds);
+        [u_lb_traj, u_ub_traj] = ocl.model.boundsTrajectory(u_struct, u_bounds, N);
+        [p_lb, p_ub] = ocl.model.bounds(p_struct, p_bounds);
+        
+        varsStruct = ocl.simultaneous.variablesStruct(N, x_struct, vi_struct, u_struct, p_struct);
+        
+        ig = ocl.simultaneous.getInitialGuess(H_norm, T, nx, ni, nu, np, ...
+                                    x0_lb, x0_ub, xF_lb, xF_ub, x_lb, x_ub, ...
+                                    z_lb, z_ub, u_lb_traj, u_ub_traj, p_lb, p_ub, ...
+                                    vi_struct);
+                                  
+        igList{k} = ocl.Variable.create(varsStruct, ig);
+      end
+    end
+    
+    function [sol,times,objective,constraints] = solve(self,v0)
+      % solve(initialGuess)
+      
+      solveTotalTic = tic;
+      
+      stage_list = self.stageList;
+      collocation_list = self.collocationList;
+      
+      lbv = cell(length(stage_list),1);
+      ubv = cell(length(stage_list),1);
+      for k=1:length(stage_list)
+        
+        stage = stage_list{k};
+        colloc = collocation_list{k};
+        
+        nx = stage.nx;
+        nu = stage.nu;
+        np = stage.np;
+        H_norm = stage.H_norm;
+        T = stage.T;
+        N = length(H_norm);
+        
+        x_struct = stage.x_struct;
+        z_struct = stage.z_struct;
+        u_struct = stage.u_struct;
+        p_struct = stage.p_struct;
+        
+        x_bounds = stage.x_bounds;
+        x0_bounds = stage.x0_bounds;
+        xF_bounds = stage.xF_bounds;
+        z_bounds = stage.z_bounds;
+        u_bounds = stage.u_bounds;
+        p_bounds = stage.p_bounds;
+        
+        vi_struct = colloc.vars;
+        ni = colloc.num_i;
+        
+        [x_lb, x_ub] = ocl.model.bounds(x_struct, x_bounds);
+        [x0_lb, x0_ub] = ocl.model.bounds(x_struct, x0_bounds);
+        [xF_lb, xF_ub] = ocl.model.bounds(x_struct, xF_bounds);
+        
+        [z_lb, z_ub] = ocl.model.bounds(z_struct, z_bounds);
+        [u_lb_traj, u_ub_traj] = ocl.model.boundsTrajectory(u_struct, u_bounds, N);
+        [p_lb, p_ub] = ocl.model.bounds(p_struct, p_bounds);
+        
+        [vi_lb, vi_ub] = ocl.collocation.bounds(vi_struct, x_lb, x_ub, z_lb, z_ub);
+        
+        [lbv_stage, ubv_stage] = ocl.simultaneous.bounds(H_norm, T, nx, ni, nu, np, ...
+                                      x_lb, x_ub, x0_lb, x0_ub, xF_lb, xF_ub, ...
+                                      vi_lb, vi_ub, u_lb_traj, u_ub_traj, p_lb, p_ub);
+        lbv{k} = lbv_stage;
+        ubv{k} = ubv_stage;
+      end
+      
+      v0 = vertcat(v0{:});
+      lbv = vertcat(lbv{:});
+      ubv = vertcat(ubv{:});
+ 
+      args = struct;
+      args.lbg = self.nlpData.constraints_LB;
+      args.ubg = self.nlpData.constraints_UB;
+      args.p = [];
+      args.lbx = lbv;
+      args.ubx = ubv;
+      args.x0 = v0;
+      
+      % execute solver
+      solveCasadiTic = tic;
+      sol = self.nlpData.solver.call(args);
+      solveCasadiTime = toc(solveCasadiTic);
+      
+      if strcmp(self.nlpData.solver.stats().return_status, 'NonIpopt_Exception_Thrown')
+        ocl.utils.warning('Solver was interrupted by user.');
+      end
+      
+      sol_values = sol.x.full();
+      
+      sol = cell(length(stage_list),1);
+      times = cell(length(stage_list),1);
+      objective = cell(length(stage_list),1);
+      constraints = cell(length(stage_list),1);
+      i_stage = 1;
+      nlpFunEvalTic = tic;
+      for k=1:length(stage_list)
+        
+        stage = stage_list{k};
+        colloc = collocation_list{k};
+        
+        nx = stage.nx;
+        nu = stage.nu;
+        nz = stage.nz;
+        np = stage.np;
+        N = stage.N;
+        
+        x_struct = stage.x_struct;
+        u_struct = stage.u_struct;
+        p_struct = stage.p_struct;
+        
+        ni = colloc.num_i;
+        d = colloc.order;
+        tau_root = colloc.tau_root;
+        
+        nv_stage = ocl.simultaneous.nvars(N, nx, ni, nu, np);
+        
+        % unpack solution of this stage to state/controls trajectories
+        V = sol_values(i_stage:i_stage+nv_stage-1);
+        [X,I,U,P,H] = ocl.simultaneous.variablesUnpack(V, N, nx, ni, nu, np);
+        
+        T0 = [0, cumsum(H(:,1:end-1))];
+        
+        x_times = zeros(1, 1+d*N);
+        x_traj = zeros(nx, 1+d*N);
+        x_traj(:,1) = X(:,1);
+        x_times(1) = T0(1);
+        i_x = 2;
+        for j=1:N      
+          [xi, ~] =  ocl.collocation.variablesUnpack(I(:,j), nx, nz, d);
+          x_traj(:,i_x:i_x+d-1) = xi;
+          x_times(i_x:i_x+d-1) = T0(j) + ocl.collocation.times(tau_root, H(j));
+          i_x = i_x + d;
+        end
+        
+        z_times = zeros(1, d*N);
+        z_traj = zeros(nz, d*N);
+        i_z = 1;
+        for j=1:N
+          [~, zi] =  ocl.collocation.variablesUnpack(I(:,j), nx, nz, d);
+          z_traj(:,i_z:i_z+d-1) = zi;
+          z_times(i_z:i_z+d-1) = T0(j) + ocl.collocation.times(tau_root, H(j));
+          i_z = i_z + d;
+        end
+        
+        u_traj = U;
+        u_times = T0;
+        
+        % store solution into trajectory structures
+        sol_struct = ocl.types.Structure();
+        times_struct = ocl.types.Structure();
+        
+        sol_struct.add('states', x_struct);
+        times_struct.add('states', [1,1]);
+        for j=1:N
+          for j_d=1:d
+            sol_struct.add('states', x_struct);
+            times_struct.add('states', [1,1]);
+          end
+        end
+        
+        for j=1:N
+          for j_d=1:d
+            sol_struct.add('algvars', z_struct);
+            times_struct.add('algvars', [1,1]);
+          end
+        end
+        
+        for j=1:N
+          sol_struct.add('controls', u_struct);
+          times_struct.add('controls', [1,1]);
+        end
+        
+        sol_struct.add('parameters', p_struct);
+        
+        for j=1:N
+          sol_struct.add('h', [1 1]);
+        end
+        
+        sol_out = ocl.Variable.create(sol_struct, 0);
+        sol_out.states.set(x_traj);
+        sol_out.algvars.set(z_traj);
+        sol_out.controls.set(u_traj);
+        sol_out.parameters.set(P(:,1));
+        sol_out.h.set(H);
+        
+        times_out = ocl.Variable.create(times_struct, 0);        
+        times_out.states.set(x_times);
+        times_out.algvars.set(z_times);
+        times_out.controls.set(u_times);
+        
+        i_stage = i_stage + nv_stage;
+        
+        sol{k} = sol_out;
+        times{k} = times_out;
+        objective{k} = 0;
+        constraints{k} = 0;
+      end
+      nlpFunEvalTime = toc(nlpFunEvalTic);
+      
+      self.timeMeasures.solveTotal      = toc(solveTotalTic);
+      self.timeMeasures.solveCasadi     = solveCasadiTime;
+      self.timeMeasures.nlpFunEval      = nlpFunEvalTime;
+    end
+  end
+  
+end
diff --git a/CasadiLibrary/CasadiVariable.m b/+ocl/+casadi/CasadiVariable.m
old mode 100755
new mode 100644
similarity index 70%
rename from CasadiLibrary/CasadiVariable.m
rename to +ocl/+casadi/CasadiVariable.m
index 44b9c59b..09d07dc8
--- a/CasadiLibrary/CasadiVariable.m
+++ b/+ocl/+casadi/CasadiVariable.m
@@ -2,7 +2,7 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-classdef CasadiVariable < Variable
+classdef CasadiVariable < ocl.Variable
   
   properties
     mx
@@ -12,19 +12,15 @@
   
   
     function var = createFromValue(type,value)
-      oclValue = OclValue(value);
+      oclValue = ocl.types.Value(value);
       [N,M,K] = size(type);
       p = reshape(1:N*M*K,N,M,K);
-      var = CasadiVariable(type,p,isa(value,'casadi.MX'),oclValue);
+      var = ocl.casadi.CasadiVariable(type,p,isa(value,'casadi.MX'),oclValue);
     end
     
     function var = create(type,mx)
-      if isa(type,'OclTree')
-        names = fieldnames(type.children);
-        id = [names{:}];
-      else
-        id = class(type);
-      end
+      
+      id = class(type);
       
       [N,M,K] = size(type);
       assert(K==1,'Not supported.');
@@ -35,16 +31,16 @@
       else
         vv = casadi.SX.sym(id,N,M);
       end
-      val = OclValue(vv);
+      val = ocl.types.Value(vv);
       p = reshape(1:N*M*K,N,M,K);
-      var = CasadiVariable(type,p,mx,val);
+      var = ocl.casadi.CasadiVariable(type,p,mx,val);
     end
     
     function obj = Matrix(size,mx)
       if nargin==1
         mx = false;
       end
-      obj = CasadiVariable.create(OclMatrix(size),mx);
+      obj = ocl.casadi.CasadiVariable.create(ocl.types.Matrix(size),mx);
     end
   end
   
@@ -53,7 +49,7 @@
     function self = CasadiVariable(type,positions,mx,val)
       % CasadiVariable(type,positions,mx,val)
       narginchk(4,4);      
-      self = self@Variable(type,positions,val);
+      self = self@ocl.Variable(type,positions,val);
       self.mx = mx;      
     end
     
diff --git a/+ocl/+collocation/bounds.m b/+ocl/+collocation/bounds.m
new file mode 100644
index 00000000..aca86cbf
--- /dev/null
+++ b/+ocl/+collocation/bounds.m
@@ -0,0 +1,15 @@
+function [lb,ub] = bounds(vi_struct, x_lb, x_ub, z_lb, z_ub)
+
+ni = length(vi_struct);
+
+lb_var = ocl.Variable.create(vi_struct, -inf * ones(ni, 1));
+ub_var = ocl.Variable.create(vi_struct, inf * ones(ni, 1));
+
+lb_var.states.set(x_lb);
+ub_var.states.set(x_ub);
+
+lb_var.algvars.set(z_lb);
+ub_var.algvars.set(z_ub);
+
+lb = lb_var.value;
+ub = ub_var.value;
diff --git a/+ocl/+collocation/coefficients.m b/+ocl/+collocation/coefficients.m
index 25964d4a..4c46b725 100644
--- a/+ocl/+collocation/coefficients.m
+++ b/+ocl/+collocation/coefficients.m
@@ -1,3 +1,15 @@
+% This function is derived from:
+%
+% An implementation of direct collocation
+% Joel Andersson, 2016
+% https://github.com/casadi/casadi/blob/master/docs/examples/matlab/direct_collocation.m
+%
+% CasADi -- A symbolic framework for dynamic optimization.
+% Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
+%                         K.U. Leuven. All rights reserved.
+% Copyright (C) 2011-2014 Greg Horn
+% Under GNU Lesser General Public License
+%
 function coeff = coefficients(tau_root)
 d = length(tau_root)-1; % order
 coeff = zeros(d, d+1);
diff --git a/+ocl/+collocation/equations.m b/+ocl/+collocation/equations.m
new file mode 100644
index 00000000..78591ecc
--- /dev/null
+++ b/+ocl/+collocation/equations.m
@@ -0,0 +1,52 @@
+% This function is derived from:
+%
+% An implementation of direct collocation
+% Joel Andersson, 2016
+% https://github.com/casadi/casadi/blob/master/docs/examples/matlab/direct_collocation.m
+%
+% CasADi -- A symbolic framework for dynamic optimization.
+% Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
+%                         K.U. Leuven. All rights reserved.
+% Copyright (C) 2011-2014 Greg Horn
+% Under GNU Lesser General Public License
+%
+function [xF, costs, equations] = ...
+  equations(colloc, x0, vars, u, h, params)
+
+C = colloc.coeff_der;
+B = colloc.coeff_int;
+
+d = colloc.order;
+
+nx = colloc.num_x;
+nz = colloc.num_z;
+
+equations = cell(d,1);
+J = 0;
+
+[x,z] = ocl.collocation.variablesUnpack(vars, nx, nz, d);
+
+% Loop over collocation points
+for j=1:d
+  
+  x_der = C(1,j+1)*x0;
+  for r=1:d
+    x_r = x(:,r);
+    x_der = x_der + C(r+1,j+1)*x_r;
+  end
+  
+  x_j = x(:,j);
+  z_j = z(:,j);
+  
+  [ode,alg] = colloc.daefun(x_j, z_j, u, params);
+  
+  qj = colloc.pathcostfun(x_j, z_j,u,params);
+  
+  equations{j} = [h*ode-x_der; alg];
+  J = J + B(j+1)*qj*h;
+end
+
+costs = J;
+equations = vertcat(equations{:});
+
+xF = ocl.collocation.getStateAtPoint(colloc, x0, vars, 1.0);
diff --git a/+ocl/+collocation/evalCoefficients.m b/+ocl/+collocation/evalCoefficients.m
index b7b32820..b501f7c4 100644
--- a/+ocl/+collocation/evalCoefficients.m
+++ b/+ocl/+collocation/evalCoefficients.m
@@ -1,6 +1,18 @@
+% This function is derived from:
+%
+% An implementation of direct collocation
+% Joel Andersson, 2016
+% https://github.com/casadi/casadi/blob/master/docs/examples/matlab/direct_collocation.m
+%
+% CasADi -- A symbolic framework for dynamic optimization.
+% Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
+%                         K.U. Leuven. All rights reserved.
+% Copyright (C) 2011-2014 Greg Horn
+% Under GNU Lesser General Public License
+%
 function r = evalCoefficients(coeff, d, point)
 
-oclAssert(point<=1 && point >=0);
+ocl.utils.assert(point<=1 && point >=0);
 
 r = zeros(d+1, 1);
 for j=1:d+1
diff --git a/+ocl/+collocation/evalCoefficientsDerivative.m b/+ocl/+collocation/evalCoefficientsDerivative.m
index 80422f25..14c07c6b 100644
--- a/+ocl/+collocation/evalCoefficientsDerivative.m
+++ b/+ocl/+collocation/evalCoefficientsDerivative.m
@@ -1,3 +1,15 @@
+% This function is derived from:
+%
+% An implementation of direct collocation
+% Joel Andersson, 2016
+% https://github.com/casadi/casadi/blob/master/docs/examples/matlab/direct_collocation.m
+%
+% CasADi -- A symbolic framework for dynamic optimization.
+% Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
+%                         K.U. Leuven. All rights reserved.
+% Copyright (C) 2011-2014 Greg Horn
+% Under GNU Lesser General Public License
+%
 function r = evalCoefficientsDerivative(coeff, tau_root, d)
 r = zeros(d+1, d+1);
 for k=1:d+1
diff --git a/+ocl/+collocation/evalCoefficientsIntegral.m b/+ocl/+collocation/evalCoefficientsIntegral.m
index 4eb4ddc0..0cdd08d4 100644
--- a/+ocl/+collocation/evalCoefficientsIntegral.m
+++ b/+ocl/+collocation/evalCoefficientsIntegral.m
@@ -1,6 +1,18 @@
+% This function is derived from:
+%
+% An implementation of direct collocation
+% Joel Andersson, 2016
+% https://github.com/casadi/casadi/blob/master/docs/examples/matlab/direct_collocation.m
+%
+% CasADi -- A symbolic framework for dynamic optimization.
+% Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
+%                         K.U. Leuven. All rights reserved.
+% Copyright (C) 2011-2014 Greg Horn
+% Under GNU Lesser General Public License
+%s
 function r = evalCoefficientsIntegral(coeff, d, point)
 
-oclAssert(point<=1 && point >=0);
+ocl.utils.assert(point<=1 && point >=0);
 
 r = zeros(d+1, 1);
 for k=1:d+1
diff --git a/+ocl/+collocation/indizes.m b/+ocl/+collocation/indizes.m
index d35cf887..0423f8fd 100644
--- a/+ocl/+collocation/indizes.m
+++ b/+ocl/+collocation/indizes.m
@@ -7,4 +7,4 @@
 ncv = nx+nz; 
 
 x_indizes = cell2mat(arrayfun(@(start_i) (start_i:start_i+nx-1)', (0:d-1)*ncv+1, 'UniformOutput', false));
-z_indizes = cell2mat(arrayfun(@(start_i) (start_i:start_i+nz-1)', (0:d-1)*ncv+1, 'UniformOutput', false));
+z_indizes = cell2mat(arrayfun(@(start_i) (start_i:start_i+nz-1)', (0:d-1)*ncv+nx+1, 'UniformOutput', false));
diff --git a/+ocl/+collocation/initialGuess.m b/+ocl/+collocation/initialGuess.m
new file mode 100644
index 00000000..d11f4b33
--- /dev/null
+++ b/+ocl/+collocation/initialGuess.m
@@ -0,0 +1,5 @@
+function r = initialGuess(i_vars, stateGuess, algvarGuess)
+ig = ocl.Variable.create(i_vars, 0);
+ig.states.set(stateGuess);
+ig.algvars.set(algvarGuess);
+r = ig.value;
\ No newline at end of file
diff --git a/+ocl/+collocation/times.m b/+ocl/+collocation/times.m
new file mode 100644
index 00000000..2c9fdba3
--- /dev/null
+++ b/+ocl/+collocation/times.m
@@ -0,0 +1,2 @@
+function r = times(tau_root, h)
+r = tau_root(2:end) * h;
\ No newline at end of file
diff --git a/+ocl/+collocation/variablesPack.m b/+ocl/+collocation/variablesPack.m
new file mode 100644
index 00000000..5502a3ff
--- /dev/null
+++ b/+ocl/+collocation/variablesPack.m
@@ -0,0 +1,14 @@
+function vi = variablesPack(x,z)
+
+d = size(x,2);
+nx = size(x,1);
+nz = size(z,1);
+
+[x_indizes, z_indizes] = ocl.collocation.indizes(nx,nz,d);
+
+vi = zeros(d*(nx+nz), 1);
+
+vi(x_indizes) = x;
+vi(z_indizes) = z;
+
+end
\ No newline at end of file
diff --git a/+ocl/+collocation/variablesUnpack.m b/+ocl/+collocation/variablesUnpack.m
new file mode 100644
index 00000000..f88dc709
--- /dev/null
+++ b/+ocl/+collocation/variablesUnpack.m
@@ -0,0 +1,8 @@
+function [x,z] = variablesUnpack(vi, nx, nz, d)
+
+[x_indizes, z_indizes] = ocl.collocation.indizes(nx,nz,d);
+
+x = reshape(vi(x_indizes), nx, d);
+z = reshape(vi(z_indizes), nz, d);
+
+end
\ No newline at end of file
diff --git a/+ocl/+examples/+cartpole/animate.m b/+ocl/+examples/+cartpole/animate.m
new file mode 100644
index 00000000..3e5870ea
--- /dev/null
+++ b/+ocl/+examples/+cartpole/animate.m
@@ -0,0 +1,28 @@
+function handles = animate(sol,times)
+
+global testRun
+
+pmax = max(abs(sol.states.p.value));
+
+states = sol.states.value;
+times = times.states.value;
+times = times(:);
+
+x = states(:,1);
+p = x(1);
+theta = x(2);
+l = 0.8;
+
+handles = ocl.examples.cartpole.draw_prepare(p, theta, l, pmax);
+
+for k=2:length(times)
+  t = times(k);
+  x = states(:,k);
+  dt = t-times(k-1);
+
+  ocl.examples.cartpole.draw(handles, t, x, l);
+
+  if isempty(testRun) || (testRun==false)
+    pause(dt);
+  end
+end
diff --git a/+ocl/+examples/+cartpole/dae.m b/+ocl/+examples/+cartpole/dae.m
new file mode 100644
index 00000000..73f9f494
--- /dev/null
+++ b/+ocl/+examples/+cartpole/dae.m
@@ -0,0 +1,23 @@
+function dae(daeh,x,z,u,p)
+
+M = 1;    % mass of the cart [kg]
+m = 0.1;  % mass of the ball [kg]
+l = 0.8;  % length of the rod [m]
+g = 9.81; % gravity constant [m/s^2]
+
+v = x.v; 
+theta = x.theta;
+omega = x.omega;
+F = u.F;
+
+a = (- l*m*sin(theta)*omega.^2 + F ...
+  + g*m*cos(theta)*sin(theta))/(M + m - m*cos(theta).^2);
+
+domega = (- l*m*cos(theta)*sin(theta)*omega.^2 + ...
+  F*cos(theta) + g*m*sin(theta) + ...
+  M*g*sin(theta))/(l*(M + m - m*cos(theta).^2));
+
+daeh.setODE('p',     v);
+daeh.setODE('theta', omega);
+daeh.setODE('v',     a);
+daeh.setODE('omega', domega);
diff --git a/+ocl/+examples/+cartpole/draw.m b/+ocl/+examples/+cartpole/draw.m
new file mode 100644
index 00000000..9640abef
--- /dev/null
+++ b/+ocl/+examples/+cartpole/draw.m
@@ -0,0 +1,19 @@
+function draw(handles, time, x, l)
+p = x(1);
+theta = x(2);
+t = time;
+
+[h2,h3,h4,h5] = handles{:};
+
+xB = p-l*sin(theta);
+yB = l*cos(theta);
+
+set(h2, 'String', sprintf('%.2f s', t));
+
+set(h3, 'Xdata', p)
+
+set(h4,'Xdata',[p xB]);
+set(h4,'Ydata',[0 yB]);
+
+set(h5,'Xdata',xB);
+set(h5,'Ydata',yB); 
diff --git a/+ocl/+examples/+cartpole/draw_prepare.m b/+ocl/+examples/+cartpole/draw_prepare.m
new file mode 100644
index 00000000..2b8313c5
--- /dev/null
+++ b/+ocl/+examples/+cartpole/draw_prepare.m
@@ -0,0 +1,29 @@
+function handles = draw_prepare(p, theta, l, pmax)
+
+ms = 10; % marker size
+
+figure;
+
+hold on;
+
+line([-pmax pmax], [0 0], 'color', 'k', 'Linewidth',1.5); hold on;
+line([-pmax -pmax], [-0.1 0.1], 'color', 'k', 'Linewidth',1.5); hold on;
+line([pmax pmax], [-0.1 0.1], 'color', 'k', 'Linewidth',1.5); hold on;
+
+h2 = text(-0.3,pmax, '0.00 s','FontSize',15);
+h3 = plot(p,0,'ks','MarkerSize',ms,'Linewidth',3);
+
+xB = p-l*sin(theta);
+yB = l*cos(theta);
+
+h4 = line([p xB], [0 yB],'color',[38,124,185]/255,'Linewidth',2);
+h5 = plot(xB,yB,'o', 'color',[170,85,0]/255,'MarkerSize',ms,'Linewidth',3);
+
+grid on;
+xlim([-pmax-l pmax+l]);
+ylim([-pmax-l pmax+l]);
+
+handles = {h2,h3,h4,h5};
+
+hold off;
+pause(0.1)
\ No newline at end of file
diff --git a/+ocl/+examples/+cartpole/main_cartpole_acados.m b/+ocl/+examples/+cartpole/main_cartpole_acados.m
new file mode 100644
index 00000000..887113e8
--- /dev/null
+++ b/+ocl/+examples/+cartpole/main_cartpole_acados.m
@@ -0,0 +1,36 @@
+clear all;
+
+solver = ocl.acados.Solver( ...
+  3, ...
+  'vars', @ocl.examples.cartpole.vars, ...
+  'dae', @ocl.examples.cartpole.dae, ...
+  'pathcosts', @ocl.examples.cartpole.pathcosts, ...
+  'terminalcost', @ocl.examples.cartpole.terminalcost, ...
+  'N', 100);
+
+solver.setInitialState('p', 0);
+solver.setInitialState('v', 0);
+solver.setInitialState('theta', pi);
+solver.setInitialState('omega', 0);
+ 
+solver.initialize('theta', [0 1], [pi 0]);
+
+[sol,times] = solver.solve();
+
+figure; hold on; grid on;
+ocl.stairs(times.controls, sol.controls.F/10.)
+xlabel('time [s]');
+ocl.plot(times.states, sol.states.p)
+xlabel('time [s]');
+ocl.plot(times.states, sol.states.v)
+xlabel('time [s]');
+ocl.plot(times.states, sol.states.theta)
+legend({'force [10*N]','position [m]','velocity [m/s]','theta [rad]'})
+xlabel('time [s]');
+
+ocl.examples.cartpole.animate(sol,times);
+
+
+
+
+
diff --git a/+ocl/+examples/+cartpole/main_cartpole_casadi.m b/+ocl/+examples/+cartpole/main_cartpole_casadi.m
new file mode 100644
index 00000000..d19f8228
--- /dev/null
+++ b/+ocl/+examples/+cartpole/main_cartpole_casadi.m
@@ -0,0 +1,33 @@
+solver = ocl.Solver( ...
+  3, ...
+  'vars', @ocl.examples.cartpole.vars, ...
+  'dae', @ocl.examples.cartpole.dae, ...
+  'pathcosts', @ocl.examples.cartpole.pathcosts, ...
+  'terminalcost', @ocl.examples.cartpole.terminalcost, ...
+  'N', 100, 'd', 3);
+
+solver.setInitialState('p', 0);
+solver.setInitialState('v', 0);
+solver.setInitialState('theta', pi);
+solver.setInitialState('omega', 0);
+
+solver.initialize('theta', [0 1], [pi 0]);
+
+[sol,times] = solver.solve();
+
+figure; hold on; grid on;
+ocl.stairs(times.controls, sol.controls.F/10.)
+xlabel('time [s]');
+ocl.plot(times.states, sol.states.p)
+xlabel('time [s]');
+ocl.plot(times.states, sol.states.v)
+xlabel('time [s]');
+ocl.plot(times.states, sol.states.theta)
+legend({'force [10*N]','position [m]','velocity [m/s]','theta [rad]'})
+xlabel('time [s]');
+
+ocl.examples.cartpole.animate(sol,times);
+
+
+
+
diff --git a/+ocl/+examples/+cartpole/main_cartpole_closedloop.m b/+ocl/+examples/+cartpole/main_cartpole_closedloop.m
new file mode 100644
index 00000000..ca6ad5df
--- /dev/null
+++ b/+ocl/+examples/+cartpole/main_cartpole_closedloop.m
@@ -0,0 +1,140 @@
+function main_cartpole_closedloop
+clear all;
+close all;
+
+T = 3;
+N = 60;
+
+solver = ocl.acados.Solver( ...
+  T, ...
+  'vars', @ocl.examples.cartpole.vars, ...
+  'dae', @ocl.examples.cartpole.dae, ...
+  'pathcosts', @ocl.examples.cartpole.pathcosts, ...
+  'terminalcost', @ocl.examples.cartpole.terminalcost, ...
+  'N', N, 'verbose', false);
+
+solver.setInitialState('p', 0);
+solver.setInitialState('v', 0);
+solver.setInitialState('theta', 0);
+solver.setInitialState('omega', 0);
+
+solver.initialize('theta', [0 1], [0 0]);
+
+solver.solve();
+
+x0 = [0;0;0;0];
+
+sim = ocl.Simulator(@ocl.examples.cartpole.vars, @ocl.examples.cartpole.dae);
+sim.reset(x0);
+
+draw_handles = ocl.examples.cartpole.draw_prepare(x0(1), x0(2), 0.8, 6);
+
+% log window
+log_fig = figure('menubar', 'none');
+log_window = uicontrol(log_fig, 'Style', 'listbox', ...
+  'Units',    'normalized', ...
+  'Position', [0,0,1,1], ...
+  'String',   {}, ...
+  'Min', 0, 'Max', 2, ...
+  'Value', []);
+
+data = struct;
+data.T = T;
+data.dt = T/N;
+data.draw_handles = draw_handles;
+data.t = 0;
+data.current_state = x0;
+data.force = {};
+
+% control loop
+control_timer = timer('TimerFcn', @(t,d) controller(t, d, solver, sim, log_window), ...
+  'ExecutionMode', 'fixedRate', 'Period', data.dt, 'UserData', data);
+
+start(control_timer);
+cli(control_timer);
+stop(control_timer);
+
+end
+
+function controller(t, ~, solver, sim, log_window)
+
+dt = t.UserData.dt;
+draw_handles = t.UserData.draw_handles;
+time = t.UserData.t;
+current_state = t.UserData.current_state;
+
+solver.setInitialState('p', current_state(1));
+solver.setInitialState('theta', current_state(2));
+solver.setInitialState('v', current_state(3));
+solver.setInitialState('omega', current_state(4));
+
+[sol,~] = solver.solve();
+
+u = sol.controls.F.value;
+
+sim.current_state = current_state;
+
+force = 0;
+if ~isempty(t.UserData.force)
+  force = t.UserData.force{end};
+  t.UserData.force(end) = [];
+end
+
+x = sim.step(u(1) + force, dt);
+x = x.value;
+
+if abs(x(1)) > 6
+  sim.current_state = [0;0;0;0];
+  solver.initialize('p', [0 1], [0 0]);
+  solver.initialize('theta', [0 1], [0 0]);
+  solver.initialize('v', [0 1], [0 0]);
+  solver.initialize('omega', [0 1], [0 0]);
+  solver.initialize('F', [0 1], [0 0]);
+end
+
+% draw
+ocl.examples.cartpole.draw(draw_handles, time, x, 0.8);
+
+lines = splitlines(solver.stats());
+set(log_window, 'String', lines);
+drawnow
+
+t.UserData.sol = sol;
+t.UserData.t = time + dt;
+t.UserData.current_state = sim.current_state;
+
+end
+
+function cli(t)
+
+  cli_info = [newline, 'You are now in the command line interface. ', newline, ...
+    'You can make use of ', ...
+    'the following commands:', newline, newline, ...
+    'q: quits the program', newline, ...
+    'f: applies a force of 30N', newline, ... 
+    '<number>:  applies a force of <number> Newton', newline];
+
+  disp(cli_info);
+
+  terminated = false;
+  while ~terminated
+    m = input('  cli >>', 's');
+    
+    if strcmp(m, 'q') || strcmp(m, 'x') || strcmp(m, 'c')
+      disp('exiting...')
+      stop(t);
+      terminated = true;
+    elseif strcmp(m, 'f')
+      disp('force!!')
+      t.UserData.force{end+1} = 30*sign(rand-0.5);
+    elseif ~isnan(str2double(m))
+      F = str2double(m);
+      disp(['force ', m, '!!!']);
+      t.UserData.force{end+1} = F*sign(rand-0.5);
+    else
+      disp('Command not recognized!')
+      disp(cli_info);
+    end
+  end
+
+end
\ No newline at end of file
diff --git a/+ocl/+examples/+cartpole/main_cartpole_time.m b/+ocl/+examples/+cartpole/main_cartpole_time.m
new file mode 100644
index 00000000..6d42638a
--- /dev/null
+++ b/+ocl/+examples/+cartpole/main_cartpole_time.m
@@ -0,0 +1,166 @@
+% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
+% Redistribution is permitted under the 3-Clause BSD License terms. Please
+% ensure the above copyright notice is visible in any derived work.
+%
+function [sol,times,solver] = main_cartpole_time
+
+  solver = ocl.Solver([], 'vars', @varsfun, 'dae', @daefun, ...
+    'terminalcost', @terminalcost, 'N', 40, 'd', 3);
+
+  p0 = 0; v0 = 0;
+  theta0 = 180*pi/180; omega0 = 0;
+
+  solver.setInitialBounds('p', p0);
+  solver.setInitialBounds('v', v0);
+  solver.setInitialBounds('theta', theta0);
+  solver.setInitialBounds('omega', omega0);
+  
+  solver.setInitialBounds('time', 0);
+
+  solver.setEndBounds('p', 0);
+  solver.setEndBounds('v', 0);
+  solver.setEndBounds('theta', 0);
+  solver.setEndBounds('omega', 0);
+
+  % Run solver to obtain solution
+  [sol,times] = solver.solve(solver.ig());
+
+  % visualize solution
+  figure; hold on; grid on;
+  ocl.stairs(times.controls, sol.controls.F/10.)
+  xlabel('time [s]');
+  ocl.plot(times.states, sol.states.p)
+  xlabel('time [s]');
+  ocl.plot(times.states, sol.states.v)
+  xlabel('time [s]');
+  ocl.plot(times.states, sol.states.theta)
+  legend({'force [10*N]','position [m]','velocity [m/s]','theta [rad]'})
+  xlabel('time [s]');
+
+  animate(sol,times);
+
+end
+
+function varsfun(sh)
+
+  sh.addState('p');
+  sh.addState('theta');
+  sh.addState('v');
+  sh.addState('omega');
+  
+  sh.addState('time', 'lb', 0, 'ub', 10);
+
+  sh.addControl('F', 'lb', -15, 'ub', 15);
+end
+
+function daefun(daeh,x,~,u,~)
+
+M = 1;    % mass of the cart [kg]
+m = 0.1;  % mass of the ball [kg]
+l = 0.8;  % length of the rod [m]
+g = 9.81; % gravity constant [m/s^2]
+
+v = x.v;
+theta = x.theta;
+omega = x.omega;
+F = u.F;
+
+a = (- l*m*sin(theta)*omega.^2 + F + g*m*cos(theta)*sin(theta))/(M + m - m*cos(theta).^2);
+
+domega = (- l*m*cos(theta)*sin(theta)*omega.^2 + F*cos(theta) + g*m*sin(theta) + M*g*sin(theta))/(l*(M + m - m*cos(theta).^2));
+
+daeh.setODE('p',     v);
+daeh.setODE('theta', omega);
+daeh.setODE('v',     a);
+daeh.setODE('omega', domega);
+
+daeh.setODE('time', 1);
+  
+end
+
+function terminalcost(ocl,x,~)
+  ocl.add( x.time );
+end
+
+function handles = animate(sol,times)
+
+  handles = {};
+  pmax = max(abs(sol.states.p.value));
+  
+  states = sol.states.value;
+  times = times.states.value;
+  times = times(:);
+  
+  for k=2:length(times)
+    t = times(k);
+    x = states(:,k);
+    dt = t-times(k-1);
+    
+    handles = draw(t, dt, x, [0,0,0,0], pmax, handles);
+  end
+end
+
+function handles = draw(time, dt, x, Xref, pmax, handles)
+  p = x(1);
+  theta = x(2);
+  t = time;
+
+  l = 0.8;
+  ms = 10;
+
+  if isempty(handles)
+  
+    figure;
+
+    hold on;
+    x_target = Xref(1);
+    y_target = 0;
+
+    line([-pmax pmax], [0 0], 'color', 'k', 'Linewidth',1.5); hold on;
+    line([-pmax -pmax], [-0.1 0.1], 'color', 'k', 'Linewidth',1.5); hold on;
+    line([pmax pmax], [-0.1 0.1], 'color', 'k', 'Linewidth',1.5); hold on;
+
+    plot(x_target, y_target, 'x', 'color', [38,124,185]/255, 'MarkerSize', ms, 'Linewidth', 2);
+    h2 = text(-0.3,pmax, '0.00 s','FontSize',15);
+    h3 = plot(p,0,'ks','MarkerSize',ms,'Linewidth',3);
+
+    xB = p-l*sin(theta);
+    yB = l*cos(theta);
+
+    h4 = line([p xB], [0 yB],'color',[38,124,185]/255,'Linewidth',2);
+    h5 = plot(xB,yB,'o', 'color',[170,85,0]/255,'MarkerSize',ms,'Linewidth',3);
+
+    grid on;
+    xlim([-pmax-l pmax+l]);
+    ylim([-pmax-l pmax+l]);
+
+    handles = {h2,h3,h4,h5};
+
+    hold off;
+    pause(1)
+
+  else
+    [h2,h3,h4,h5] = handles{:};
+
+    xB = p-l*sin(theta);
+    yB = l*cos(theta);
+
+    set(h2, 'String', sprintf('%.2f s', t));
+
+    set(h3, 'Xdata', p)
+
+    set(h4,'Xdata',[p xB]);
+    set(h4,'Ydata',[0 yB]);
+
+    set(h5,'Xdata',xB);
+    set(h5,'Ydata',yB); 
+  end
+
+  global testRun
+  if isempty(testRun) || (testRun==false)
+    pause(dt);
+  end
+end
+
+
+
diff --git a/+ocl/+examples/+cartpole/main_eco4wind_demo.m b/+ocl/+examples/+cartpole/main_eco4wind_demo.m
new file mode 100644
index 00000000..9c3d0346
--- /dev/null
+++ b/+ocl/+examples/+cartpole/main_eco4wind_demo.m
@@ -0,0 +1,60 @@
+clear all;
+
+T = 3;
+
+ipopt_solver = ocl.Solver( ...
+  T, ...
+  'vars', @ocl.examples.cartpole.vars, ...
+  'dae', @ocl.examples.cartpole.dae, ...
+  'pathcosts', @ocl.examples.cartpole.pathcosts, ...
+  'terminalcost', @ocl.examples.cartpole.terminalcost, ...
+  'N', 100, 'd', 3);
+
+ipopt_solver.setInitialState('p', 0);
+ipopt_solver.setInitialState('v', 0);
+ipopt_solver.setInitialState('theta', pi);
+ipopt_solver.setInitialState('omega', 0);
+
+acados_solver = ocl.acados.Solver( ...
+  T, ...
+  'vars', @ocl.examples.cartpole.vars, ...
+  'dae', @ocl.examples.cartpole.dae, ...
+  'pathcosts', @ocl.examples.cartpole.pathcosts, ...
+  'terminalcost', @ocl.examples.cartpole.terminalcost, ...
+  'N', 100);
+
+acados_solver.setInitialState('p', 0);
+acados_solver.setInitialState('v', 0);
+acados_solver.setInitialState('theta', pi);
+acados_solver.setInitialState('omega', 0);
+
+%% Run first ipopt and then acados
+[sol,times] = ipopt_solver.solve();
+
+acados_solver.initialize('p', times.states, sol.states.p, T);
+acados_solver.initialize('v', times.states, sol.states.v, T);
+acados_solver.initialize('theta', times.states, sol.states.theta, T);
+acados_solver.initialize('omega', times.states, sol.states.omega, T);
+
+acados_solver.initialize('F', times.controls, sol.controls.F, T);
+
+[sol,times] = acados_solver.solve();
+
+figure; hold on; grid on;
+ocl.stairs(times.controls, sol.controls.F/10.)
+xlabel('time [s]');
+ocl.plot(times.states, sol.states.p)
+xlabel('time [s]');
+ocl.plot(times.states, sol.states.v)
+xlabel('time [s]');
+ocl.plot(times.states, sol.states.theta)
+legend({'force [10*N]','position [m]','velocity [m/s]','theta [rad]'})
+xlabel('time [s]');
+
+ocl.examples.cartpole.animate(sol,times);
+
+
+
+
+
+
diff --git a/+ocl/+examples/+cartpole/pathcosts.m b/+ocl/+examples/+cartpole/pathcosts.m
new file mode 100644
index 00000000..7bf7c759
--- /dev/null
+++ b/+ocl/+examples/+cartpole/pathcosts.m
@@ -0,0 +1,8 @@
+function pathcosts(ch, x, z, u, p)
+
+ch.add( 1e3  * x.p^2     );
+ch.add( 1e-2 * x.v^2     );
+ch.add( 1e3  * x.theta^2 );
+ch.add( 1e-2 * x.omega^2 );
+
+ch.add( 1e-2 * u.F^2     );
diff --git a/+ocl/+examples/+cartpole/terminalcost.m b/+ocl/+examples/+cartpole/terminalcost.m
new file mode 100644
index 00000000..7f29aab8
--- /dev/null
+++ b/+ocl/+examples/+cartpole/terminalcost.m
@@ -0,0 +1,6 @@
+function terminalcost(ch, x, p)
+
+ch.add( 1e3  * x.p^2     );
+ch.add( 1e-2 * x.v^2     );
+ch.add( 1e3  * x.theta^2 );
+ch.add( 1e-2 * x.omega^2 );
diff --git a/+ocl/+examples/+cartpole/vars.m b/+ocl/+examples/+cartpole/vars.m
new file mode 100644
index 00000000..d958cc83
--- /dev/null
+++ b/+ocl/+examples/+cartpole/vars.m
@@ -0,0 +1,9 @@
+function vars(vh)
+
+vh.addState('p');
+vh.addState('theta');
+vh.addState('v');
+vh.addState('omega');
+
+vh.addControl('F', 'lb', -15, 'ub', 15);
+
diff --git a/+ocl/+examples/+mass_spring/dae.m b/+ocl/+examples/+mass_spring/dae.m
new file mode 100644
index 00000000..e51b3eea
--- /dev/null
+++ b/+ocl/+examples/+mass_spring/dae.m
@@ -0,0 +1,26 @@
+function dae(h, x, u)
+
+s = size(x);
+nx = s(1);
+
+s = size(u);
+nu = s(1);
+
+num_masses = nx/2;
+
+A = zeros(nx, nx);
+for k=1:num_masses
+	A(k, num_masses+k) = 1.0;
+	A(num_masses+k, k) = -2.0;
+end
+for k=1:num_masses-1
+	A(num_masses+k, k+1) = 1.0;
+	A(num_masses+k+1, k) = 1.0;
+end
+
+B = zeros(nx, nu);
+for k=1:nu
+	B(num_masses+k, k) = 1.0;
+end
+
+h.setODE('x', A*x+B*u);
\ No newline at end of file
diff --git a/+ocl/+examples/+mass_spring/gridcosts.m b/+ocl/+examples/+mass_spring/gridcosts.m
new file mode 100644
index 00000000..ede3ad33
--- /dev/null
+++ b/+ocl/+examples/+mass_spring/gridcosts.m
@@ -0,0 +1,13 @@
+function gridcosts(h, k, K, x)
+
+if k==K
+  s = size(x);
+  nx = s(1);
+  
+  dWx = ones(nx, 1);
+
+  yr_x = zeros(nx, 1);
+  ymyr_e = x - yr_x;
+	h.add(0.5 * ymyr_e.' * (dWx .* ymyr_e));
+end
+
diff --git a/+ocl/+examples/+mass_spring/main_acados.m b/+ocl/+examples/+mass_spring/main_acados.m
new file mode 100644
index 00000000..b974f4d1
--- /dev/null
+++ b/+ocl/+examples/+mass_spring/main_acados.m
@@ -0,0 +1,27 @@
+num_masses = 4;
+
+varsfh = @(h) ocl.examples.mass_spring.vars(h,num_masses);
+daefh = @(h,x,z,u,p) ocl.examples.mass_spring.dae(h,x,u);
+pathcostsfh = @(h,x,z,u,p) ocl.examples.mass_spring.pathcosts(h,x,u);
+gridcostsfh = @(h,k,K,x,p) ocl.examples.mass_spring.gridcosts(h,k,K,x);
+
+solver = ocl.acados.AcadosSolver(10, varsfh, daefh, pathcostsfh, gridcostsfh, 'N', 20);
+
+x0 = zeros(2*num_masses, 1);
+x0(1) = 2.5;
+x0(2) = 2.5;
+
+solver.setInitialState('x', x0);
+
+% ig = solver.getInitialGuess();
+[sol,times] = solver.solve();
+
+figure()
+subplot(2, 1, 1)
+ocl.plot(times.states, sol.states.x);
+title('trajectory')
+ylabel('x')
+subplot(2, 1, 2)
+ocl.stairs(times.controls, sol.controls.u.');
+ylabel('u')
+xlabel('sample')
\ No newline at end of file
diff --git a/+ocl/+examples/+mass_spring/main_casadi.m b/+ocl/+examples/+mass_spring/main_casadi.m
new file mode 100644
index 00000000..9ce7dc3d
--- /dev/null
+++ b/+ocl/+examples/+mass_spring/main_casadi.m
@@ -0,0 +1,26 @@
+num_masses = 4;
+
+varsfh = @(h) ocl.examples.mass_spring.vars(h,num_masses);
+daefh = @(h,x,z,u,p) ocl.examples.mass_spring.dae(h,x,u);
+pathcostsfh = @(h,x,z,u,p) ocl.examples.mass_spring.pathcosts(h,x,u);
+gridcostsfh = @(h,k,K,x,p) ocl.examples.mass_spring.gridcosts(h,k,K,x);
+
+solver = ocl.Solver(10, varsfh, daefh, pathcostsfh, gridcostsfh, 'N', 20, 'd', 2);
+
+x0 = zeros(2*num_masses, 1);
+x0(1) = 2.5;
+x0(2) = 2.5;
+
+solver.setInitialBounds('x', x0);
+
+[sol,times] = solver.solve(solver.ig());
+
+figure()
+subplot(2, 1, 1)
+ocl.plot(times.states, sol.states.x);
+title('trajectory')
+ylabel('x')
+subplot(2, 1, 2)
+ocl.stairs(times.controls, sol.controls.u.');
+ylabel('u')
+xlabel('sample')
\ No newline at end of file
diff --git a/+ocl/+examples/+mass_spring/pathcosts.m b/+ocl/+examples/+mass_spring/pathcosts.m
new file mode 100644
index 00000000..3fdea930
--- /dev/null
+++ b/+ocl/+examples/+mass_spring/pathcosts.m
@@ -0,0 +1,16 @@
+function pathcosts(h, x, u)
+
+s = size(x);
+nx = s(1);
+
+s = size(u);
+nu = s(1);
+
+yr_u = zeros(nu, 1);
+yr_x = zeros(nx, 1);
+dWu = 2*ones(nu, 1);
+dWx = ones(nx, 1);
+
+ymyr = [u; x] - [yr_u; yr_x];
+
+h.add(0.5 * ymyr.' * ([dWu; dWx] .* ymyr));
\ No newline at end of file
diff --git a/+ocl/+examples/+mass_spring/vars.m b/+ocl/+examples/+mass_spring/vars.m
new file mode 100644
index 00000000..54138cd1
--- /dev/null
+++ b/+ocl/+examples/+mass_spring/vars.m
@@ -0,0 +1,7 @@
+function vars(h, num_masses)
+
+lbx = horzcat(-4*ones(1,num_masses), -inf*ones(1,num_masses));
+ubx = horzcat(4*ones(1,num_masses), inf*ones(1,num_masses));
+
+h.addState('x', [2*num_masses 1], 'lb', lbx, 'ub', ubx);
+h.addControl('u', [num_masses-1 1], 'lb', -0.5, 'ub', 0.5);
\ No newline at end of file
diff --git a/+ocl/+examples/+pendulum/animate.m b/+ocl/+examples/+pendulum/animate.m
new file mode 100644
index 00000000..c0ba7278
--- /dev/null
+++ b/+ocl/+examples/+pendulum/animate.m
@@ -0,0 +1,36 @@
+function animate(l, p_traj, times)
+
+p = p_traj(:,1);
+
+figure;
+plot(0,0,'ob', 'MarkerSize', 22)
+hold on
+
+h_line = plot([0,p(1)],[0,p(2)],'-k', 'LineWidth', 4);
+h_bob = plot(p(1),p(2),'ok', 'MarkerSize', 22, 'MarkerFaceColor','r');
+hold off
+
+xlim([-l-0.1,l+0.1])
+ylim([-l-0.1,l+0.1])
+
+for k=1:size(p_traj, 2)-1
+  tic;
+  p = p_traj(:,k);
+  
+  set(h_line, 'XData', [0 p(1)]);
+  set(h_line, 'YData', [0 p(2)]);
+  
+  set(h_bob, 'XData', p(1));
+  set(h_bob, 'YData', p(2));
+  
+  pause(times(k+1)-times(k) - toc);
+end
+
+p = p_traj(:,end);
+
+set(h_line, 'XData', [0 p(1)]);
+set(h_line, 'YData', [0 p(2)]);
+
+set(h_bob, 'XData', p(1));
+set(h_bob, 'YData', p(2));
+
diff --git a/+ocl/+examples/+pendulum/daefun.m b/+ocl/+examples/+pendulum/daefun.m
index ae785baa..2088c296 100644
--- a/+ocl/+examples/+pendulum/daefun.m
+++ b/+ocl/+examples/+pendulum/daefun.m
@@ -1,6 +1,6 @@
-function daefun(sh,x,z,u,p)
+function daefun(sh,x,z,u,conf)
 
-  ddp = - 1/p.m * z.lambda*x.p - [0;9.81] + [u.F;0];
+  ddp = - 1/conf.m * z.lambda*x.p - [0;9.81] + [u.F;0];
 
   sh.setODE('p',x.v);
   sh.setODE('v',ddp);
diff --git a/+ocl/+examples/+pendulum/gridcosts.m b/+ocl/+examples/+pendulum/gridcosts.m
new file mode 100644
index 00000000..f381afde
--- /dev/null
+++ b/+ocl/+examples/+pendulum/gridcosts.m
@@ -0,0 +1,6 @@
+function gridcosts(ch,k,K,x,~)
+if k==K
+  ch.add(1e-6*x.time);
+end
+
+
diff --git a/+ocl/+examples/+pendulum/icfun.m b/+ocl/+examples/+pendulum/icfun.m
index 34888d5e..878f8daa 100644
--- a/+ocl/+examples/+pendulum/icfun.m
+++ b/+ocl/+examples/+pendulum/icfun.m
@@ -1,6 +1,6 @@
-function icfun(ic,x,p)
+function icfun(ic,x,conf)
   % initial condition function
-  l = p.l;
+  l = conf.l;
   p = x.p;
   v = x.v;
 
diff --git a/+ocl/+examples/+pendulum/pathcosts.m b/+ocl/+examples/+pendulum/pathcosts.m
new file mode 100644
index 00000000..68181f1a
--- /dev/null
+++ b/+ocl/+examples/+pendulum/pathcosts.m
@@ -0,0 +1,3 @@
+function pathcosts(ch,~,~,controls,~)
+F  = controls.F;
+ch.add( F^2 );
diff --git a/+ocl/+examples/+pendulum/simcallback.m b/+ocl/+examples/+pendulum/simcallback.m
deleted file mode 100644
index 476be604..00000000
--- a/+ocl/+examples/+pendulum/simcallback.m
+++ /dev/null
@@ -1,15 +0,0 @@
-function simcallback(x,~,~,t0,t1,param)
-  p = x.p.value;
-  l = param.l.value;
-  dt = t1-t0;
-
-  plot(0,0,'ob', 'MarkerSize', 22)
-  hold on
-  plot([0,p(1)],[0,p(2)],'-k', 'LineWidth', 4)
-  plot(p(1),p(2),'ok', 'MarkerSize', 22, 'MarkerFaceColor','r')
-  xlim([-l,l])
-  ylim([-l,l])
-
-  pause(dt.value);
-  hold off
-end
\ No newline at end of file
diff --git a/+ocl/+examples/+pendulum/simcallbacksetup.m b/+ocl/+examples/+pendulum/simcallbacksetup.m
deleted file mode 100644
index 85039221..00000000
--- a/+ocl/+examples/+pendulum/simcallbacksetup.m
+++ /dev/null
@@ -1,3 +0,0 @@
-function simcallbacksetup(~)
-  figure;
-end
\ No newline at end of file
diff --git a/+ocl/+examples/+pendulum/varsfun.m b/+ocl/+examples/+pendulum/varsfun.m
index 4cdee4f9..f32bfc5c 100644
--- a/+ocl/+examples/+pendulum/varsfun.m
+++ b/+ocl/+examples/+pendulum/varsfun.m
@@ -5,7 +5,4 @@ function varsfun(sh)
   
   sh.addControl('F');
   sh.addAlgVar('lambda');
-
-  sh.addParameter('m');
-  sh.addParameter('l');
 end
\ No newline at end of file
diff --git a/+ocl/+examples/bouncingball.m b/+ocl/+examples/bouncingball.m
index 8979c649..5d6f5896 100644
--- a/+ocl/+examples/bouncingball.m
+++ b/+ocl/+examples/bouncingball.m
@@ -10,19 +10,16 @@
   
   after_contact.setEndStateBounds('s', 1);
 
-  solver = OclSolver({before_contact, after_contact}, {@stage_transition});
+  solver = ocl.Solver({before_contact, after_contact}, {@stage_transition});
 
   [sol,times] = solver.solve(solver.getInitialGuess());
 
-  figure
-  spy(full(solver.jacobian_pattern(sol)))
-  
   % stage 1
   figure; 
   subplot(1,2,1)
   hold on; grid on;
-  oclPlot(times{1}.states, sol{1}.states.s)
-  oclPlot(times{1}.states, sol{1}.states.v)
+  ocl.plot(times{1}.states, sol{1}.states.s)
+  ocl.plot(times{1}.states, sol{1}.states.v)
   legend({'s','v'})
   xlabel('time [s]');
   ylim([-5 3])
@@ -32,9 +29,9 @@
   % stage 2
   subplot(1,2,2)
   hold on; grid on;
-  oclPlot(times{2}.states, sol{2}.states.s)
-  oclPlot(times{2}.states, sol{2}.states.v)
-  oclStairs(times{2}.states, [sol{2}.controls.F;sol{2}.controls.F(end)])
+  ocl.plot(times{2}.states, sol{2}.states.s)
+  ocl.plot(times{2}.states, sol{2}.states.v)
+  ocl.stairs(times{2}.controls, sol{2}.controls.F)
   legend({'s','v','F'})
   xlabel('time [s]');
   ylim([-5 3])
diff --git a/+ocl/+examples/bouncingball_sim.m b/+ocl/+examples/bouncingball_sim.m
index f23de989..729a4aea 100644
--- a/+ocl/+examples/bouncingball_sim.m
+++ b/+ocl/+examples/bouncingball_sim.m
@@ -12,14 +12,12 @@
   stage0.setEndStateBounds('s', 0);
   stage.setEndStateBounds('s', 0);
 
-  solver = OclSolver(num2cell([stage0,repmat(stage,1,num_stages-1)]), ...
-                  repmat({@transition},1,num_stages-1));
+  solver = ocl.Solver(num2cell([stage0,repmat(stage,1,num_stages-1)]), ...
+                      repmat({@transition},1,num_stages-1));
 
   [sol,times] = solver.solve(solver.getInitialGuess());
 
-  figure
-  spy(full(solver.jacobian_pattern(sol)))
-  
+
   vw = VideoWriter(fullfile(getenv('OPENOCL_WORK'),'bouncingball.avi'));
   vw.FrameRate = 30;
   open(vw)
diff --git a/+ocl/+examples/cartpole.m b/+ocl/+examples/cartpole.m
index 870724bc..a8e8a3a6 100644
--- a/+ocl/+examples/cartpole.m
+++ b/+ocl/+examples/cartpole.m
@@ -4,7 +4,8 @@
 %
 function [sol,times,solver] = cartpole
 
-  solver = ocl.Solver([], 'vars', @varsfun, 'dae', @daefun, 'gridcosts', @gridcosts, 'N', 40, 'd', 3);
+  solver = ocl.Solver([], 'vars', @varsfun, 'dae', @daefun, ...
+    'terminalcost', @terminalcost, 'N', 40, 'd', 3);
 
   p0 = 0; v0 = 0;
   theta0 = 180*pi/180; omega0 = 0;
@@ -26,13 +27,13 @@
 
   % visualize solution
   figure; hold on; grid on;
-  oclStairs(times.controls, sol.controls.F/10.)
+  ocl.stairs(times.controls, sol.controls.F/10.)
   xlabel('time [s]');
-  oclPlot(times.states, sol.states.p)
+  ocl.plot(times.states, sol.states.p)
   xlabel('time [s]');
-  oclPlot(times.states, sol.states.v)
+  ocl.plot(times.states, sol.states.v)
   xlabel('time [s]');
-  oclPlot(times.states, sol.states.theta)
+  ocl.plot(times.states, sol.states.theta)
   legend({'force [10*N]','position [m]','velocity [m/s]','theta [rad]'})
   xlabel('time [s]');
 
@@ -80,10 +81,8 @@ function daefun(sh,x,~,u,~)
   
 end
 
-function gridcosts(self,k,K,x,~)
-  if k == K
-    self.add( x.time );
-  end
+function terminalcost(ocl,x,~)
+  ocl.add( x.time );
 end
 
 function handles = animate(sol,times)
@@ -91,8 +90,8 @@ function gridcosts(self,k,K,x,~)
   handles = {};
   pmax = max(abs(sol.states.p.value));
   
-  states = sol.integrator.states.value;
-  times = times.integrator.value;
+  states = sol.states.value;
+  times = times.states.value;
   times = times(:);
   
   snap_at = floor(linspace(2,length(times),4));
@@ -136,7 +135,7 @@ function gridcosts(self,k,K,x,~)
     h2 = text(-0.3,pmax, '0.00 s','FontSize',15);
     h3 = plot(p,0,'ks','MarkerSize',ms,'Linewidth',3);
 
-    xB = p-l*sin(theta);
+    xB = p+l*sin(theta);
     yB = l*cos(theta);
 
     h4 = line([p xB], [0 yB],'color',[38,124,185]/255,'Linewidth',2);
diff --git a/+ocl/+examples/pendulum.m b/+ocl/+examples/pendulum.m
index 61e3b3ec..baf8d194 100644
--- a/+ocl/+examples/pendulum.m
+++ b/+ocl/+examples/pendulum.m
@@ -3,52 +3,38 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function [vars,times,solver] = pendulum
-
-  solver = ocl.Solver([], ...
-                      @ocl.examples.pendulum.varsfun, ...
-                      @ocl.examples.pendulum.daefun, ...
-                      @pathcosts, @gridcosts, ...
-                      'callback_setup', @ocl.examples.pendulum.simcallbacksetup, ...
-                      'callback', @ocl.examples.pendulum.simcallback, ...
-                      'N', 40);
-  
-  solver.setBounds('time', 0, 15);
-  
-  solver.setBounds('p',       -[1;1], [1;1]);
-  solver.setBounds('v',       -[3;3], [3;3]);
-  solver.setBounds('F',       -25, 25);
-  solver.setBounds('lambda',  -50, 50);
-  solver.setBounds('m',       1);
-  solver.setBounds('l',       1);
-
-  solver.setInitialBounds('time', 0);
-  solver.setInitialBounds('p', [0;-1],[0;-1]);
-  solver.setInitialBounds('v', [0.5;0]);
-
-  solver.setEndBounds('p',     [0,1]);
-  solver.setEndBounds('v',     [-1;-1], [1;1]);
-
-  vars = solver.getInitialGuess();
-  vars.states.p.set([0;-1]);
-  vars.states.v.set([0.1;0]);
-  vars.controls.F.set(-10);
-
-  [solution,times] = solver.solve(vars);
-
-  figure
-  solver.solutionCallback(times,solution);
-
-  snapnow;
-end
-
-function gridcosts(ch,k,K,x,~)
-  if k==K
-    ch.add(1e-6*x.time);
-  end
-end
-
-function pathcosts(ch,~,~,controls,~)
-  F  = controls.F;
-  ch.add( F^2 );
-end
+function [ig,times,solver] = pendulum
+
+conf = struct;
+conf.l = 1;
+conf.m = 1;
+
+solver = ocl.Solver([], ...
+  @ocl.examples.pendulum.varsfun, ...
+  @(h,x,z,u,p) ocl.examples.pendulum.daefun(h,x,z,u,conf), ...
+  @ocl.examples.pendulum.pathcosts, ...
+  @ocl.examples.pendulum.gridcosts, ...
+  'N', 100);
+
+solver.setBounds('time', 0, 15);
+
+solver.setBounds('v',       -[3;3], [3;3]);
+solver.setBounds('F',       -40, 40);
+
+solver.setInitialBounds('time', 0);
+solver.setInitialBounds('p', [0; -conf.l]);
+solver.setInitialBounds('v', [0.5;0]);
+
+solver.setEndBounds('p',     [0,0], [0,inf]);
+solver.setEndBounds('v',     [-1;-1], [1;1]);
+
+ig = solver.getInitialGuess();
+ig.states.p.set([0;-1]);
+ig.states.v.set([0.1;0]);
+ig.controls.F.set(-10);
+
+[solution,times] = solver.solve(ig);
+
+ocl.examples.pendulum.animate(conf.l, solution.states.p.value, times.value);
+
+snapnow;
diff --git a/+ocl/+examples/pendulum_sim.m b/+ocl/+examples/pendulum_sim.m
index 62776fd0..2a5c27bc 100644
--- a/+ocl/+examples/pendulum_sim.m
+++ b/+ocl/+examples/pendulum_sim.m
@@ -3,31 +3,37 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function xVec = pendulum_sim
+function p_vec = pendulum_sim
 
-  system = ocl.System(@ocl.examples.pendulum.varsfun, ...
-                      @ocl.examples.pendulum.daefun, ...
-                      @ocl.examples.pendulum.icfun, ...
-                      'callbacksetup', @ocl.examples.pendulum.simcallbacksetup, ...
-                      'callback', @ocl.examples.pendulum.simcallback);
-  simulator = ocl.Simulator(system);
+  conf = struct;
+  conf.l = 1;
+  conf.m = 1;
+  
+  simulator = ocl.Simulator(@ocl.examples.pendulum.varsfun, ...
+                      @(h,x,z,u,p) ocl.examples.pendulum.daefun(h,x,z,u,conf), ...
+                      @(h,x,p) ocl.examples.pendulum.icfun(h,x,conf));
 
-  states = simulator.getStates();
-  states.p.set([0,1]);
-  states.v.set([-0.5,-1]);
-
-  p = simulator.getParameters();
-  p.m.set(1);
-  p.l.set(1);
+  x0 = simulator.getStates();
+  x0.p.set([0,conf.l]);
+  x0.v.set([-0.5,-1]);
 
   times = 0:0.1:4;
-
-  % simulate using a given series of control inputs
-  controlsSeries = simulator.getControlsVec(length(times)-1);
-  controlsSeries.F.set(10);
-
+ 
   figure
-  [xVec,~,~] = simulator.simulate(states,times,controlsSeries,p,true);
+  simulator.reset(x0);
+  
+  p_vec = zeros(2,length(times));
+  p_vec(:,1) = x0.p.value; 
+  
+  for k=1:length(times)-1
+    
+    dt = times(k+1)-times(k);
+    [x,~] = simulator.step(10, dt);
+    
+    p_vec(:,k+1) = x.p.value;
+  end
+  
+  ocl.examples.pendulum.animate(conf.l, p_vec, times);
   snapnow;
 end
 
diff --git a/+ocl/+model/bounds.m b/+ocl/+model/bounds.m
new file mode 100644
index 00000000..4f03ba86
--- /dev/null
+++ b/+ocl/+model/bounds.m
@@ -0,0 +1,19 @@
+function [lb, ub] = bounds(var_struct, bounds)
+
+lb_var = ocl.Variable.create(var_struct, -inf);
+ub_var = ocl.Variable.create(var_struct, inf);
+
+for k=1:length(bounds.data)
+  
+  d = bounds.data{k};
+  id = d.id;
+  lower = d.lower;
+  upper = d.upper;
+  
+  lb_var.get(id).set(lower);
+  ub_var.get(id).set(upper);
+  
+end
+
+lb = lb_var.value;
+ub = ub_var.value;
\ No newline at end of file
diff --git a/+ocl/+model/boundsTrajectory.m b/+ocl/+model/boundsTrajectory.m
new file mode 100644
index 00000000..e8c21cb6
--- /dev/null
+++ b/+ocl/+model/boundsTrajectory.m
@@ -0,0 +1,25 @@
+function [lb, ub] = boundsTrajectory(var_struct, bounds, N)
+
+traj_struct = ocl.types.Structure();
+traj_struct.addRepeated({'traj'}, {var_struct}, N);
+
+lb_var = ocl.Variable.create(traj_struct, -inf);
+ub_var = ocl.Variable.create(traj_struct, inf);
+
+lb_var = lb_var.traj;
+ub_var = ub_var.traj;
+
+for k=1:length(bounds.data)
+  
+  d = bounds.data{k};
+  id = d.id;
+  lower = d.lower;
+  upper = d.upper;
+  
+  lb_var.get(id).set(lower);
+  ub_var.get(id).set(upper);
+  
+end
+
+lb = lb_var.value;
+ub = ub_var.value;
\ No newline at end of file
diff --git a/+ocl/+model/dae.m b/+ocl/+model/dae.m
new file mode 100644
index 00000000..2e194f34
--- /dev/null
+++ b/+ocl/+model/dae.m
@@ -0,0 +1,18 @@
+function [ode,alg] = dae(daefh, ...
+  x_struct, z_struct, u_struct, p_struct, x_order, ...
+  x, z, u, p)
+% evaluate the system equations for the assigned variables
+
+x = ocl.Variable.create(x_struct,x);
+z = ocl.Variable.create(z_struct,z);
+u = ocl.Variable.create(u_struct,u);
+p = ocl.Variable.create(p_struct,p);
+
+daehandler = ocl.DaeHandler();
+daefh(daehandler,x,z,u,p);
+
+nx = length(x_struct);
+nz = length(z_struct);
+
+ode = daehandler.getOde(nx, x_order);
+alg = daehandler.getAlg(nz);
\ No newline at end of file
diff --git a/+ocl/+model/gridconstraints.m b/+ocl/+model/gridconstraints.m
new file mode 100644
index 00000000..7d3c4805
--- /dev/null
+++ b/+ocl/+model/gridconstraints.m
@@ -0,0 +1,11 @@
+function [val,lb,ub] = gridconstraints(fh,x_struct, p_struct, k, N, x, p)
+gridConHandler = ocl.Constraint();
+x = ocl.Variable.create(x_struct,x);
+p = ocl.Variable.create(p_struct,p);
+
+fh(gridConHandler,k,N,x,p);
+
+val = gridConHandler.values;
+lb = gridConHandler.lowerBounds;
+ub = gridConHandler.upperBounds;
+
diff --git a/+ocl/+model/gridcosts.m b/+ocl/+model/gridcosts.m
new file mode 100644
index 00000000..16b8152d
--- /dev/null
+++ b/+ocl/+model/gridcosts.m
@@ -0,0 +1,9 @@
+function r = gridcosts(fh, x_struct, p_struct, k, K, x, p)
+gridCostHandler = ocl.Cost();
+
+x = ocl.Variable.create(x_struct,x);
+p = ocl.Variable.create(p_struct,p);
+
+fh(gridCostHandler,k,K,x,p);
+
+r = gridCostHandler.value;
\ No newline at end of file
diff --git a/+ocl/+model/ic.m b/+ocl/+model/ic.m
new file mode 100644
index 00000000..60e1d1b8
--- /dev/null
+++ b/+ocl/+model/ic.m
@@ -0,0 +1,9 @@
+function ic = ic(icfh, x_struct,p_struct,x,p)
+% initial condition function
+icHandler = ocl.Constraint();
+x = ocl.Variable.create(x_struct,x);
+p = ocl.Variable.create(p_struct,p);
+icfh(icHandler,x,p)
+ic = icHandler.values;
+assert(all(icHandler.lowerBounds==0) && all(icHandler.upperBounds==0),...
+  'In initial condition are only equality constraints allowed.');
\ No newline at end of file
diff --git a/+ocl/+model/pathcosts.m b/+ocl/+model/pathcosts.m
new file mode 100644
index 00000000..cdba2858
--- /dev/null
+++ b/+ocl/+model/pathcosts.m
@@ -0,0 +1,13 @@
+function r = pathcosts(fh, x_struct, z_struct, u_struct, p_struct, x, z, u, p)
+% ocl.model.pathcosts(pathcostsfh, states, algvars, controls, parameters, x,z,u,p)
+%
+pcHandler = ocl.Cost();
+
+x = ocl.Variable.create(x_struct,x);
+z = ocl.Variable.create(z_struct,z);
+u = ocl.Variable.create(u_struct,u);
+p = ocl.Variable.create(p_struct,p);
+
+fh(pcHandler,x,z,u,p);
+
+r = pcHandler.value;
diff --git a/+ocl/+model/terminalcost.m b/+ocl/+model/terminalcost.m
new file mode 100644
index 00000000..1483e3f4
--- /dev/null
+++ b/+ocl/+model/terminalcost.m
@@ -0,0 +1,11 @@
+function r = terminalcost(fh, x_struct, p_struct, x, p)
+% ocl.model.pathcosts(pathcostsfh, states, parameters, x, p)
+%
+pcHandler = ocl.Cost();
+
+x = ocl.Variable.create(x_struct,x);
+p = ocl.Variable.create(p_struct,p);
+
+fh(pcHandler,x,p);
+
+r = pcHandler.value;
diff --git a/+ocl/+model/vars.m b/+ocl/+model/vars.m
new file mode 100644
index 00000000..0a043aee
--- /dev/null
+++ b/+ocl/+model/vars.m
@@ -0,0 +1,18 @@
+function [x_struct, z_struct, u_struct, p_struct, ...
+          x_bounds, z_bounds, u_bounds, p_bounds, ...
+          x_order] = vars(varsfh)
+        
+vh = ocl.VarHandler;
+varsfh(vh);
+
+x_struct = vh.x_struct;
+z_struct = vh.z_struct;
+u_struct = vh.u_struct;
+p_struct = vh.p_struct;
+
+x_bounds = vh.x_bounds;
+z_bounds = vh.z_bounds;
+u_bounds = vh.u_bounds;
+p_bounds = vh.p_bounds;
+
+x_order = vh.x_order;
\ No newline at end of file
diff --git a/+ocl/+simultaneous/bounds.m b/+ocl/+simultaneous/bounds.m
new file mode 100644
index 00000000..a707fe0e
--- /dev/null
+++ b/+ocl/+simultaneous/bounds.m
@@ -0,0 +1,47 @@
+function [lb_stage,ub_stage] = bounds(H_norm, T, nx, ni, nu, np, ...
+                                      x_lb, x_ub, x0_lb, x0_ub, xF_lb, xF_ub, ...
+                                      vi_lb, vi_ub, u_lb_traj, u_ub_traj, p_lb, p_ub)
+
+N = length(H_norm);
+nv_stage = ocl.simultaneous.nvars(N, nx, ni, nu, np);
+
+lb_stage = -inf * ones(nv_stage,1);
+ub_stage = inf * ones(nv_stage,1);
+
+[X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = ocl.simultaneous.indizes(N, nx, ni, nu, np);
+
+% states
+for m=1:size(X_indizes,2)
+  lb_stage(X_indizes(:,m)) = x_lb;
+  ub_stage(X_indizes(:,m)) = x_ub;
+end
+
+% Merge the two vectors of bound values for lower bounds and upper bounds.
+% Bound values can only get narrower, e.g. higher for lower bounds.
+lb_stage(X_indizes(:,1)) = max(x_lb, x0_lb);
+ub_stage(X_indizes(:,1)) = min(x_ub, x0_ub);
+
+lb_stage(X_indizes(:,end)) = max(x_lb, xF_lb);
+ub_stage(X_indizes(:,end)) = min(x_ub, xF_ub);
+
+% integrator bounds
+for m=1:size(I_indizes,2)
+  lb_stage(I_indizes(:,m)) = vi_lb;
+  ub_stage(I_indizes(:,m)) = vi_ub;
+end
+
+% controls
+lb_stage(U_indizes) = u_lb_traj;
+ub_stage(U_indizes) = u_ub_traj;
+
+% parameters (only set the initial parameters)
+lb_stage(P_indizes(:,1)) = p_lb;
+ub_stage(P_indizes(:,1)) = p_ub;
+
+% timesteps
+if isempty(T)
+  lb_stage(H_indizes) = 0.0;
+else
+  lb_stage(H_indizes) = H_norm * T;
+  ub_stage(H_indizes) = H_norm * T;
+end
\ No newline at end of file
diff --git a/+ocl/+simultaneous/equations.m b/+ocl/+simultaneous/equations.m
index 48fd81d0..2145d516 100644
--- a/+ocl/+simultaneous/equations.m
+++ b/+ocl/+simultaneous/equations.m
@@ -1,24 +1,10 @@
-function [costs,constraints,constraints_lb,constraints_ub,times,x0,p0] = ...
-  equations(stage, stage_vars, controls_regularization, ...
+function [costs,constraints,constraints_lb,constraints_ub,x0,p0] = ...
+  equations(H_norm, T, nx, ni, nu, np, gridcostfun, gridconstraintfun, ...
+            terminalcostfun, integratormap, stage_vars, controls_regularization, ...
             controls_regularization_value)
 
-H_norm = stage.H_norm;
-T = stage.T;
-nx = stage.nx;
-ni = stage.integrator.num_i;
-nu = stage.nu;
-np = stage.np;
-gridcost_fun = @stage.gridcostfun;
-gridcon_fun = @stage.gridconstraintfun;
-
-[~,N] = ocl.simultaneous.nvars(H_norm, nx, ni, nu, np);
-[X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = ocl.simultaneous.getStageIndizes(stage);
-
-X = reshape(stage_vars(X_indizes), nx, N+1);
-I = reshape(stage_vars(I_indizes), ni, N);
-U = reshape(stage_vars(U_indizes), nu, N);
-P = reshape(stage_vars(P_indizes), np, N+1);
-H = reshape(stage_vars(H_indizes), 1 , N);
+N = length(H_norm);
+[X,I,U,P,H] = ocl.simultaneous.variablesUnpack(stage_vars, N, nx, ni, nu, np);
 
 % grid constraints, grid costs
 gridcon = cell(1,N+1);
@@ -26,10 +12,12 @@
 gridcon_ub = cell(1,N+1);
 gridcost = 0;
 for k=1:N+1
-  [gridcon{k}, gridcon_lb{k}, gridcon_ub{k}] = gridcon_fun(k, N+1, X(:,k), P(:,k));
-  gridcost = gridcost + gridcost_fun(k, N+1, X(:,k), P(:,k));
+  [gridcon{k}, gridcon_lb{k}, gridcon_ub{k}] = gridconstraintfun(k, N+1, X(:,k), P(:,k));
+  gridcost = gridcost + gridcostfun(k, N+1, X(:,k), P(:,k));
 end
 
+gridcost = gridcost + terminalcostfun(X(:,N+1), P(:,N+1));
+
 % point costs
 % grid = ocl.simultaneous.normalizedStateTimes(stage);
 % grid_N = grid(1:end-1,:);
@@ -43,7 +31,7 @@
 %   
 % end
 
-[xend_arr, cost_arr, int_eq_arr, int_times] = stage.integratormap(X(:,1:end-1), I, U, H, P(:,1:end-1));
+[xend_arr, cost_arr, int_eq_arr] = integratormap(X(:,1:end-1), I, U, H, P(:,1:end-1));
 
 % timestep constraints
 h_eq = double.empty(0,N-1);
@@ -108,13 +96,5 @@
   costs = costs + controls_regularization_value*(Uvec'*Uvec);
 end
 
-% times output
-T0 = [0, cumsum(H(:,1:end-1))];
-for k=1:size(int_times,1)
-  int_times(k,:) = T0 + int_times(k,:);
-end
-times = [T0; int_times; T0];
-times = [times(:); T0(end)+H(end)];
-
 x0 = X(:,1);
 p0 = P(:,1);
\ No newline at end of file
diff --git a/+ocl/+simultaneous/getBounds.m b/+ocl/+simultaneous/getBounds.m
deleted file mode 100644
index 97a80f17..00000000
--- a/+ocl/+simultaneous/getBounds.m
+++ /dev/null
@@ -1,46 +0,0 @@
-function [lb_stage,ub_stage] = getBounds(stage)
-
-[nv_stage,~] = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.num_i, stage.nu, stage.np);
-
-lb_stage = -inf * ones(nv_stage,1);
-ub_stage = inf * ones(nv_stage,1);
-
-[X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = ocl.simultaneous.getStageIndizes(stage);
-
-% states
-for m=1:size(X_indizes,2)
-  lb_stage(X_indizes(:,m)) = stage.stateBounds.lower;
-  ub_stage(X_indizes(:,m)) = stage.stateBounds.upper;
-end
-
-% Merge the two vectors of bound values for lower bounds and upper bounds.
-% Bound values can only get narrower, e.g. higher for lower bounds.
-lb_stage(X_indizes(:,1)) = max(stage.stateBounds.lower,stage.stateBounds0.lower);
-ub_stage(X_indizes(:,1)) = min(stage.stateBounds.upper,stage.stateBounds0.upper);
-
-lb_stage(X_indizes(:,end)) = max(stage.stateBounds.lower,stage.stateBoundsF.lower);
-ub_stage(X_indizes(:,end)) = min(stage.stateBounds.upper,stage.stateBoundsF.upper);
-
-% integrator bounds
-for m=1:size(I_indizes,2)
-  lb_stage(I_indizes(:,m)) = stage.integrator.integratorBounds.lower;
-  ub_stage(I_indizes(:,m)) = stage.integrator.integratorBounds.upper;
-end
-
-% controls
-for m=1:size(U_indizes,2)
-  lb_stage(U_indizes(:,m)) = stage.controlBounds.lower;
-  ub_stage(U_indizes(:,m)) = stage.controlBounds.upper;
-end
-
-% parameters (only set the initial parameters)
-lb_stage(P_indizes(:,1)) = stage.parameterBounds.lower;
-ub_stage(P_indizes(:,1)) = stage.parameterBounds.upper;
-
-% timesteps
-if isempty(stage.T)
-  lb_stage(H_indizes) = 0.0;
-else
-  lb_stage(H_indizes) = stage.H_norm * stage.T;
-  ub_stage(H_indizes) = stage.H_norm * stage.T;
-end
\ No newline at end of file
diff --git a/+ocl/+simultaneous/getFirstState.m b/+ocl/+simultaneous/getFirstState.m
index 7833392b..baaf60ba 100644
--- a/+ocl/+simultaneous/getFirstState.m
+++ b/+ocl/+simultaneous/getFirstState.m
@@ -1,3 +1,10 @@
-function x = getFirstState(stage,stageVars)
-[X_indizes, ~, ~, ~, ~] = ocl.simultaneous.getStageIndizes(stage);
-x = stageVars(X_indizes(:,1));
\ No newline at end of file
+function x = getFirstState(stage, colloc, stage_vars)
+
+N = stage.N;
+nx = stage.nx;
+ni = colloc.num_i;
+nu = stage.nu;
+np = stage.np;
+
+[X_indizes, ~, ~, ~, ~] = ocl.simultaneous.indizes(N, nx, ni, nu, np);
+x = stage_vars(X_indizes(:,1));
\ No newline at end of file
diff --git a/+ocl/+simultaneous/getInitialGuess.m b/+ocl/+simultaneous/getInitialGuess.m
index bf8b6b2e..ef07ac55 100644
--- a/+ocl/+simultaneous/getInitialGuess.m
+++ b/+ocl/+simultaneous/getInitialGuess.m
@@ -1,42 +1,44 @@
-function ig_stage = getInitialGuess(stage)
+function ig_stage = getInitialGuess(H_norm, T, nx, ni, nu, np, ...
+                                    x0_lb, x0_ub, xF_lb, xF_ub, x_lb, x_ub, ...
+                                    z_lb, z_ub, u_lb_traj, u_ub_traj, p_lb, p_ub, ...
+                                    vi_struct)
 % creates an initial guess from the information that we have about
 % bounds in the stage
 
-[nv_stage,N] = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.num_i, stage.nu, stage.np);
+N = length(H_norm);
+nv_stage = ocl.simultaneous.nvars(N, nx, ni, nu, np);
 
-[X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = ocl.simultaneous.getStageIndizes(stage);
+[X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = ocl.simultaneous.indizes(N, nx, ni, nu, np);
 
 ig_stage = 0 * ones(nv_stage,1);
 
-igx0 = ocl.simultaneous.igFromBounds(stage.stateBounds0);
-igxF = ocl.simultaneous.igFromBounds(stage.stateBoundsF);
+igx0 = ocl.simultaneous.igFromBounds(x0_lb, x0_ub);
+igxF = ocl.simultaneous.igFromBounds(xF_lb, xF_ub);
 
 ig_stage(X_indizes(:,1)) = igx0;
 ig_stage(X_indizes(:,end)) = igxF;
 
-algVarsGuess = ocl.simultaneous.igFromBounds(stage.integrator.algvarBounds);
+algVarsGuess = ocl.simultaneous.igFromBounds(z_lb, z_ub);
 for m=1:N
   xGuessInterp = igx0 + (m-1)/N.*(igxF-igx0);
   % integrator variables
-  ig_stage(I_indizes(:,m)) = stage.integrator.getInitialGuess(xGuessInterp, algVarsGuess);
+  ig_stage(I_indizes(:,m)) = ocl.collocation.initialGuess(vi_struct, xGuessInterp, algVarsGuess);
   
   % states
   ig_stage(X_indizes(:,m)) = xGuessInterp;
 end
 
 % controls
-for m=1:size(U_indizes,2)
-  ig_stage(U_indizes(:,m)) = ocl.simultaneous.igFromBounds(stage.controlBounds);
-end
+ig_stage(U_indizes) = ocl.simultaneous.igFromBounds(u_lb_traj, u_ub_traj);
 
 % parameters
 for m=1:size(P_indizes,2)
-  ig_stage(P_indizes(:,m)) = ocl.simultaneous.igFromBounds(stage.parameterBounds);
+  ig_stage(P_indizes(:,m)) = ocl.simultaneous.igFromBounds(p_lb, p_ub);
 end
 
 % timesteps
-if isempty(stage.T)
-  ig_stage(H_indizes) = stage.H_norm;
+if isempty(T)
+  ig_stage(H_indizes) = H_norm;
 else
-  ig_stage(H_indizes) = stage.H_norm * stage.T;
+  ig_stage(H_indizes) = H_norm * T;
 end
\ No newline at end of file
diff --git a/+ocl/+simultaneous/getInitialGuessWithUserData.m b/+ocl/+simultaneous/getInitialGuessWithUserData.m
index 6bf8f5b5..91f96065 100644
--- a/+ocl/+simultaneous/getInitialGuessWithUserData.m
+++ b/+ocl/+simultaneous/getInitialGuessWithUserData.m
@@ -1,28 +1,35 @@
-function ig_stage = getInitialGuessWithUserData(stage, ig_data)
+function ig_stage = getInitialGuessWithUserData(ig_stage, stage, colloc)
 
-ig_values = ocl.simultaneous.getInitialGuess(stage);
-varsStruct = ocl.simultaneous.variables(stage);
-ig_stage = Variable.create(varsStruct, ig_values);
+H_norm = stage.H_norm;
+nt = colloc.num_t;
+
+x_guess = stage.x_guess.data;
+
+x_times = [0, cumsum(H_norm)]';
+
+colloc_times = zeros(nt, length(H_norm));
+time = 0;
+for k=1:length(H_norm)
+  h = H_norm(k);
+  colloc_times(:,k) = time + ocl.collocation.times(colloc.tau_root, h);
+  time = time + H_norm(k);
+end
+colloc_times = colloc_times(:);
 
 % incoorperate user input ig data for state trajectories
-names = fieldnames(ig_data.data);
+names = fieldnames(x_guess);
 for k=1:length(names)
   id = names{k};
   
-  xdata = ig_data.data.(id).x;
-  ydata = ig_data.data.(id).y;
+  xdata = x_guess.(id).x;
+  ydata = x_guess.(id).y;
   
   % state trajectories
-  xtarget = ocl.simultaneous.normalizedStateTimes(stage);
-  ytarget = interp1(xdata,ydata,xtarget,'linear','extrap');
-  
+  ytarget = interp1(xdata, ydata, x_times,'linear','extrap');
   ocl.types.variable.setFromNdMatrix(ig_stage.states.get(id), ytarget);
   
-  xtarget = ocl.simultaneous.normalizedIntegratorTimes(stage);
-  xtarget = xtarget(:);
-  
-  ytarget = interp1(xdata,ydata,xtarget,'linear','extrap');
-  
+  % integrator states
+  ytarget = interp1(xdata, ydata, colloc_times,'linear','extrap');
   ocl.types.variable.setFromNdMatrix(ig_stage.integrator.states.get(id), ytarget);
   
 end
\ No newline at end of file
diff --git a/+ocl/+simultaneous/getLastState.m b/+ocl/+simultaneous/getLastState.m
index 1dfe29e1..dc3c4038 100644
--- a/+ocl/+simultaneous/getLastState.m
+++ b/+ocl/+simultaneous/getLastState.m
@@ -1,3 +1,10 @@
-function x = getLastState(stage,stageVars)
-[X_indizes, ~, ~, ~, ~] = ocl.simultaneous.getStageIndizes(stage);
-x = stageVars(X_indizes(:,end));
\ No newline at end of file
+function x = getLastState(stage,colloc,stageVars)
+N = stage.N;
+nx = stage.nx;
+ni = colloc.num_i;
+nu = stage.nu;
+np = stage.np;
+
+[X_indizes, ~, ~, ~, ~] = ocl.simultaneous.indizes(N, nx, ni, nu, np);
+x = stageVars(X_indizes(:,end));
+
diff --git a/+ocl/+simultaneous/igFromBounds.m b/+ocl/+simultaneous/igFromBounds.m
index e4fa9cba..aa2d9e94 100644
--- a/+ocl/+simultaneous/igFromBounds.m
+++ b/+ocl/+simultaneous/igFromBounds.m
@@ -1,20 +1,17 @@
-function guess = igFromBounds(bounds)
+function guess = igFromBounds(lower, upper)
 % Averages the bounds to get an initial guess value.
 % Makes sure no nan values are produced, defaults to 0.
 
-lowVal  = bounds.lower;
-upVal   = bounds.upper;
-
-guess = (lowVal + upVal) / 2;
+guess = (lower + upper) / 2;
 
 % set to lowerBounds if upperBounds are inf
-indizes = isinf(upVal);
-guess(indizes) = lowVal(indizes);
+indizes = isinf(upper);
+guess(indizes) = lower(indizes);
 
 % set to upperBounds of lowerBoudns are inf
-indizes = isinf(lowVal);
-guess(indizes) = upVal(indizes);
+indizes = isinf(lower);
+guess(indizes) = upper(indizes);
 
 % set to zero if both lower and upper bounds are inf
-indizes = isinf(lowVal) & isinf(upVal);
+indizes = isinf(lower) & isinf(upper);
 guess(indizes) = 0;
\ No newline at end of file
diff --git a/+ocl/+simultaneous/getStageIndizes.m b/+ocl/+simultaneous/indizes.m
similarity index 86%
rename from +ocl/+simultaneous/getStageIndizes.m
rename to +ocl/+simultaneous/indizes.m
index eeb4a568..ed0e6c96 100644
--- a/+ocl/+simultaneous/getStageIndizes.m
+++ b/+ocl/+simultaneous/indizes.m
@@ -1,10 +1,4 @@
-function [X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = getStageIndizes(stage)
-
-N = length(stage.H_norm);
-nx = stage.nx;
-ni = stage.integrator.num_i;
-nu = stage.nu;
-np = stage.np;
+function [X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = indizes(N, nx, ni, nu, np)
 
 % number of variables in one control interval
 % + 1 for the timestep
diff --git a/+ocl/+simultaneous/normalizedIntegratorTimes.m b/+ocl/+simultaneous/normalizedIntegratorTimes.m
deleted file mode 100644
index da4fa4e3..00000000
--- a/+ocl/+simultaneous/normalizedIntegratorTimes.m
+++ /dev/null
@@ -1,11 +0,0 @@
-function r = normalizedIntegratorTimes(stage)
-H_norm = stage.H_norm;
-integrator = stage.integrator;
-
-r = zeros(length(H_norm), integrator.num_t);
-time = 0;
-for k=1:length(H_norm)
-  h = H_norm(k);
-  r(k,:) = time + h * ocl.collocation.collocationPoints(stage.integrator.order);
-  time = time + H_norm(k);
-end
diff --git a/+ocl/+simultaneous/normalizedStateTimes.m b/+ocl/+simultaneous/normalizedStateTimes.m
deleted file mode 100644
index b439dd6d..00000000
--- a/+ocl/+simultaneous/normalizedStateTimes.m
+++ /dev/null
@@ -1,2 +0,0 @@
-function r = normalizedStateTimes(stage)
-r = [0, cumsum(stage.H_norm)]';
\ No newline at end of file
diff --git a/+ocl/+simultaneous/nvars.m b/+ocl/+simultaneous/nvars.m
index 432a3827..6a61a87d 100644
--- a/+ocl/+simultaneous/nvars.m
+++ b/+ocl/+simultaneous/nvars.m
@@ -1,6 +1,4 @@
-function [nv_stage,N] = nvars(H_norm, nx, ni, nu, np)
-% number of control intervals
-N = length(H_norm);
+function [nv_stage] = nvars(N, nx, ni, nu, np)
 
 % N control interval which each have states, integrator vars,
 % controls, parameters, and timesteps.
diff --git a/+ocl/+simultaneous/times.m b/+ocl/+simultaneous/times.m
index 3717f908..b52d1294 100644
--- a/+ocl/+simultaneous/times.m
+++ b/+ocl/+simultaneous/times.m
@@ -1,5 +1,13 @@
-function stage_time_struct = times(stage)
-stage_time_struct = OclStructure();
-stage_time_struct.addRepeated({'states', 'integrator', 'controls'}, ...
-  {OclMatrix([1,1]), OclMatrix([stage.integrator.num_t,1]), OclMatrix([1,1])}, length(stage.H_norm));
-stage_time_struct.add('states', OclMatrix([1,1]));
+function times_r = times(H, colloc)
+
+tau_root = colloc.tau_root;
+
+% times output
+times_r = zeros(length(H), length(tau_root)-1);
+T0 = [0, cumsum(H(:,1:end-1))];
+for k=1:size(H,2)
+  times_r(k,:) = T0(k) + ocl.collocation.times(tau_root, H(k));
+end
+times_r = times_r';
+times_r = [T0; times_r; T0];
+times_r = [times_r(:); T0(end)+H(end)];
\ No newline at end of file
diff --git a/+ocl/+simultaneous/variables.m b/+ocl/+simultaneous/variables.m
deleted file mode 100644
index 35a6994f..00000000
--- a/+ocl/+simultaneous/variables.m
+++ /dev/null
@@ -1,10 +0,0 @@
-function stage_vars_structure = variables(stage)
-stage_vars_structure = OclStructure();
-stage_vars_structure.addRepeated({'states','integrator','controls','parameters','h'}, ...
-  {stage.states, ...
-  stage.integrator.vars, ...
-  stage.controls, ...
-  stage.parameters, ...
-  OclMatrix([1,1])}, length(stage.H_norm));
-stage_vars_structure.add('states', stage.states);
-stage_vars_structure.add('parameters', stage.parameters);
diff --git a/+ocl/+simultaneous/variablesPack.m b/+ocl/+simultaneous/variablesPack.m
new file mode 100644
index 00000000..57136854
--- /dev/null
+++ b/+ocl/+simultaneous/variablesPack.m
@@ -0,0 +1,17 @@
+function V = variablesPack(X, I, U, P, H)
+
+N = length(H);
+nx = size(X,1);
+ni = size(I,1);
+np = size(P,1);
+
+nv = nvars(N, nx, ni, nu, np);
+
+[X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = ocl.simultaneous.indizes(N, nx, ni, nu, np);
+
+V = zeros(nv, 1);
+V(X_indizes) = X;
+V(I_indizes) = I;
+V(U_indizes) = U;
+V(P_indizes) = P;
+V(H_indizes) = H;
diff --git a/+ocl/+simultaneous/variablesStruct.m b/+ocl/+simultaneous/variablesStruct.m
new file mode 100644
index 00000000..33fb84a3
--- /dev/null
+++ b/+ocl/+simultaneous/variablesStruct.m
@@ -0,0 +1,10 @@
+function stage_vars_structure = variablesStruct(N, states, integrator_vars, controls, parameters)
+stage_vars_structure = ocl.types.Structure();
+stage_vars_structure.addRepeated({'states','integrator','controls','parameters','h'}, ...
+  {states, ...
+   integrator_vars, ...
+   controls, ...
+   parameters, ...
+  ocl.types.Matrix([1,1])}, N);
+stage_vars_structure.add('states', states);
+stage_vars_structure.add('parameters', parameters);
diff --git a/+ocl/+simultaneous/variablesUnpack.m b/+ocl/+simultaneous/variablesUnpack.m
new file mode 100644
index 00000000..d21f85c5
--- /dev/null
+++ b/+ocl/+simultaneous/variablesUnpack.m
@@ -0,0 +1,9 @@
+function [X,I,U,P,H] = variablesUnpack(V, N, nx, ni, nu, np)
+
+[X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = ocl.simultaneous.indizes(N, nx, ni, nu, np);
+
+X = reshape(V(X_indizes), nx, N+1);
+I = reshape(V(I_indizes), ni, N);
+U = reshape(V(U_indizes), nu, N);
+P = reshape(V(P_indizes), np, N+1);
+H = reshape(V(H_indizes), 1 , N);
\ No newline at end of file
diff --git a/+ocl/+test/testOclFunction.m b/+ocl/+test/testOclFunction.m
deleted file mode 100644
index e078cbf1..00000000
--- a/+ocl/+test/testOclFunction.m
+++ /dev/null
@@ -1,46 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-
-function testOclFunction
-  
-  fh = @(~,x,y,z)OclFunction_F1(x,y,z);
-  fun = OclFunction([],fh,{[3,1],[6,1],[3,3]},2);
-  
-  x = [4,5,6]';
-  y = 10;
-  z = eye(3);
-  [r1,r2] = fun.evaluate(x,y,z);
-  assertEqual(r1,z*x)
-  assertEqual(r2,ones(6,1)*y*2)
-  
-  
-  cfun = CasadiFunction(fun);
-  [r1,r2] = cfun.evaluate(x,y,z);
-  assertEqual(r1,z*x)
-  assertEqual(r2,ones(6,1)*y*2)
-  
-  
-end
-
-function [r1,r2] = OclFunction_F1(x,y,z)
-  
-  xs = OclMatrix([3,1]);
-  ys = OclStructure();
-  ys.add('x1',xs);
-  ys.add('x2',xs);
-  zs = OclMatrix([3,3]);
-  
-  xv = Variable.create(xs,x);
-  yv = Variable.create(ys,y);
-  zv = Variable.create(zs,z);
-  
-  r1 = zv*xv;
-  r2 = yv * 2;
-  
-  r1 = r1.value;
-  r2 = r2.value;
-end
-
-  
\ No newline at end of file
diff --git a/+ocl/+test/testOclOCP.m b/+ocl/+test/testOclOCP.m
deleted file mode 100644
index f1058d46..00000000
--- a/+ocl/+test/testOclOCP.m
+++ /dev/null
@@ -1,170 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-
-function testOclOCP
-
-% ocp empty test
-ocp = OclOCP();
-s = OclSystem(@emptyVars,@emptyEq);
-s.setup();
-opt = OclOptions;
-opt.controls_regularization = false;
-h = OclOcpHandler(1,s,ocp,opt);
-h.setup();
-assertEqual(h.pathCostsFun.evaluate([],[],[],[]),0);
-assertEqual(h.arrivalCostsFun.evaluate([],[]),0);
-
-[val,lb,ub] = h.pathConstraintsFun.evaluate([],[]);
-assertEqual(val,[]);
-assertEqual(lb,[]);
-assertEqual(ub,[]);
-[val,lb,ub] = h.boundaryConditionsFun.evaluate([],[],[]);
-assertEqual(val,[]);
-assertEqual(lb,[]);
-assertEqual(ub,[]);
-
-% ocp valid test
-ocp = OclOCP(@validPathCosts, @validArrivalCosts, @validPathConstraints, ...
-             @validBoundaryConditions, @validDiscreteCosts);
-s = OclSystem(@validVars,@validEq);
-N = 2;
-nv = (N+1)*s.nx+N*s.nu;
-h = OclOcpHandler(1,s,ocp,opt);
-h.setup();
-h.setNlpVarsStruct(OclMatrix([nv,1]));
-
-c = h.pathCostsFun.evaluate(ones(s.nx,1),ones(s.nz,1),ones(s.nu,1),ones(s.np,1));
-assertEqual(c,26+1e-3*12);
-
-c = h.arrivalCostsFun.evaluate(ones(s.nx,1),ones(s.np,1));
-assertEqual(c, -1);
-
-% path constraints in the form of : -inf <= val <= 0 or 0 <= val <= 0
-[val,lb,ub] = h.pathConstraintsFun.evaluate(ones(s.nx,1),ones(s.np,1));
-% ub all zero
-assertEqual(ub,zeros(36,1));
-% lb either zero for eq or -inf for ineq
-assertEqual(lb,[-inf,-inf,0,0,-inf,-inf,-inf*ones(1,5),0,-inf*ones(1,12),-inf*ones(1,12)].');
-% val = low - high
-assertEqual(val,[0,0,0,0,0,-1,2,2,2,2,2,0,-3*ones(1,12),zeros(1,12)].');
-
-% bc
-[val,lb,ub] = h.boundaryConditionsFun.evaluate(2*ones(s.nx,1),3*ones(s.nx,1),ones(s.np,1));
-assertEqual(ub,zeros(3,1));
-assertEqual(lb,[0,-inf,-inf].');
-assertEqual(val,[-1,1,-4].');
-
-c = h.discreteCostsFun.evaluate(ones(nv,1));
-assertEqual(c, nv);
-  
-end
-
-function emptyVars(self)    
-end
-
-function emptyEq(self,x,z,u,p)     
-end
-
-function validVars(self)
-  self.addState('a');
-  self.addState('b',1);
-  self.addState('c',7);
-  self.addState('d',[1,1]);
-  self.addState('e',[1,4]);
-  self.addState('f',[5,1]);
-  self.addState('g',[3,4]);
-
-  self.addState('ttt')
-
-  self.addControl('a'); % same as state!? (conflict bounds in Simultaneous)
-  self.addControl('h',1);
-  self.addControl('i',7);
-  self.addControl('j',[1,1]);
-  self.addControl('k',[1,4]);
-  self.addControl('l',[5,1]);
-  self.addControl('m',[3,4]);
-
-  self.addAlgVar('n');
-  self.addAlgVar('o',1);
-  self.addAlgVar('p',7);
-  self.addAlgVar('q',[1,1]);
-  self.addAlgVar('r',[1,4]);
-  self.addAlgVar('s',[5,1]);
-  self.addAlgVar('t',[3,4]);
-
-  self.addParameter('u');
-  self.addParameter('v',1);
-  self.addParameter('w',7);
-  self.addParameter('x',[1,1]);
-  self.addParameter('y',[1,4]);
-  self.addParameter('z',[5,1]);
-  self.addParameter('aa',[3,4]);
-end
-function validEq(self,x,z,u,p)
-
-  self.setODE('g',p.aa+z.t);
-  self.setODE('b',z.n);
-  self.setODE('a',p.u);
-  self.setODE('d',x.a+x.b*z.o+p.u*p.x);
-  self.setODE('c',z.p);
-  self.setODE('f',z.s);
-  self.setODE('e',u.k);
-
-  self.setODE('ttt',1);
-
-  % 31x1
-  self.setAlgEquation(reshape(p.y,4,1));
-  self.setAlgEquation(reshape(z.t,12,1));
-  self.setAlgEquation(reshape(x.g,12,1)+[u.a,u.h,u.j,4,5,6,z.n,z.q,p.u,10,11,12].');
-  self.setAlgEquation(p.y(:,1:3,:));
-end
-
-function validPathCosts(self,x,z,u,p)
-  self.add(x.a); % 1
-  self.add(x.c.'*x.c); % 7
-  self.add(1e-3*sum(sum(u.m))+sum(sum(p.z))+sum(sum(z.t))+x.ttt+1); % 1e-3*12+26
-  self.add(0); % 0 ([]) or () ? invalid!
-  self.add(-1); % -1
-end
-
-function validArrivalCosts(self,xf,p)
-  self.add(xf.d);
-  self.add(0);
-  self.add(-1);
-  self.add(-1*p.v*1);
-end
-
-function validPathConstraints(self,x,p)
-
-  % scalar with constant
-  self.add(x.a,'<=',1);
-  self.add(x.a,'>=',1);
-  self.add(x.a,'==',1);
-  self.add(1,'==',x.a);
-  self.add(1,'>=',x.a);
-  self.add(1,'<=',x.ttt+p.aa(1,1,1));
-
-  % vector with vector
-  self.add(x.f,'>=',2+ones(5,1));
-
-  % scalar with scalar
-  self.add(x.d,'==',x.b);
-
-  % matrix 3x4 with scalar
-  self.add(x.g,'<=',4);
-
-  % matrix with matrix 3x4
-  self.add(x.g,'<=',p.aa);
-end
-
-function validBoundaryConditions(self,x0,xf,p)
-  self.add(x0.a,'==',xf.a);
-  self.add(x0.a,'>=',xf.a*p.x);
-  self.add(x0.b,'<=',xf.a+xf.a);
-end
-
-function validDiscreteCosts(self,vars)
-  self.add(sum(vars));
-end
\ No newline at end of file
diff --git a/+ocl/+test/testOclSystem.m b/+ocl/+test/testOclSystem.m
deleted file mode 100644
index 52f6fe7b..00000000
--- a/+ocl/+test/testOclSystem.m
+++ /dev/null
@@ -1,143 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-
-function testOclSystem
-
-s = OclSystem(@emptyVars,@emptyEq);
-assertEqual(s.nx,0);
-assertEqual(s.nu,0);
-assertEqual(s.np,0);
-assertEqual(s.nz,0);
-assertEqual(s.daefun([],[],[],[]),[]);
-
-s = OclSystem(@validVars, @validEq);
-assertEqual(s.nx,32);
-assertEqual(s.nu,31);
-assertEqual(s.np,31);
-assertEqual(s.nz,31);
-[dx,alg] = s.daefun(ones(s.nx,1),ones(s.nz,1),ones(s.nu,1),ones(s.np,1));
-assertEqual(dx,[1,1,1,1,1,1,1,1,1,3,1,1,1,1,1,1,1,1,1,2,2,2,2,2,2,2,2,2,2,2,2,1].')
-assertEqual(alg,[1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,2,2,2,5,6,7,2,2,2,11,12,13,1,1,1].')
-
-s = OclSystem(@missOdeVars,@missOdeEq);
-fh = @(x,z,u,p) s.daefun(x,z,u,p);
-assertException('ode', fh, zeros(s.nx,1), zeros(s.nz,1), zeros(s.nu,1), zeros(s.np,1));
-
-s = OclSystem(@doubleOdeVars,@doubleOdeEq);
-fh = @(x,z,u,p) s.daefun(x,z,u,p);
-assertException('ode', fh, zeros(s.nx,1), zeros(s.nz,1), zeros(s.nu,1), zeros(s.np,1));
-
-s = OclSystem(@wrongOdeVars,@wrongOdeEq);
-fh = @(x,z,u,p) s.daefun(x,z,u,p);
-assertException('exist', fh, zeros(s.nx,1), zeros(s.nz,1), zeros(s.nu,1), zeros(s.np,1));
-
-s = OclSystem(@missDaeVars,@missDaeEq);
-fh = @(x,z,u,p) s.daefun(x,z,u,p);
-assertException('algebraic equations', fh, zeros(s.nx,1), zeros(s.nz,1), zeros(s.nu,1), zeros(s.np,1));
-
-s = OclSystem(@manyDaeVars,@manyDaeEq);
-fh = @(x,z,u,p) s.daefun(x,z,u,p);
-assertException('algebraic equations', fh, zeros(s.nx,1), zeros(s.nz,1), zeros(s.nu,1), zeros(s.np,1));
-
-end
-
-function doubleOdeVars(self)    
-  self.addState('x');
-end
-function doubleOdeEq(self,x,z,u,p)   
-  self.setODE('x',x);
-  self.setODE('x',x+x);
-end
-
-function emptyVars(self)    
-end
-function emptyEq(self,x,z,u,p)     
-end
-
-function manyDaeVars(self)    
-  self.addState('x');
-  self.addAlgVar('z');
-end
-function manyDaeEq(self,x,z,u,p)   
-  self.setODE('x',x);
-  self.setAlgEquation(z);
-  self.setAlgEquation(z);
-end
-
-function missDaeVars(self)    
-  self.addState('x');
-  self.addAlgVar('z');
-end
-function missDaeEq(self,x,z,u,p)   
-  self.setODE('x',x);
-end
-
-function missOdeVars(self)    
-  self.addState('x')
-end
-function missOdeEq(self,x,z,u,p)     
-end
-
-function validVars(self)
-  self.addState('a');
-  self.addState('b',1);
-  self.addState('c',7);
-  self.addState('d',[1,1]);
-  self.addState('e',[1,4]);
-  self.addState('f',[5,1]);
-  self.addState('g',[3,4]);
-
-  self.addState('ttt')
-
-  self.addControl('a'); % same as state!? (conflict bounds in Simultaneous)
-  self.addControl('h',1);
-  self.addControl('i',7);
-  self.addControl('j',[1,1]);
-  self.addControl('k',[1,4]);
-  self.addControl('l',[5,1]);
-  self.addControl('m',[3,4]);
-
-  self.addAlgVar('n');
-  self.addAlgVar('o',1);
-  self.addAlgVar('p',7);
-  self.addAlgVar('q',[1,1]);
-  self.addAlgVar('r',[1,4]);
-  self.addAlgVar('s',[5,1]);
-  self.addAlgVar('t',[3,4]);
-
-  self.addParameter('u');
-  self.addParameter('v',1);
-  self.addParameter('w',7);
-  self.addParameter('x',[1,1]);
-  self.addParameter('y',[1,4]);
-  self.addParameter('z',[5,1]);
-  self.addParameter('aa',[3,4]);
-end
-function validEq(self,x,z,u,p)
-
-  self.setODE('g',p.aa+z.t);
-  self.setODE('b',z.n);
-  self.setODE('a',p.u);
-  self.setODE('d',x.a+x.b*z.o+p.u*p.x);
-  self.setODE('c',z.p);
-  self.setODE('f',z.s);
-  self.setODE('e',u.k);
-
-  self.setODE('ttt',1);
-
-  % 31x1
-  self.setAlgEquation(reshape(p.y,4,1));
-  self.setAlgEquation(reshape(z.t,12,1));
-  self.setAlgEquation(reshape(x.g,12,1)+[u.a,u.h,u.j,4,5,6,z.n,z.q,p.u,10,11,12].');
-  self.setAlgEquation(p.y(:,1:3,:));
-end
-
-function wrongOdeVars(self)    
-  self.addState('x');
-end
-function wrongOdeEq(self,x,z,u,p)   
-  self.setODE('x',x);
-  self.setODE('y',x+x);
-end
\ No newline at end of file
diff --git a/+ocl/+test/+run/all.m b/+ocl/+tests/+run/all.m
similarity index 88%
rename from +ocl/+test/+run/all.m
rename to +ocl/+tests/+run/all.m
index ce228447..90eb7acf 100644
--- a/+ocl/+test/+run/all.m
+++ b/+ocl/+tests/+run/all.m
@@ -3,4 +3,4 @@
 % ensure the above copyright notice is visible in any derived work.
 %
 function [nFails] = all
- nFails = ocl.test.run.core(1);
\ No newline at end of file
+ nFails = ocl.tests.run.core(1);
\ No newline at end of file
diff --git a/+ocl/+test/+run/core.m b/+ocl/+tests/+run/core.m
similarity index 88%
rename from +ocl/+test/+run/core.m
rename to +ocl/+tests/+run/core.m
index c9fe14b1..d32dc44d 100644
--- a/+ocl/+test/+run/core.m
+++ b/+ocl/+tests/+run/core.m
@@ -26,25 +26,25 @@
   cd(oclDir);
   [status,version] = system('git rev-parse HEAD');
   if status~=0
-    oclInfo('Could not get git hash, log filename will not include the hash. This is not a problem.')
+    ocl.utils.info('Could not get git hash, log filename will not include the hash. This is not a problem.')
     version='nogithash';  
   end
   
   if isempty(testDir)
-    error('Test directory not set. Run StartupOCL again.')
+    ocl.utils.error('Test directory not set. Run StartupOCL again.')
   end
   
   ocl.utils.setTestRun(true)
   
-  tests{1}.name = 'Variable';         tests{end}.file = 'ocl.test.testVariable';
-  tests{end+1}.name = 'TreeVariable'; tests{end}.file = 'ocl.test.testTreeVariable';
-  tests{end+1}.name = 'VarStructure'; tests{end}.file = 'ocl.test.testVarStructure';
-  tests{end+1}.name = 'OclSystem';    tests{end}.file = 'ocl.test.testOclSystem';
-  tests{end+1}.name = 'OclStage';       tests{end}.file = 'ocl.test.testOclStage';
-  tests{end+1}.name = 'Integrator';   tests{end}.file = 'ocl.test.testOclIntegrator';
+  tests{1}.name = 'Variable';         tests{end}.file = 'ocl.tests.variable';
+  tests{end+1}.name = 'TreeVariable'; tests{end}.file = 'ocl.tests.treeVariable';
+  tests{end+1}.name = 'VarStructure'; tests{end}.file = 'ocl.tests.structure';
+  tests{end+1}.name = 'ocl.Model';    tests{end}.file = 'ocl.tests.model';
+  tests{end+1}.name = 'ocl.Stage';       tests{end}.file = 'ocl.tests.stage';
+  tests{end+1}.name = 'ocl.Integrator';   tests{end}.file = 'ocl.tests.integrator';
   
   if testExamples
-    tests{end+1}.name = 'Examples';      tests{end}.file = 'ocl.test.examples';
+    tests{end+1}.name = 'Examples';      tests{end}.file = 'ocl.tests.examples';
   end
   
   NTests = length(tests);
@@ -141,7 +141,7 @@ function printResults(testResult)
       outputString = sprintf('%s Tests failed\n',testResult.name);
       fprintf(resultsFile,outputString);fprintf(outputString);
       
-      if isOctave()
+      if ocl.utils.isOctave()
         disp(getReportOctave(testResult.exception))  %% OCTAVE
       else
         disp(getReport(testResult.exception))  %% MATLAB
diff --git a/+ocl/+test/+run/log.m b/+ocl/+tests/+run/log.m
similarity index 84%
rename from +ocl/+test/+run/log.m
rename to +ocl/+tests/+run/log.m
index 813114c5..82ee594b 100644
--- a/+ocl/+test/+run/log.m
+++ b/+ocl/+tests/+run/log.m
@@ -3,4 +3,4 @@
 % ensure the above copyright notice is visible in any derived work.
 %
 function [nFails] = log(suffix)
- nFails = ocl.test.run.core(1,1,suffix);
\ No newline at end of file
+ nFails = ocl.tests.run.core(1,1,suffix);
\ No newline at end of file
diff --git a/+ocl/+test/argumentParser.m b/+ocl/+tests/argumentParser.m
similarity index 100%
rename from +ocl/+test/argumentParser.m
rename to +ocl/+tests/argumentParser.m
diff --git a/+ocl/+test/examples.m b/+ocl/+tests/examples.m
similarity index 64%
rename from +ocl/+test/examples.m
rename to +ocl/+tests/examples.m
index 87108a17..1ed6bfb9 100644
--- a/+ocl/+test/examples.m
+++ b/+ocl/+tests/examples.m
@@ -7,7 +7,7 @@
 
 % test basic example (VanDerPol)
 [sol,~,solver] = ocl.examples.vanderpol;
-assert(all(abs(sol.controls.F.value - ...
+ocl.utils.assertAlmostEqual(sol.controls.F.value, ...
   [ 0.2434
   0.9611
   0.9074
@@ -37,12 +37,12 @@
   -0.0037
   -0.0017
   -0.0005
-  -0.0000]) < 1e-3 ), 'Control vector of solution is wrong in Example.');
+  -0.0000], 'Control vector of solution is wrong in Example.');
 o1 = solver.timeMeasures;
 
 % test ball and beam example problem
 [sol,~,solver] = ocl.examples.ballandbeam;
-assertAlmostEqual(sol.controls.tau(1:5:end).value, ...
+ocl.utils.assertAlmostEqual(sol.controls.tau(1:5:end).value, ...
                   [-20;-9.67180946278377;-6.83499933773107;-3.3277726553036;-0.594240414712151;1.26802912244169;0.938453275453379;-0.199369534081799;-0.838286223053903;-0.292251460119773], ...
                   'Ball and beam problem Test failed.', 1e-3);
 
@@ -50,7 +50,7 @@
 
 % test race car problem
 [sol,~,solver] = ocl.examples.racecar;
-assertAlmostEqual(sol.controls.dFx(1,:,1:5:end).value,...
+ocl.utils.assertAlmostEqual(sol.controls.dFx(1,:,1:5:end).value,...
       [-0.0101362795430379;-0.999999558480492;0.319856962019424;-0.764994370307151;0.7697294885374;-0.126456278919074;0.580563346802815;-0.661025508901183;0.999998743528033;-0.9999996584554],...
       'Solve RaceCar Test failed.',1e-2);
 
@@ -58,24 +58,24 @@
  
 % test pendulum simulation
 simTic = tic;
-statesVec = ocl.examples.pendulum_sim;
-assertAlmostEqual(statesVec.p(1,:,1:10:41).value, ...
-                  [-0.545396928860894;-0.998705998524533;-0.35261807316407;-0.944329126056321;0], ...
+p_vec = ocl.examples.pendulum_sim;
+ocl.utils.assertAlmostEqual(p_vec(1,1:10:41), ...
+                  [-0.54539692886092 -0.998705998524509 -0.35261785193535 -0.944326275381522 0.0204417663509827], ...
                   'PendulumSim Test failed.');
 o4 = struct;
 o4.simulationTest = toc(simTic);
 
 % test cart pole
 [sol,~,solver] = ocl.examples.cartpole;
-res = sol.states.theta(:,:,1:10:end).value;
+res = sol.states.theta(:,:,1:30:end).value;
 truth = [3.14159265358979;3.86044803075832;2.52342234356076;0.885691999280203;0];
-assertAlmostEqual(res, truth, 'Cart pole test failed.');
+ocl.utils.assertAlmostEqual(res, truth, 'Cart pole test failed.');
 o5 = solver.timeMeasures;
 
 % test bouncing ball
 [sol,~,solver] = ocl.examples.bouncingball;
 stage_1 = sol{1}.states.s.value;
-assertAlmostEqual(stage_1, [1;0.888888888888889;0.555555555555555;0], 'Cart pole test failed.');
+ocl.utils.assertAlmostEqual(stage_1, [1;0.987654320987654;0.888888888888889;0.802469135802469;0.555555555555555;0.395061728395062;0], 'Bouncing ball test failed.');
 stage_2 = sol{2}.states.s.value;
-assertAlmostEqual(stage_2, [0;0.452518978545424;0.870025304727232;1.16127214163633;1.23501265236362;1], 'Cart pole test failed.');
+ocl.utils.assertAlmostEqual(stage_2, [0;0.149660685505038;0.452518978545424;0.6006473173636;0.870025304727232;0.986205684555515;1.16127214163633;1.21508895017169;1.23501265236362;1.19605027730302;1], 'Bouncing ball test failed.');
 o6 = solver.timeMeasures;
\ No newline at end of file
diff --git a/+ocl/+test/testOclIntegrator.m b/+ocl/+tests/integrator.m
similarity index 66%
rename from +ocl/+test/testOclIntegrator.m
rename to +ocl/+tests/integrator.m
index adbfbc1b..5b24ae9c 100644
--- a/+ocl/+test/testOclIntegrator.m
+++ b/+ocl/+tests/integrator.m
@@ -3,24 +3,26 @@
 % ensure the above copyright notice is visible in any derived work.
 %
 
-function testOclIntegrator
+function integrator
 
 % linear system defined below
-s = OclSystem(@linearVars,@linearEq);
+[x_struct, z_struct, u_struct, p_struct, ...
+          ~, ~, ~, ~, ...
+          x_order] = ocl.model.vars(@linearVars);
 
 d = 2;
-integ = OclCollocation(s.states, s.algvars, s.controls, s.parameters, @s.daefun, @(varargin)0, d, ...
-                       s.stateBounds, s.algvarBounds);
+collocation = ocl.Collocation(x_struct, z_struct, u_struct, p_struct, x_order, ...
+                              @linearEq, @(varargin)0, d);
 
 N = 60;
 T = 1;
 h = T/N;
 
-xsym = casadi.SX.sym('x',s.nx);
-xisym = casadi.SX.sym('xi',s.nx*d);
-usym = casadi.SX.sym('u',s.nu);
+xsym = casadi.SX.sym('x', length(x_struct));
+xisym = casadi.SX.sym('xi', length(x_struct)*d);
+usym = casadi.SX.sym('u', length(u_struct));
 
-[~, ~, equations, ~] = integ.integratorfun(xsym,xisym,usym,h,[]);
+[~, ~, equations] = ocl.collocation.equations(collocation, xsym, xisym, usym, h, []);
 
 nlp    = struct('x', vertcat(xsym,xisym,usym), 'f', 0, 'g', equations);
 
@@ -44,7 +46,7 @@
   x = v(2+d*2-1:2+d*2);
 end
 
-assertAlmostEqual(x,[1;1],'Integrator test (constant velocity) failed');
+ocl.utils.assertAlmostEqual(x,[1;1],'Integrator test (constant velocity) failed');
 
 % constant start velocity, constant acceleration
 p0 = 2;
@@ -62,7 +64,7 @@
 pEnd = p0 + v0*T + 0.5*u*T^2;
 vEnd = v0 + u*T;
 
-assertAlmostEqual(x,[pEnd;vEnd],'Integrator test (constant acceleration) failed',0.1);
+ocl.utils.assertAlmostEqual(x,[pEnd;vEnd],'Integrator test (constant acceleration) failed',0.1);
 
 end
 
diff --git a/+ocl/+tests/model.m b/+ocl/+tests/model.m
new file mode 100644
index 00000000..cbc3feb8
--- /dev/null
+++ b/+ocl/+tests/model.m
@@ -0,0 +1,260 @@
+% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
+% Redistribution is permitted under the 3-Clause BSD License terms. Please
+% ensure the above copyright notice is visible in any derived work.
+%
+
+function model
+
+% empty dae test
+[x_struct, z_struct, u_struct, p_struct, ...
+          ~, ~, ~, ~, ...
+          x_order] = ocl.model.vars(@emptyVars);
+        
+daefun = @(x,z,u,p) ocl.model.dae( ...
+        @emptyEq, ...
+        x_struct, ...
+        z_struct, ...
+        u_struct, ...
+        p_struct, ...
+        x_order, x, z, u, p);
+      
+nx = length(x_struct);
+nz = length(z_struct);
+nu = length(u_struct);
+np = length(p_struct);
+
+ocl.utils.assertEqual(nx,0);
+ocl.utils.assertEqual(nz,0);
+ocl.utils.assertEqual(nu,0);
+ocl.utils.assertEqual(np,0);
+
+ocl.utils.assertEqual(daefun([],[],[],[]),[]);
+
+% valid dae test
+[x_struct, z_struct, u_struct, p_struct, ...
+          ~, ~, ~, ~, ...
+          x_order] = ocl.model.vars(@validVars);
+        
+daefun = @(x,z,u,p) ocl.model.dae( ...
+        @validEq, ...
+        x_struct, ...
+        z_struct, ...
+        u_struct, ...
+        p_struct, ...
+        x_order, x, z, u, p);
+      
+nx = length(x_struct);
+nz = length(z_struct);
+nu = length(u_struct);
+np = length(p_struct);
+
+ocl.utils.assertEqual(nx,32);
+ocl.utils.assertEqual(nu,31);
+ocl.utils.assertEqual(np,31);
+ocl.utils.assertEqual(nz,31);
+[dx,alg] = daefun(ones(nx,1),ones(nz,1),ones(nu,1),ones(np,1));
+ocl.utils.assertEqual(dx,[1,1,1,1,1,1,1,1,1,3,1,1,1,1,1,1,1,1,1,2,2,2,2,2,2,2,2,2,2,2,2,1].')
+ocl.utils.assertEqual(alg,[1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,2,2,2,5,6,7,2,2,2,11,12,13,1,1,1].')
+
+
+% miss ode test
+[x_struct, z_struct, u_struct, p_struct, ...
+          ~, ~, ~, ~, ...
+          x_order] = ocl.model.vars(@missOdeVars);
+        
+daefun = @(x,z,u,p) ocl.model.dae( ...
+        @missOdeEq, ...
+        x_struct, ...
+        z_struct, ...
+        u_struct, ...
+        p_struct, ...
+        x_order, x, z, u, p);
+      
+nx = length(x_struct);
+nz = length(z_struct);
+nu = length(u_struct);
+np = length(p_struct);
+
+ocl.utils.assertException('ode', daefun, zeros(nx,1), zeros(nz,1), zeros(nu,1), zeros(np,1));
+
+
+% double ode test
+[x_struct, z_struct, u_struct, p_struct, ...
+          ~, ~, ~, ~, ...
+          x_order] = ocl.model.vars(@doubleOdeVars);
+        
+daefun = @(x,z,u,p) ocl.model.dae( ...
+        @doubleOdeEq, ...
+        x_struct, ...
+        z_struct, ...
+        u_struct, ...
+        p_struct, ...
+        x_order, x, z, u, p);
+      
+nx = length(x_struct);
+nz = length(z_struct);
+nu = length(u_struct);
+np = length(p_struct);
+
+ocl.utils.assertException('ode', daefun, zeros(nx,1), zeros(nz,1), zeros(nu,1), zeros(np,1));
+
+% wrong ode test
+[x_struct, z_struct, u_struct, p_struct, ...
+          ~, ~, ~, ~, ...
+          x_order] = ocl.model.vars(@wrongOdeVars);
+        
+daefun = @(x,z,u,p) ocl.model.dae( ...
+        @wrongOdeEq, ...
+        x_struct, ...
+        z_struct, ...
+        u_struct, ...
+        p_struct, ...
+        x_order, x, z, u, p);
+      
+nx = length(x_struct);
+nz = length(z_struct);
+nu = length(u_struct);
+np = length(p_struct);
+
+ocl.utils.assertException('exist', daefun, zeros(nx,1), zeros(nz,1), zeros(nu,1), zeros(np,1));
+
+% missing dae test
+[x_struct, z_struct, u_struct, p_struct, ...
+          ~, ~, ~, ~, ...
+          x_order] = ocl.model.vars(@missDaeVars);
+        
+daefun = @(x,z,u,p) ocl.model.dae( ...
+        @missDaeEq, ...
+        x_struct, ...
+        z_struct, ...
+        u_struct, ...
+        p_struct, ...
+        x_order, x, z, u, p);
+      
+nx = length(x_struct);
+nz = length(z_struct);
+nu = length(u_struct);
+np = length(p_struct);
+
+ocl.utils.assertException('algebraic equations', daefun, zeros(nx,1), zeros(nz,1), zeros(nu,1), zeros(np,1));
+
+% too many dae test
+[x_struct, z_struct, u_struct, p_struct, ...
+          ~, ~, ~, ~, ...
+          x_order] = ocl.model.vars(@missDaeVars);
+        
+daefun = @(x,z,u,p) ocl.model.dae( ...
+        @missDaeEq, ...
+        x_struct, ...
+        z_struct, ...
+        u_struct, ...
+        p_struct, ...
+        x_order, x, z, u, p);
+      
+nx = length(x_struct);
+nz = length(z_struct);
+nu = length(u_struct);
+np = length(p_struct);
+
+ocl.utils.assertException('algebraic equations', daefun, zeros(nx,1), zeros(nz,1), zeros(nu,1), zeros(np,1));
+
+end
+
+function doubleOdeVars(self)    
+  self.addState('x');
+end
+function doubleOdeEq(self,x,z,u,p)   
+  self.setODE('x',x);
+  self.setODE('x',x+x);
+end
+
+function emptyVars(self)    
+end
+function emptyEq(self,x,z,u,p)     
+end
+
+function manyDaeVars(self)    
+  self.addState('x');
+  self.addAlgVar('z');
+end
+function manyDaeEq(self,x,z,u,p)   
+  self.setODE('x',x);
+  self.setAlgEquation(z);
+  self.setAlgEquation(z);
+end
+
+function missDaeVars(self)    
+  self.addState('x');
+  self.addAlgVar('z');
+end
+function missDaeEq(self,x,z,u,p)   
+  self.setODE('x',x);
+end
+
+function missOdeVars(self)    
+  self.addState('x')
+end
+function missOdeEq(self,x,z,u,p)     
+end
+
+function validVars(self)
+  self.addState('a');
+  self.addState('b',1);
+  self.addState('c',7);
+  self.addState('d',[1,1]);
+  self.addState('e',[1,4]);
+  self.addState('f',[5,1]);
+  self.addState('g',[3,4]);
+
+  self.addState('ttt')
+
+  self.addControl('a'); % same as state!? (conflict bounds in Simultaneous)
+  self.addControl('h',1);
+  self.addControl('i',7);
+  self.addControl('j',[1,1]);
+  self.addControl('k',[1,4]);
+  self.addControl('l',[5,1]);
+  self.addControl('m',[3,4]);
+
+  self.addAlgVar('n');
+  self.addAlgVar('o',1);
+  self.addAlgVar('p',7);
+  self.addAlgVar('q',[1,1]);
+  self.addAlgVar('r',[1,4]);
+  self.addAlgVar('s',[5,1]);
+  self.addAlgVar('t',[3,4]);
+
+  self.addParameter('u');
+  self.addParameter('v',1);
+  self.addParameter('w',7);
+  self.addParameter('x',[1,1]);
+  self.addParameter('y',[1,4]);
+  self.addParameter('z',[5,1]);
+  self.addParameter('aa',[3,4]);
+end
+function validEq(self,x,z,u,p)
+
+  self.setODE('g',p.aa+z.t);
+  self.setODE('b',z.n);
+  self.setODE('a',p.u);
+  self.setODE('d',x.a+x.b*z.o+p.u*p.x);
+  self.setODE('c',z.p);
+  self.setODE('f',z.s);
+  self.setODE('e',u.k);
+
+  self.setODE('ttt',1);
+
+  % 31x1
+  self.setAlgEquation(reshape(p.y,4,1));
+  self.setAlgEquation(reshape(z.t,12,1));
+  self.setAlgEquation(reshape(x.g,12,1)+[u.a,u.h,u.j,4,5,6,z.n,z.q,p.u,10,11,12].');
+  self.setAlgEquation(p.y(:,1:3,:));
+end
+
+function wrongOdeVars(self)    
+  self.addState('x');
+end
+function wrongOdeEq(self,x,z,u,p)   
+  self.setODE('x',x);
+  self.setODE('y',x+x);
+end
\ No newline at end of file
diff --git a/+ocl/+test/run.m b/+ocl/+tests/run.m
similarity index 88%
rename from +ocl/+test/run.m
rename to +ocl/+tests/run.m
index 02ed9f1a..3edd2fbc 100644
--- a/+ocl/+test/run.m
+++ b/+ocl/+tests/run.m
@@ -4,4 +4,4 @@
 %
 
 function [nFails] = run()
-  nFails = ocl.test.run.core();
\ No newline at end of file
+  nFails = ocl.tests.run.core();
\ No newline at end of file
diff --git a/+ocl/+test/testOclStage.m b/+ocl/+tests/stage.m
similarity index 51%
rename from +ocl/+test/testOclStage.m
rename to +ocl/+tests/stage.m
index 46b5f678..d1772bc4 100644
--- a/+ocl/+test/testOclStage.m
+++ b/+ocl/+tests/stage.m
@@ -1,39 +1,62 @@
-function testOclStage
+function stage
 
 % stage empty test
-stage = OclStage(1, @emptyVars, @emptyDae);
-assertEqual(stage.pathcostfun([],[],[],[]),0);
-assertEqual(stage.gridcostfun(1,10,[],[]),0);
-
-[val,lb,ub] = stage.gridconstraintfun(1,10,[],[]);
-assertEqual(val,[]);
-assertEqual(lb,[]);
-assertEqual(ub,[]);
+stage = ocl.Stage(1, @emptyVars, @emptyDae);
+
+pathcostfun = @(x,z,u,p) ocl.model.pathcosts(stage.pathcostsfh, ...
+                                             stage.x_struct, ...
+                                             stage.z_struct, ...
+                                             stage.u_struct, ...
+                                             stage.p_struct, ...
+                                             x, z, u, p);
+                                           
+gridcostfun = @(k,K,x,p) ocl.model.gridcosts(stage.gridcostsfh, stage.x_struct, stage.p_struct, k, K, x, p);
+gridconstraintfun = @(k,K,x,p) ocl.model.gridconstraints(stage.gridconstraintsfh, stage.x_struct, stage.p_struct, k, K, x, p);
+      
+                                           
+ocl.utils.assertEqual(pathcostfun([],[],[],[]),0);
+ocl.utils.assertEqual(gridcostfun(1,10,[],[]),0);
+
+[val,lb,ub] = gridconstraintfun(1,10,[],[]);
+ocl.utils.assertEqual(val,[]);
+ocl.utils.assertEqual(lb,[]);
+ocl.utils.assertEqual(ub,[]);
 
 % stage valid test
-stage = OclStage(1, @validVars, @validDae, ...
+stage = ocl.Stage(1, @validVars, @validDae, ...
                   @validPathCosts, @validgridCosts, @validgridConstraints);
+                
+
+pathcostfun = @(x,z,u,p) ocl.model.pathcosts(stage.pathcostsfh, ...
+                                             stage.x_struct, ...
+                                             stage.z_struct, ...
+                                             stage.u_struct, ...
+                                             stage.p_struct, ...
+                                             x, z, u, p);
+                                           
+gridcostfun = @(k,K,x,p) ocl.model.gridcosts(stage.gridcostsfh, stage.x_struct, stage.p_struct, k, K, x, p);
+gridconstraintfun = @(k,K,x,p) ocl.model.gridconstraints(stage.gridconstraintsfh, stage.x_struct, stage.p_struct, k, K, x, p);
 
-c = stage.pathcostfun(ones(stage.nx,1),ones(stage.nz,1),ones(stage.nu,1),ones(stage.np,1));
-assertEqual(c,26+1e-3*12);
+c = pathcostfun(ones(stage.nx,1),ones(stage.nz,1),ones(stage.nu,1),ones(stage.np,1));
+ocl.utils.assertEqual(c,26+1e-3*12);
 
-c = stage.gridcostfun(5,5,ones(stage.nx,1),ones(stage.np,1));
-assertEqual(c, -1);
+c = gridcostfun(5,5,ones(stage.nx,1),ones(stage.np,1));
+ocl.utils.assertEqual(c, -1);
 
 % path constraints in the form of : -inf <= val <= 0 or 0 <= val <= 0
-[val,lb,ub] = stage.gridconstraintfun(2,5,ones(stage.nx,1),ones(stage.np,1));
+[val,lb,ub] = gridconstraintfun(2,5,ones(stage.nx,1),ones(stage.np,1));
 % ub all zero
-assertEqual(ub,zeros(36,1));
+ocl.utils.assertEqual(ub,zeros(36,1));
 % lb either zero for eq or -inf for ineq
-assertEqual(lb,[-inf,-inf,0,0,-inf,-inf,-inf*ones(1,5),0,-inf*ones(1,12),-inf*ones(1,12)].');
+ocl.utils.assertEqual(lb,[-inf,-inf,0,0,-inf,-inf,-inf*ones(1,5),0,-inf*ones(1,12),-inf*ones(1,12)].');
 % val = low - high
-assertEqual(val,[0,0,0,0,0,-1,2,2,2,2,2,0,-3*ones(1,12),zeros(1,12)].');
+ocl.utils.assertEqual(val,[0,0,0,0,0,-1,2,2,2,2,2,0,-3*ones(1,12),zeros(1,12)].');
 
 % bc
-[val,lb,ub] = stage.gridconstraintfun(1,5,2*ones(stage.nx,1),ones(stage.np,1));
-assertEqual(ub,zeros(3,1));
-assertEqual(lb,[0,-inf,-inf].');
-assertEqual(val,[-1,1,-4].');
+[val,lb,ub] = gridconstraintfun(1,5,2*ones(stage.nx,1),ones(stage.np,1));
+ocl.utils.assertEqual(ub,zeros(3,1));
+ocl.utils.assertEqual(lb,[0,-inf,-inf].');
+ocl.utils.assertEqual(val,[-1,1,-4].');
   
 end
 
diff --git a/+ocl/+test/testVarStructure.m b/+ocl/+tests/structure.m
similarity index 78%
rename from +ocl/+test/testVarStructure.m
rename to +ocl/+tests/structure.m
index b73ec6b5..3b594b4f 100644
--- a/+ocl/+test/testVarStructure.m
+++ b/+ocl/+tests/structure.m
@@ -3,9 +3,13 @@
 % ensure the above copyright notice is visible in any derived work.
 %
 
-function testVarStructure
-  
-x = OclStructure();
+function structure
+
+assertEqual = @ocl.utils.assertEqual;
+assertSqueezeEqual = @ocl.utils.assertSqueezeEqual;
+assertSetEqual = @ocl.utils.assertSetEqual;
+ 
+x = ocl.types.Structure();
 assertEqual(x.size,[0 1]);
 x.add('x1',[1,2]);
 x.add('x2',[3,2]);
@@ -16,11 +20,11 @@
 assertEqual(p,[3,4,5;6,7,8]')
 assertEqual(x.size,[8 1])
 
-x = OclStructure();
+x = ocl.types.Structure();
 x.add('x1',[1,8]);
 assertEqual(x.size,[8 1])
 
-x = OclStructure();
+x = ocl.types.Structure();
 x.add('x1',[1,3]);
 x.add('x2',[3,2]);
 x.add('x1',[1,3]);
@@ -30,20 +34,20 @@
 [~,p] = x.get('x2');
 assertSqueezeEqual(p,{4:9})
 
-u = OclStructure();
+u = ocl.types.Structure();
 u.add('x1',[1,3]);
 u.add('x3',[3,3]);
 u.add('x1',[1,3]);
 u.add('x3',[3,3]);
 
-x = OclStructure();
+x = ocl.types.Structure();
 x.add('x1',[1,3]);
 x.add('u',u);
 x.add('u',u);
 x.add('x2',[3,2]);
 x.add('x1',[1,3]);
 
-y = OclStructure();
+y = ocl.types.Structure();
 y.add('x1',[1,2]);
 y.add('x1',[1,2]);
 y.add('x1',[1,2]);
@@ -60,10 +64,6 @@
 [~,p] = t.get('x1',p);
 assertSqueezeEqual(p, {[4,5,6],[16,17,18]} );
 
-% flat operator
-f = x.flat();
-[t,p] = f.get('x1');
-assertSetEqual(p,{[1,2,3],[4,5,6],[16,17,18],[28,29,30],[40,41,42],[58,59,60]} );
 
 
 
diff --git a/+ocl/+tests/trajectory.m b/+ocl/+tests/trajectory.m
new file mode 100644
index 00000000..545f7f15
--- /dev/null
+++ b/+ocl/+tests/trajectory.m
@@ -0,0 +1,23 @@
+assertEqual = @ocl.utils.assertEqual;
+
+x_struct = ocl.types.Structure;
+x_struct.add('p',[3,1]);
+x_struct.add('w',[1,1]);
+x_struct.add('R',[3,3]);
+
+t = linspace(0,10,51);
+p = [sin(t);cos(t);2*sin(t)];
+w = tanh(t);
+R = [t;t;t;2*t;2*t;2*t;3*t;3*t;3*t];
+x_data = [p;w;R];
+
+x_traj = ocl.types.Trajectory(x_struct, t, x_data);
+
+assertEqual(x_traj.gridpoints, t);
+
+assertEqual(x_traj{21}.get('p').value, p(:,21));
+assertEqual(x_traj.at(t(21)).get('p').value, p(:,21));
+
+assertEqual(x_traj{12:15}.get('p').value, p(:,12:15));
+
+assertEqual(x_traj.at(t(21)).p.value, p(:,21));
\ No newline at end of file
diff --git a/+ocl/+test/testTreeVariable.m b/+ocl/+tests/treeVariable.m
similarity index 85%
rename from +ocl/+test/testTreeVariable.m
rename to +ocl/+tests/treeVariable.m
index 99563c09..ebb984e9 100644
--- a/+ocl/+test/testTreeVariable.m
+++ b/+ocl/+tests/treeVariable.m
@@ -2,14 +2,17 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function testTreeVariable
+function treeVariable
 
-xStruct = OclStructure();
+assertEqual = @ocl.utils.assertEqual;
+assertSqueezeEqual = @ocl.utils.assertSqueezeEqual;
+
+xStruct = ocl.types.Structure();
 xStruct.add('x1',[1,2]);
 xStruct.add('x2',[3,2]);
 xStruct.add('x1',[1,2]);
 
-x = Variable.create(xStruct,4);
+x = ocl.Variable.create(xStruct,4);
 
 %%% set
 x(:) = (1:10).';
@@ -22,17 +25,17 @@
 %%% slice
 assert(isequal(x.x1(1,1,:).value,[1;9]));
 
-x = OclStructure();
+x = ocl.types.Structure();
 x.add('p',[3,1]);
 x.add('R',[3,3]);
 x.add('v',[3,1]);
 x.add('w',[3,1]);
 
-u = OclStructure();
+u = ocl.types.Structure();
 u.add('elev',[1,1]);
 u.add('ail',[1,1]);
 
-state = Variable.create(x,0);
+state = ocl.Variable.create(x,0);
 
 state.R = eye(3);
 state.p = [100;0;-50];
@@ -49,11 +52,11 @@
 assert( isequal( state.get('p').value,   [100;0;50] ) )
 assert( isequal( state.size,   [18 1] ) )
 
-ocpVar = OclStructure();
+ocpVar = ocl.types.Structure();
 ocpVar.addRepeated({'x','u'},{x,u},5);
 ocpVar.add('x',x);
 
-v = Variable.create(ocpVar,0);
+v = ocl.Variable.create(ocpVar,0);
 v.x.R.set(eye(3));
 v.x.p.set([100;0;50]);
 v.x.v.set([20;0;0]);
@@ -118,7 +121,7 @@
 assertEqual(v.x.R(:,2).value, A(:,2));
 
 % set tests
-if ~isOctave()
+if ~ocl.utils.isOctave()
   v.x.R(:,:,end) = eye(3);
   assertEqual( v.x.R(:,:,end).value, eye(3) );
 end
diff --git a/+ocl/+test/testVariable.m b/+ocl/+tests/variable.m
similarity index 95%
rename from +ocl/+test/testVariable.m
rename to +ocl/+tests/variable.m
index 0e1aaaaa..d42a527e 100644
--- a/+ocl/+test/testVariable.m
+++ b/+ocl/+tests/variable.m
@@ -3,7 +3,7 @@
 % ensure the above copyright notice is visible in any derived work.
 %
 
-function testVariable
+function variable
 
 % small number of numeric value comparison
 mx = false;
@@ -13,8 +13,8 @@
 v2 = [1,5.01,3;6,5,4];
 
 %%% constructor
-a1 = CasadiVariable.Matrix([3,1],mx);
-a2 = CasadiVariable.Matrix([2,3],mx);
+a1 = ocl.casadi.CasadiVariable.Matrix([3,1],mx);
+a2 = ocl.casadi.CasadiVariable.Matrix([2,3],mx);
 s1 = a1.value;
 s2 = a2.value;
 
@@ -104,8 +104,8 @@
      0.0292    0.4886    0.4588];
 b = [0.9631,0.5468,0.5211]';
 
-aA = CasadiVariable.Matrix([3,3],mx);
-ab = CasadiVariable.Matrix([3,1],mx);
+aA = ocl.casadi.CasadiVariable.Matrix([3,3],mx);
+ab = ocl.casadi.CasadiVariable.Matrix([3,1],mx);
 
 sA = aA.value;
 sb = ab.value;
@@ -209,7 +209,7 @@
 %assert(isequal(full(f(v1,v2)),vTest))
 
 % keep type
-assert(isa(aTest,'CasadiVariable'));
+assert(isa(aTest,'ocl.casadi.CasadiVariable'));
 
 end
 
diff --git a/+ocl/+types/+variable/setFromNdMatrix.m b/+ocl/+types/+variable/setFromNdMatrix.m
index fd570424..7c07d3b9 100644
--- a/+ocl/+types/+variable/setFromNdMatrix.m
+++ b/+ocl/+types/+variable/setFromNdMatrix.m
@@ -7,7 +7,7 @@ function setFromNdMatrix(variable, value)
 [Nv,Mv,Kv] = size(value);
 
 if mod(Np,Nv)~=0 || mod(Mp,Mv)~=0 || mod(Kp,Kv)~=0
-  oclError('Can not set values to variable. Dimensions do not match.')
+  ocl.utils.error('Can not set values to variable. Dimensions do not match.')
 end
 
 val = variable.val;
diff --git a/+ocl/+types/+variable/toNdMatrix.m b/+ocl/+types/+variable/toNdMatrix.m
index cce94dea..8f003429 100644
--- a/+ocl/+types/+variable/toNdMatrix.m
+++ b/+ocl/+types/+variable/toNdMatrix.m
@@ -1,5 +1,5 @@
 function vout = toNdMatrix(variable)
-oclAssert(isa(variable, 'Variable'));   
+ocl.utils.assert(isa(variable, 'ocl.Variable'));   
 p = variable.positions;
 vout = zeros(size(p,3), size(p,1), size(p,2));
 for k=1:size(p,3)
diff --git a/+ocl/+types/Bounds.m b/+ocl/+types/Bounds.m
new file mode 100644
index 00000000..0a8f0be4
--- /dev/null
+++ b/+ocl/+types/Bounds.m
@@ -0,0 +1,45 @@
+classdef Bounds < handle
+
+  properties
+    data_p
+  end
+
+  methods
+    function self = Bounds()
+      self.data_p = struct;
+    end
+    
+    function r = data(self)
+      d = self.data_p;
+      
+      names = fieldnames(d);
+      r = cell(length(names), 1);
+      
+      for k=1:length(names)
+        id = names{k};
+        r{k} = d.(id);
+      end
+    end
+
+    function set(self, id, varargin)
+
+      lower = -inf;
+      upper = inf;
+
+      if nargin >= 3
+        lower = varargin{1};
+        upper = varargin{1};
+      end
+      if nargin >= 4
+        upper = varargin{2};
+      end
+
+      d = struct;
+      d.id = id;
+      d.lower = lower;
+      d.upper = upper;
+      
+      self.data_p.(id) = d;
+    end
+  end
+end
diff --git a/+ocl/InitialGuess.m b/+ocl/+types/InitialGuess.m
similarity index 73%
rename from +ocl/InitialGuess.m
rename to +ocl/+types/InitialGuess.m
index f0cfc83b..303196fd 100644
--- a/+ocl/InitialGuess.m
+++ b/+ocl/+types/InitialGuess.m
@@ -9,17 +9,18 @@
 
     function self = InitialGuess(states_struct)
       self.states_struct = states_struct;
+      self.data = struct;
     end
     
-    function add(self, id, xdata, ydata)
+    function set(self, id, xdata, ydata)
       
       % check if id is a state id
-      trajectory_structure = OclStructure();
+      trajectory_structure = ocl.types.Structure();
       trajectory_structure.addRepeated({'states'}, ...
                                        {self.states_struct.get(id)}, ...
                                        length(xdata));
       
-      trajectory = Variable.create(trajectory_structure, 0);
+      trajectory = ocl.Variable.create(trajectory_structure, 0);
       states = trajectory.states;
       states.set(ydata);
       
diff --git a/Core/Variables/OclMatrix.m b/+ocl/+types/Matrix.m
similarity index 83%
rename from Core/Variables/OclMatrix.m
rename to +ocl/+types/Matrix.m
index b7cf4a39..f1f0fa29 100644
--- a/Core/Variables/OclMatrix.m
+++ b/+ocl/+types/Matrix.m
@@ -2,7 +2,7 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-classdef OclMatrix < OclStructure
+classdef Matrix < ocl.types.Structure
   %OCLMATRIX Matrix valued structure for variables
   %
   properties
@@ -11,8 +11,8 @@
   
   methods
     
-    function self = OclMatrix(size)
-      % OclMatrix(size)
+    function self = Matrix(size)
+      % ocl.types.Matrix(size)
       self.msize = size;
     end
     function [N,M,K] = size(self)
diff --git a/Core/Variables/OclStructure.m b/+ocl/+types/Structure.m
old mode 100755
new mode 100644
similarity index 66%
rename from Core/Variables/OclStructure.m
rename to +ocl/+types/Structure.m
index b1e0d280..ab3aaae9
--- a/Core/Variables/OclStructure.m
+++ b/+ocl/+types/Structure.m
@@ -2,7 +2,7 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-classdef OclStructure < handle
+classdef Structure < handle
   % OCLTREE Basic datatype represent variables in a tree like structure.
   %
   properties
@@ -11,8 +11,8 @@
   end
 
   methods
-    function self = OclStructure()
-      % OclTree()
+    function self = Structure()
+      % ocl.types.Structure()
       narginchk(0,0);
       self.children = struct;
       self.len = 0;
@@ -32,19 +32,19 @@ function add(self,id,in2)
         N = 1;
         M = 1;
         K = 1;
-        obj = OclMatrix([N,M]);
+        obj = ocl.types.Matrix([N,M]);
       elseif isnumeric(in2) && length(in2) == 1
         % args:(id,length)
         N = in2;
         M = 1;
         K = 1;
-        obj = OclMatrix([N,M]);
+        obj = ocl.types.Matrix([N,M]);
       elseif isnumeric(in2)
         % args:(id,size)
         N = in2(1);
         M = in2(2);
         K = 1;
-        obj = OclMatrix([N,M]);
+        obj = ocl.types.Matrix([N,M]);
       else
         % args:(id,obj)
         [N,M,K] = in2.size;
@@ -91,6 +91,10 @@ function addObject(self,id,obj,pos)
       p = self.merge(pos,p);
     end
     
+    function r = length(self)
+      r = self.len;
+    end
+    
     function [N,M,K] = size(self)
       if nargout>1
         N = self.len;
@@ -118,48 +122,6 @@ function addObject(self,id,obj,pos)
       end
     end % merge
     
-
-    function tree = flat(self)
-      tree = OclStructure();
-      self.iterateLeafs((1:self.len).',tree);
-    end
-    
-    
-    function iterateLeafs(self,positions,treeOut)
-      childrenIds = fieldnames(self.children);
-      for k=1:length(childrenIds)
-        id = childrenIds{k};
-        [child,pos] = self.get(id,positions);
-        if isa(child,'OclMatrix')
-          treeOut.addObject(id,child,pos);
-        elseif isa(child,'OclStructure')
-          child.iterateLeafs(pos,treeOut);
-        end
-      end
-    end 
-    
-    function valueStruct = toStruct(self,value)
-      positions = (1:self.len).';
-      valueStruct = self.iterateStruct(positions,value);
-    end
-    
-    function [valueStruct,posStruct] = iterateStruct(self,positions,value)
-      
-      valueStruct = struct;
-      valueStruct.value = value(positions);
-      valueStruct.positions = positions;
-      childrenIds = fieldnames(self.children);
-      for k=1:length(childrenIds)
-        id = childrenIds{k};
-        [child,pos] = self.get(id,positions);
-        if isa(child,'OclStructure')
-          childValueStruct = child.iterateStruct(pos,value);
-          valueStruct.(id) = childValueStruct;
-        end
-      end
-    end
-    
-    
   end % methods
 end % class
 
diff --git a/Core/Variables/OclValue.m b/+ocl/+types/Value.m
old mode 100755
new mode 100644
similarity index 87%
rename from Core/Variables/OclValue.m
rename to +ocl/+types/Value.m
index caf3150c..5d399ae9
--- a/Core/Variables/OclValue.m
+++ b/+ocl/+types/Value.m
@@ -2,7 +2,7 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-classdef OclValue < handle
+classdef Value < handle
   % OCLVALUE Class for storing values (numeric or symbolic)
   properties
     val
@@ -20,7 +20,7 @@
   end
   
   methods
-    function self = OclValue(v)
+    function self = Value(v)
       narginchk(1,1); 
       self.val = v;
     end
@@ -33,8 +33,8 @@ function set(self,type,pos,value)
       % set(type,positions,value)
       if ~iscell(value)
         % value is numeric or casadi
-        pos = OclValue.squeeze(pos);
-        value = OclValue.squeeze(value);
+        pos = ocl.types.Value.squeeze(pos);
+        value = ocl.types.Value.squeeze(value);
         [Np,Mp,Kp] = size(pos);
         [Nv,Mv] = size(value);
         if isempty(value) || Nv*Mv==0
@@ -42,7 +42,7 @@ function set(self,type,pos,value)
         end
 
         if mod(Np,Nv)~=0 || mod(Mp,Mv)~=0
-          oclError('Can not set values to variable. Dimensions do not match.')
+          ocl.utils.error('Can not set values to variable. Dimensions do not match.')
         end
         
         for k=1:Kp
diff --git a/+ocl/+utils/Reference.m b/+ocl/+utils/Reference.m
new file mode 100644
index 00000000..e0990f24
--- /dev/null
+++ b/+ocl/+utils/Reference.m
@@ -0,0 +1,20 @@
+classdef Reference < handle
+  
+  properties
+    data
+  end
+  
+  methods 
+    function self = Reference(value)
+      self.set(value);
+    end
+    
+    function set(self, value)
+      self.data = value;
+    end
+    
+    function r = get(self)
+      r = self.data;
+    end
+  end
+end
\ No newline at end of file
diff --git a/+ocl/+utils/argError.m b/+ocl/+utils/argError.m
new file mode 100644
index 00000000..43a62bb5
--- /dev/null
+++ b/+ocl/+utils/argError.m
@@ -0,0 +1,4 @@
+function argError(arg)
+ocl.utils.error(['Wrong value for argument ', arg, ' given. Please check the docs at ', ocl.utils.docMessage()]);
+end
+
diff --git a/+ocl/+utils/assert.m b/+ocl/+utils/assert.m
new file mode 100644
index 00000000..b4b950f0
--- /dev/null
+++ b/+ocl/+utils/assert.m
@@ -0,0 +1,3 @@
+function assert(condition, varargin)
+  assert(condition,varargin{:});
+end
\ No newline at end of file
diff --git a/Core/utils/assertAlmostEqual.m b/+ocl/+utils/assertAlmostEqual.m
similarity index 100%
rename from Core/utils/assertAlmostEqual.m
rename to +ocl/+utils/assertAlmostEqual.m
diff --git a/Core/utils/assertEqual.m b/+ocl/+utils/assertEqual.m
similarity index 100%
rename from Core/utils/assertEqual.m
rename to +ocl/+utils/assertEqual.m
diff --git a/Core/utils/assertException.m b/+ocl/+utils/assertException.m
similarity index 82%
rename from Core/utils/assertException.m
rename to +ocl/+utils/assertException.m
index eb3ec717..683891ae 100644
--- a/Core/utils/assertException.m
+++ b/+ocl/+utils/assertException.m
@@ -8,7 +8,7 @@ function assertException(compStr,fh, varargin)
     fh(varargin{:});
   catch e
     thrown = true;
-    assert(contains(e.message,'oclException'),['Wrong exception raised. Not an oclException! ', e.message]);
+    assert(contains(e.message,'OCL EXCEPTION'),['Wrong exception raised. Not an ocl.utils.exception! ', e.message]);
     assert(contains(e.message,compStr), ['Wrong exception raised.', e.message]);
   end
   if ~thrown
diff --git a/Core/utils/assertSetEqual.m b/+ocl/+utils/assertSetEqual.m
similarity index 75%
rename from Core/utils/assertSetEqual.m
rename to +ocl/+utils/assertSetEqual.m
index ee0b4b1e..e4b125eb 100644
--- a/Core/utils/assertSetEqual.m
+++ b/+ocl/+utils/assertSetEqual.m
@@ -5,7 +5,7 @@
 function assertSetEqual(A,B)
   if iscell(A);A=cell2mat(A);end
   if iscell(B);B=cell2mat(B);end
-  assertEqual(numel(A), numel(B))
-  assertEqual(numel(intersect(A,B)), numel(A))
+  ocl.utils.assertEqual(numel(A), numel(B))
+  ocl.utils.assertEqual(numel(intersect(A,B)), numel(A))
   
   
\ No newline at end of file
diff --git a/Core/utils/assertSqueezeEqual.m b/+ocl/+utils/assertSqueezeEqual.m
similarity index 90%
rename from Core/utils/assertSqueezeEqual.m
rename to +ocl/+utils/assertSqueezeEqual.m
index d3713ef2..370f67ea 100644
--- a/Core/utils/assertSqueezeEqual.m
+++ b/+ocl/+utils/assertSqueezeEqual.m
@@ -18,6 +18,6 @@ function assertSqueezeEqual(a,b,varargin)
 a = a(:);
 b = b(:);
 
-assertEqual(a,b,varargin{:})
+ocl.utils.assertEqual(a,b,varargin{:})
   
 end
diff --git a/+ocl/+utils/checkStartup.m b/+ocl/+utils/checkStartup.m
index 5b981e74..2af5e6d1 100644
--- a/+ocl/+utils/checkStartup.m
+++ b/+ocl/+utils/checkStartup.m
@@ -3,10 +3,17 @@
 % ensure the above copyright notice is visible in any derived work.
 %
 function checkStartup()
-  persistent been_here
-  if isempty(been_here) || ~been_here
+  OCL_CASADI_SETUP = getenv('OCL_CASADI_SETUP');
+  
+  if strcmp(OCL_CASADI_SETUP, 'true')
+    ocl_casadi_setup_completed = true;
+  else
+    ocl_casadi_setup_completed = false;
+  end
+  
+  if isempty(ocl_casadi_setup_completed) || ~ocl_casadi_setup_completed
     disp('Running OpenOCL setup procedure. This may required your input, and may take a while at the first time.')
-    ocl.utils.StartupOCL();
-    been_here = true;
+    ocl.utils.startup();
+    setenv('OCL_CASADI_SETUP', 'true');
   end
 end
\ No newline at end of file
diff --git a/+ocl/+utils/clean.m b/+ocl/+utils/clean.m
index 8d5f2f18..7507eb33 100644
--- a/+ocl/+utils/clean.m
+++ b/+ocl/+utils/clean.m
@@ -1,5 +1,7 @@
 function clean
 
+  setenv('OCL_CASADI_SETUP', 'false');
+
   oclPath  = fileparts(which('ocl'));
   addpath(fullfile(oclPath,'Lib','casadi'));
   rmpath(fullfile(oclPath,'Lib','casadi'));
diff --git a/Core/utils/oclDeprecation.m b/+ocl/+utils/deprecation.m
similarity index 80%
rename from Core/utils/oclDeprecation.m
rename to +ocl/+utils/deprecation.m
index ea99d830..46d6b2ba 100644
--- a/Core/utils/oclDeprecation.m
+++ b/+ocl/+utils/deprecation.m
@@ -2,5 +2,5 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function oclDeprecation(msg)
-  oclWarning(msg);
\ No newline at end of file
+function deprecation(msg)
+  ocl.utils.warning(msg);
\ No newline at end of file
diff --git a/Core/utils/oclDocMessage.m b/+ocl/+utils/docMessage.m
similarity index 64%
rename from Core/utils/oclDocMessage.m
rename to +ocl/+utils/docMessage.m
index bdc1dd72..2575d54f 100644
--- a/Core/utils/oclDocMessage.m
+++ b/+ocl/+utils/docMessage.m
@@ -1,2 +1,2 @@
-function s = oclDocMessage()
+function s = docMessage()
   s = 'Read the docs at: https://openocl.org/api-docs/';
diff --git a/+ocl/+utils/emptyfh.m b/+ocl/+utils/emptyfh.m
new file mode 100644
index 00000000..dbdc1688
--- /dev/null
+++ b/+ocl/+utils/emptyfh.m
@@ -0,0 +1,2 @@
+function r = emptyfh()
+r = @(varargin)[];
\ No newline at end of file
diff --git a/+ocl/+utils/emptyfun.m b/+ocl/+utils/emptyfun.m
new file mode 100644
index 00000000..4deca8e2
--- /dev/null
+++ b/+ocl/+utils/emptyfun.m
@@ -0,0 +1,2 @@
+function r = emptyfun(varargin)
+r = [];
\ No newline at end of file
diff --git a/Core/utils/oclInfo.m b/+ocl/+utils/error.m
similarity index 86%
rename from Core/utils/oclInfo.m
rename to +ocl/+utils/error.m
index a937e4e6..50b66b09 100644
--- a/Core/utils/oclInfo.m
+++ b/+ocl/+utils/error.m
@@ -2,5 +2,5 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function oclInfo(msg)
-  disp(msg)
\ No newline at end of file
+function error(msg)
+  error(msg)
\ No newline at end of file
diff --git a/Core/utils/oclError.m b/+ocl/+utils/exception.m
similarity index 79%
rename from Core/utils/oclError.m
rename to +ocl/+utils/exception.m
index 72b11c52..41f4fd84 100644
--- a/Core/utils/oclError.m
+++ b/+ocl/+utils/exception.m
@@ -2,5 +2,5 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function oclError(msg)
-  error(msg)
\ No newline at end of file
+function exception(msg)
+  error(['OCL EXCEPTION: ', msg])
\ No newline at end of file
diff --git a/+ocl/+utils/fieldnamesContain.m b/+ocl/+utils/fieldnamesContain.m
new file mode 100644
index 00000000..2e365f3f
--- /dev/null
+++ b/+ocl/+utils/fieldnamesContain.m
@@ -0,0 +1,3 @@
+function r = fieldnamesContain(names, id)
+  r = any(strcmp(names,id));
+end
\ No newline at end of file
diff --git a/+ocl/+utils/findjobj.m b/+ocl/+utils/findjobj.m
new file mode 100644
index 00000000..4a875cda
--- /dev/null
+++ b/+ocl/+utils/findjobj.m
@@ -0,0 +1,3456 @@
+function [handles,levels,parentIdx,listing] = findjobj(container,varargin) %#ok<*CTCH,*ASGLU,*MSNU,*NASGU>
+%findjobj Find java objects contained within a specified java container or Matlab GUI handle
+%
+% Syntax:
+%    [handles, levels, parentIds, listing] = findjobj(container, 'PropName',PropValue(s), ...)
+%
+% Input parameters:
+%    container - optional handle to java container uipanel or figure. If unsupplied then current figure will be used
+%    'PropName',PropValue - optional list of property pairs (case insensitive). PropName may also be named -PropName
+%         'position' - filter results based on those elements that contain the specified X,Y position or a java element
+%                      Note: specify a Matlab position (X,Y = pixels from bottom left corner), not a java one
+%         'size'     - filter results based on those elements that have the specified W,H (in pixels)
+%         'class'    - filter results based on those elements that contain the substring  (or java class) PropValue
+%                      Note1: filtering is case insensitive and relies on regexp, so you can pass wildcards etc.
+%                      Note2: '-class' is an undocumented findobj PropName, but only works on Matlab (not java) classes
+%         'property' - filter results based on those elements that possess the specified case-insensitive property string
+%                      Note1: passing a property value is possible if the argument following 'property' is a cell in the
+%                             format of {'propName','propValue'}. Example: FINDJOBJ(...,'property',{'Text','click me'})
+%                      Note2: partial property names (e.g. 'Tex') are accepted, as long as they're not ambiguous
+%         'depth'    - filter results based on specified depth. 0=top-level, Inf=all levels (default=Inf)
+%         'flat'     - same as specifying: 'depth',0
+%         'not'      - negates the following filter: 'not','class','c' returns all elements EXCEPT those with class 'c'
+%         'persist'  - persist figure components information, allowing much faster results for subsequent invocations
+%         'nomenu'   - skip menu processing, for "lean" list of handles & much faster processing;
+%                      This option is the default for HG containers but not for figure, Java or no container
+%         'print'    - display all java elements in a hierarchical list, indented appropriately
+%                      Note1: optional PropValue of element index or handle to java container
+%                      Note2: normally this option would be placed last, after all filtering is complete. Placing this
+%                             option before some filters enables debug print-outs of interim filtering results.
+%                      Note3: output is to the Matlab command window unless the 'listing' (4th) output arg is requested
+%         'list'     - same as 'print'
+%         'debug'    - list found component positions in the Command Window
+%
+% Output parameters:
+%    handles   - list of handles to java elements
+%    levels    - list of corresponding hierarchy level of the java elements (top=0)
+%    parentIds - list of indexes (in unfiltered handles) of the parent container of the corresponding java element
+%    listing   - results of 'print'/'list' options (empty if these options were not specified)
+%
+%    Note: If no output parameter is specified, then an interactive window will be displayed with a
+%    ^^^^  tree view of all container components, their properties and callbacks.
+%
+% Examples:
+%    findjobj;                     % display list of all javaelements of currrent figure in an interactive GUI
+%    handles = findjobj;           % get list of all java elements of current figure (inc. menus, toolbars etc.)
+%    findjobj('print');            % list all java elements in current figure
+%    findjobj('print',6);          % list all java elements in current figure, contained within its 6th element
+%    handles = findjobj(hButton);                                     % hButton is a matlab button
+%    handles = findjobj(gcf,'position',getpixelposition(hButton,1));  % same as above but also return hButton's panel
+%    handles = findjobj(hButton,'persist');                           % same as above, persist info for future reuse
+%    handles = findjobj('class','pushbutton');                        % get all pushbuttons in current figure
+%    handles = findjobj('class','pushbutton','position',123,456);     % get all pushbuttons at the specified position
+%    handles = findjobj(gcf,'class','pushbutton','size',23,15);       % get all pushbuttons with the specified size
+%    handles = findjobj('property','Text','not','class','button');    % get all non-button elements with 'text' property
+%    handles = findjobj('-property',{'Text','click me'});             % get all elements with 'text' property = 'click me'
+%
+% Sample usage:
+%    hButton = uicontrol('string','click me');
+%    jButton = findjobj(hButton,'nomenu');
+%      % or: jButton = findjobj('property',{'Text','click me'});
+%    jButton.setFlyOverAppearance(1);
+%    jButton.setCursor(java.awt.Cursor.getPredefinedCursor(java.awt.Cursor.HAND_CURSOR));
+%    set(jButton,'FocusGainedCallback',@myMatlabFunction);   % some 30 callback points available...
+%    jButton.get;   % list all changeable properties...
+%
+%    hEditbox = uicontrol('style','edit');
+%    jEditbox = findjobj(hEditbox,'nomenu');
+%    jEditbox.setCaretColor(java.awt.Color.red);
+%    jEditbox.KeyTypedCallback = @myCallbackFunc;  % many more callbacks where this came from...
+%    jEdit.requestFocus;
+%
+% Technical explanation & details:
+%    http://undocumentedmatlab.com/blog/findjobj/
+%    http://undocumentedmatlab.com/blog/findjobj-gui-display-container-hierarchy/
+%
+% Known issues/limitations:
+%    - Cannot currently process multiple container objects - just one at a time
+%    - Initial processing is a bit slow when the figure is laden with many UI components (so better use 'persist')
+%    - Passing a simple container Matlab handle is currently filtered by its position+size: should find a better way to do this
+%    - Matlab uipanels are not implemented as simple java panels, and so they can't be found using this utility
+%    - Labels have a write-only text property in java, so they can't be found using the 'property',{'Text','string'} notation
+%
+% Warning:
+%    This code heavily relies on undocumented and unsupported Matlab functionality.
+%    It works on Matlab 7+, but use at your own risk!
+%
+% Bugs and suggestions:
+%    Please send to Yair Altman (altmany at gmail dot com)
+%
+% Change log:
+%    2019-07-03: Additional fix for R2018b; added separate findjobj_fast utility
+%    2018-09-21: Fix for R2018b suggested by Eddie (FEX); speedup suggested by Martin Lehmann (FEX); alert if trying to use with uifigure
+%    2017-04-13: Fixed two edge-cases (one suggested by H. Koch)
+%    2016-04-19: Fixed edge-cases in old Matlab release; slightly improved performance even further
+%    2016-04-14: Improved performance for the most common use-case (single input/output): improved code + allow inspecting groot
+%    2016-04-11: Improved performance for the most common use-case (single input/output)
+%    2015-01-12: Differentiate between overlapping controls (for example in different tabs); fixed case of docked figure
+%    2014-10-20: Additional fixes for R2014a, R2014b
+%    2014-10-13: Fixes for R2014b
+%    2014-01-04: Minor fix for R2014a; check for newer FEX version up to twice a day only
+%    2013-12-29: Only check for newer FEX version in non-deployed mode; handled case of invisible figure container
+%    2013-10-08: Fixed minor edge case (retrieving multiple scroll-panes)
+%    2013-06-30: Additional fixes for the upcoming HG2
+%    2013-05-15: Fix for the upcoming HG2
+%    2013-02-21: Fixed HG-Java warnings
+%    2013-01-23: Fixed callbacks table grouping & editing bugs; added hidden properties to the properties tooltip; updated help section
+%    2013-01-13: Improved callbacks table; fixed tree refresh failure; fixed: tree node-selection didn't update the props pane nor flash the selected component
+%    2012-07-25: Fixes for R2012a as well as some older Matlab releases
+%    2011-12-07: Fixed 'File is empty' messages in compiled apps
+%    2011-11-22: Fix suggested by Ward
+%    2011-02-01: Fixes for R2011a
+%    2010-06-13: Fixes for R2010b; fixed download (m-file => zip-file)
+%    2010-04-21: Minor fix to support combo-boxes (aka drop-down, popup-menu) on Windows
+%    2010-03-17: Important release: Fixes for R2010a, debug listing, objects not found, component containers that should be ignored etc.
+%    2010-02-04: Forced an EDT redraw before processing; warned if requested handle is invisible
+%    2010-01-18: Found a way to display label text next to the relevant node name
+%    2009-10-28: Fixed uitreenode warning
+%    2009-10-27: Fixed auto-collapse of invisible container nodes; added dynamic tree tooltips & context-menu; minor fix to version-check display
+%    2009-09-30: Fix for Matlab 7.0 as suggested by Oliver W; minor GUI fix (classname font)
+%    2009-08-07: Fixed edge-case of missing JIDE tables
+%    2009-05-24: Added support for future Matlab versions that will not support JavaFrame
+%    2009-05-15: Added sanity checks for axes items
+%    2009-04-28: Added 'debug' input arg; increased size tolerance 1px => 2px
+%    2009-04-23: Fixed location of popupmenus (always 20px high despite what's reported by Matlab...); fixed uiinspect processing issues; added blog link; narrower action buttons
+%    2009-04-09: Automatic 'nomenu' for uicontrol inputs; significant performance improvement
+%    2009-03-31: Fixed position of some Java components; fixed properties tooltip; fixed node visibility indication
+%    2009-02-26: Indicated components visibility (& auto-collapse non-visible containers); auto-highlight selected component; fixes in node icons, figure title & tree refresh; improved error handling; display FindJObj version update description if available
+%    2009-02-24: Fixed update check; added dedicated labels icon
+%    2009-02-18: Fixed compatibility with old Matlab versions
+%    2009-02-08: Callbacks table fixes; use uiinspect if available; fix update check according to new FEX website
+%    2008-12-17: R2008b compatibility
+%    2008-09-10: Fixed minor bug as per Johnny Smith
+%    2007-11-14: Fixed edge case problem with class properties tooltip; used existing object icon if available; added checkbox option to hide standard callbacks
+%    2007-08-15: Fixed object naming relative property priorities; added sanity check for illegal container arg; enabled desktop (0) container; cleaned up warnings about special class objects
+%    2007-08-03: Fixed minor tagging problems with a few Java sub-classes; displayed UIClassID if text/name/tag is unavailable
+%    2007-06-15: Fixed problems finding HG components found by J. Wagberg
+%    2007-05-22: Added 'nomenu' option for improved performance; fixed 'export handles' bug; fixed handle-finding/display bugs; "cleaner" error handling
+%    2007-04-23: HTMLized classname tooltip; returned top-level figure Frame handle for figure container; fixed callbacks table; auto-checked newer version; fixed Matlab 7.2 compatibility issue; added HG objects tree
+%    2007-04-19: Fixed edge case of missing figure; displayed tree hierarchy in interactive GUI if no output args; workaround for figure sub-menus invisible unless clicked
+%    2007-04-04: Improved performance; returned full listing results in 4th output arg; enabled partial property names & property values; automatically filtered out container panels if children also returned; fixed finding sub-menu items
+%    2007-03-20: First version posted on the MathWorks file exchange: <a href="http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=14317">http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=14317</a>
+%
+% See also:
+%    java, handle, findobj, findall, javaGetHandles, uiinspect (on the File Exchange)
+
+% License to use and modify this code is granted freely to all interested, as long as the original author is
+% referenced and attributed as such. The original author maintains the right to be solely associated with this work.
+
+% Programmed and Copyright by Yair M. Altman: altmany(at)gmail.com
+% $Revision: 1.52 $  $Date: 2019/07/03 19:09:23 $
+
+    % Ensure Java AWT is enabled
+    error(javachk('awt'));
+
+    persistent pContainer pHandles pLevels pParentIdx pPositions
+
+    try
+        % Initialize
+        handles = handle([]);
+        levels = [];
+        parentIdx = [];
+        positions = [];   % Java positions start at the top-left corner
+        %sizes = [];
+        listing = '';
+        hg_levels = [];
+        hg_handles = handle([]);  % HG handles are double
+        hg_parentIdx = [];
+        nomenu = false;
+        menuBarFoundFlag = false;
+        hFig = [];
+
+        % Force an EDT redraw before processing, to ensure all uicontrols etc. are rendered
+        drawnow;  pause(0.02);
+
+        % Default container is the current figure's root panel
+        if nargin
+            if isempty(container)  % empty container - bail out
+                return;
+            elseif ischar(container)  % container skipped - this is part of the args list...
+                varargin = [{container}, varargin];
+                origContainer = getCurrentFigure;
+                [container,contentSize] = getRootPanel(origContainer);
+            elseif isequal(container,0)  % root
+                origContainer = handle(container);
+                container = com.mathworks.mde.desk.MLDesktop.getInstance.getMainFrame;
+                contentSize = [container.getWidth, container.getHeight];
+            elseif ishghandle(container) % && ~isa(container,'java.awt.Container')
+                container = container(1);  % another current limitation...
+                hFig = ancestor(container,'figure');
+                oldWarn = warning('off','MATLAB:HandleGraphics:ObsoletedProperty:JavaFrame');  % R2008b compatibility
+                try hJavaFrame = get(hFig,'JavaFrame'); catch, hJavaFrame = []; end
+                warning(oldWarn);
+                if isempty(hJavaFrame)  % alert if trying to use with web-based (not Java-based) uifigure
+                    error('YMA:findjobj:NonJavaFigure', 'Findjobj only works with Java-based figures, not web-based (App Designer) uifigures');
+                end
+                origContainer = handle(container);
+                if isa(origContainer,'uimenu') || isa(origContainer,'matlab.ui.container.Menu')
+                    % getpixelposition doesn't work for menus... - damn!
+                    varargin = {'class','MenuPeer', 'property',{'Label',strrep(get(container,'Label'),'&','')}, varargin{:}};
+                elseif ~isa(origContainer, 'figure') && ~isempty(hFig) && ~isa(origContainer, 'matlab.ui.Figure')
+                    % For a single input & output, try using the fast variant
+                    if nargin==1 && nargout==1
+                        try
+                            handles = findjobj_fast(container);
+                            if ~isempty(handles)
+                                try handles = handle(handles,'callbackproperties'); catch, end
+                                return
+                            end
+                        catch
+                            % never mind - proceed normally using the slower variant below...
+                        end
+                    end
+
+                    % See limitations section above: should find a better way to directly refer to the element's java container
+                    try
+                        % Note: 'PixelBounds' is undocumented and unsupported, but much faster than getpixelposition!
+                        % ^^^^  unfortunately, its Y position is inaccurate in some cases - damn!
+                        %size = get(container,'PixelBounds');
+                        pos = fix(getpixelposition(container,1));
+                        %varargin = {'position',pos(1:2), 'size',pos(3:4), 'not','class','java.awt.Panel', varargin{:}};
+                    catch
+                        try
+                            figName = get(hFig,'name');
+                            if strcmpi(get(hFig,'number'),'on')
+                                figName = regexprep(['Figure ' num2str(hFig) ': ' figName],': $','');
+                            end
+                            mde = com.mathworks.mde.desk.MLDesktop.getInstance;
+                            jFig = mde.getClient(figName);
+                            if isempty(jFig), error('dummy');  end
+                        catch
+                            jFig = get(hJavaFrame,'FigurePanelContainer');
+                        end
+                        pos = [];
+                        try
+                            pxsize = get(container,'PixelBounds');
+                            pos = [pxsize(1)+5, jFig.getHeight - (pxsize(4)-5)];
+                        catch
+                            % never mind...
+                        end
+                    end
+                    if size(pos,2) == 2
+                        pos(:,3:4) = 0;
+                    end
+                    if ~isempty(pos)
+                        if isa(handle(container),'uicontrol') && strcmp(get(container,'style'),'popupmenu')
+                            % popupmenus (combo-box dropdowns) are always 20px high
+                            pos(2) = pos(2) + pos(4) - 20;
+                            pos(4) = 20;
+                        end
+                        %varargin = {'position',pos(1:2), 'size',size(3:4)-size(1:2)-10, 'not','class','java.awt.Panel', varargin{:}};
+                        varargin = {'position',pos(1:2)+[0,pos(4)], 'size',pos(3:4), 'not','class','java.awt.Panel', 'nomenu', varargin{:}};
+                    end
+                elseif isempty(hFig)
+                    hFig = handle(container);
+                end
+                [container,contentSize] = getRootPanel(hFig);
+            elseif isjava(container)
+                % Maybe a java container obj (will crash otherwise)
+                origContainer = container;
+                contentSize = [container.getWidth, container.getHeight];
+            else
+                error('YMA:findjobj:IllegalContainer','Input arg does not appear to be a valid GUI object');
+            end
+        else
+            % Default container = current figure
+            origContainer = getCurrentFigure;
+            [container,contentSize] = getRootPanel(origContainer);
+        end
+
+        % Check persistency
+        if isequal(pContainer,container)
+            % persistency requested and the same container is reused, so reuse the hierarchy information
+            [handles,levels,parentIdx,positions] = deal(pHandles, pLevels, pParentIdx, pPositions);
+        else
+            % Pre-allocate space of complex data containers for improved performance
+            handles = repmat(handles,1,1000);
+            positions = zeros(1000,2);
+
+            % Check whether to skip menu processing
+            nomenu = paramSupplied(varargin,'nomenu');
+
+            % Traverse the container hierarchy and extract the elements within
+            traverseContainer(container,0,1);
+
+            % Remove unnecessary pre-allocated elements
+            dataLen = length(levels);
+            handles  (dataLen+1:end) = [];
+            positions(dataLen+1:end,:) = [];
+        end
+
+        % Process persistency check before any filtering is done
+        if paramSupplied(varargin,'persist')
+            [pContainer, pHandles, pLevels, pParentIdx, pPositions] = deal(container,handles,levels,parentIdx,positions);
+        end
+
+        % Save data for possible future use in presentObjectTree() below
+        allHandles = handles;
+        allLevels  = levels;
+        allParents = parentIdx;
+        selectedIdx = 1:length(handles);
+        %[positions(:,1)-container.getX, container.getHeight - positions(:,2)]
+
+        % Debug-list all found compponents and their positions
+        if paramSupplied(varargin,'debug')
+            for origHandleIdx = 1 : length(allHandles)
+                thisObj = handles(origHandleIdx);
+                pos = sprintf('%d,%d %dx%d',[positions(origHandleIdx,:) getWidth(thisObj) getHeight(thisObj)]);
+                disp([repmat(' ',1,levels(origHandleIdx)) '[' pos '] ' char(toString(thisObj))]);
+            end
+        end
+
+        % Process optional args
+        % Note: positions is NOT returned since it's based on java coord system (origin = top-left): will confuse Matlab users
+        processArgs(varargin{:});
+
+        % De-cell and trim listing, if only one element was found (no use for indented listing in this case)
+        if iscell(listing) && length(listing)==1
+            listing = strtrim(listing{1});
+        end
+
+        % If no output args and no listing requested, present the FINDJOBJ interactive GUI
+        if nargout == 0 && isempty(listing)
+
+            % Get all label positions
+            hg_labels = [];
+            labelPositions = getLabelsJavaPos(container);
+
+            % Present the GUI (object tree)
+            if ~isempty(container)
+                presentObjectTree();
+            else
+                warnInvisible(varargin{:});
+            end
+
+        % Display the listing, if this was specifically requested yet no relevant output arg was specified
+        elseif nargout < 4 && ~isempty(listing)
+            if ~iscell(listing)
+                disp(listing);
+            else
+                for listingIdx = 1 : length(listing)
+                    disp(listing{listingIdx});
+                end
+            end
+        end
+
+        % If the number of output handles does not match the number of inputs, try to match via tooltips
+        if nargout && numel(handles) ~= numel(origContainer)
+            newHandles = handle([]);
+            switchHandles = false;
+            handlesToCheck = handles;
+            if isempty(handles)
+                handlesToCheck = allHandles;
+            end
+            for origHandleIdx = 1 : numel(origContainer)
+                try
+                    thisContainer = origContainer(origHandleIdx);
+                    if isa(thisContainer,'figure') || isa(thisContainer,'matlab.ui.Figure')
+                        break;
+                    end
+                    try
+                        newHandles(origHandleIdx) = handlesToCheck(origHandleIdx); %#ok<AGROW>
+                    catch
+                        % maybe no corresponding handle in [handles]
+                    end
+
+                    % Assign a new random tooltip to the original (Matlab) handle
+                    oldTooltip = get(thisContainer,'Tooltip');
+                    newTooltip = num2str(rand,99);
+                    try
+                        set(thisContainer,'TooltipString',newTooltip);
+                    catch
+                        set(thisContainer,'Tooltip',newTooltip);
+                    end
+                    drawnow;  % force the Java handle to sync with the Matlab prop-change
+                    try
+                        % Search for a Java handle that has the newly-generated tooltip
+                        for newHandleIdx = 1 : numel(handlesToCheck)
+                            testTooltip = '';
+                            thisHandle = handlesToCheck(newHandleIdx);
+                            try
+                                testTooltip = char(thisHandle.getToolTipText);
+                            catch
+                                try testTooltip = char(thisHandle.getTooltipText); catch, end
+                            end
+                            if isempty(testTooltip)
+                                % One more attempt - assume it's enclosed by a scroll-pane
+                                try testTooltip = char(thisHandle.getViewport.getView.getToolTipText); catch, end
+                            end
+                            if strcmp(testTooltip, newTooltip)
+                                newHandles(origHandleIdx) = thisHandle;
+                                switchHandles = true;
+                                break;
+                            end
+                        end
+                    catch
+                        % never mind - skip to the next handle
+                    end
+                    set(thisContainer,'Tooltip',oldTooltip);
+                catch
+                    % never mind - skip to the next handle (maybe handle doesn't have a tooltip prop)
+                end
+            end
+            if switchHandles
+                handles = newHandles;
+            end
+        end
+
+        % Display a warning if the requested handle was not found because it's invisible
+        if nargout && isempty(handles)
+            warnInvisible(varargin{:});
+        end
+
+        return;  %debug point
+
+    catch
+        % 'Cleaner' error handling - strip the stack info etc.
+        err = lasterror;  %#ok
+        err.message = regexprep(err.message,'Error using ==> [^\n]+\n','');
+        if isempty(findstr(mfilename,err.message))
+            % Indicate error origin, if not already stated within the error message
+            err.message = [mfilename ': ' err.message];
+        end
+        rethrow(err);
+    end
+
+    %% Display a warning if the requested handle was not found because it's invisible
+    function warnInvisible(varargin)
+        try
+            if strcmpi(get(hFig,'Visible'),'off')
+                pos = get(hFig,'Position');
+                set(hFig, 'Position',pos-[5000,5000,0,0], 'Visible','on');
+                drawnow; pause(0.01);
+                [handles,levels,parentIdx,listing] = findjobj(origContainer,varargin{:});
+                set(hFig, 'Position',pos, 'Visible','off');
+            end
+        catch
+            % ignore - move on...
+        end
+        try
+            stk = dbstack;
+            stkNames = {stk.name};
+            OutputFcnIdx = find(~cellfun(@isempty,regexp(stkNames,'_OutputFcn')));
+            if ~isempty(OutputFcnIdx)
+                OutputFcnName = stkNames{OutputFcnIdx};
+                warning('YMA:FindJObj:OutputFcn',['No Java reference was found for the requested handle, because the figure is still invisible in ' OutputFcnName '()']);
+            elseif ishandle(origContainer) && isprop(origContainer,'Visible') && strcmpi(get(origContainer,'Visible'),'off')
+                warning('YMA:FindJObj:invisibleHandle','No Java reference was found for the requested handle, probably because it is still invisible');
+            end
+        catch
+            % Never mind...
+        end
+    end
+
+    %% Check existence of a (case-insensitive) optional parameter in the params list
+    function [flag,idx] = paramSupplied(paramsList,paramName)
+        %idx = find(~cellfun('isempty',regexpi(paramsList(cellfun(@ischar,paramsList)),['^-?' paramName])));
+        idx = find(~cellfun('isempty',regexpi(paramsList(cellfun('isclass',paramsList,'char')),['^-?' paramName])));  % 30/9/2009 fix for ML 7.0 suggested by Oliver W
+        flag = any(idx);
+    end
+
+    %% Get current figure (even if its 'HandleVisibility' property is 'off')
+    function curFig = getCurrentFigure
+        oldShowHidden = get(0,'ShowHiddenHandles');
+        set(0,'ShowHiddenHandles','on');  % minor fix per Johnny Smith
+        curFig = gcf;
+        set(0,'ShowHiddenHandles',oldShowHidden);
+    end
+
+    %% Get Java reference to top-level (root) panel - actually, a reference to the java figure
+    function [jRootPane,contentSize] = getRootPanel(hFig)
+        try
+            contentSize = [0,0];  % initialize
+            jRootPane = hFig;
+            figName = get(hFig,'name');
+            if strcmpi(get(hFig,'number'),'on')
+                figName = regexprep(['Figure ' num2str(hFig) ': ' figName],': $','');
+            end
+            mde = com.mathworks.mde.desk.MLDesktop.getInstance;
+            jFigPanel = mde.getClient(figName);
+            jRootPane = jFigPanel;
+            jRootPane = jFigPanel.getRootPane;
+        catch
+            try
+                warning('off','MATLAB:HandleGraphics:ObsoletedProperty:JavaFrame');  % R2008b compatibility
+                jFrame = get(hFig,'JavaFrame');
+                jFigPanel = get(jFrame,'FigurePanelContainer');
+                jRootPane = jFigPanel;
+                jRootPane = jFigPanel.getComponent(0).getRootPane;
+            catch
+                % Never mind
+            end
+        end
+        try
+            % If invalid RootPane - try another method...
+            warning('off','MATLAB:HandleGraphics:ObsoletedProperty:JavaFrame');  % R2008b compatibility
+            jFrame = get(hFig,'JavaFrame');
+            jAxisComponent = get(jFrame,'AxisComponent');
+            jRootPane = jAxisComponent.getParent.getParent.getRootPane;
+        catch
+            % Never mind
+        end
+        try
+            % If invalid RootPane, retry up to N times
+            tries = 10;
+            while isempty(jRootPane) && tries>0  % might happen if figure is still undergoing rendering...
+                drawnow; pause(0.001);
+                tries = tries - 1;
+                jRootPane = jFigPanel.getComponent(0).getRootPane;
+            end
+
+            % If still invalid, use FigurePanelContainer which is good enough in 99% of cases... (menu/tool bars won't be accessible, though)
+            if isempty(jRootPane)
+                jRootPane = jFigPanel;
+            end
+            contentSize = [jRootPane.getWidth, jRootPane.getHeight];
+
+            % Try to get the ancestor FigureFrame
+            jRootPane = jRootPane.getTopLevelAncestor;
+        catch
+            % Never mind - FigurePanelContainer is good enough in 99% of cases... (menu/tool bars won't be accessible, though)
+        end
+    end
+
+    %% Traverse the container hierarchy and extract the elements within
+    function traverseContainer(jcontainer,level,parent)
+        persistent figureComponentFound menuRootFound
+
+        % Record the data for this node
+        %disp([repmat(' ',1,level) '<= ' char(jcontainer.toString)])
+        thisIdx = length(levels) + 1;
+        levels(thisIdx) = level;
+        parentIdx(thisIdx) = parent;
+        try newHandle = handle(jcontainer,'callbackproperties'); catch, newHandle = handle(jcontainer); end
+        try handles(thisIdx) = newHandle; catch, handles = newHandle; end
+        try
+            positions(thisIdx,:) = getXY(jcontainer);
+            %sizes(thisIdx,:) = [jcontainer.getWidth, jcontainer.getHeight];
+        catch
+            positions(thisIdx,:) = [0,0];
+            %sizes(thisIdx,:) = [0,0];
+        end
+        if level>0
+            positions(thisIdx,:) = positions(thisIdx,:) + positions(parent,:);
+            if ~figureComponentFound && ...
+               strcmp(jcontainer.getName,'fComponentContainer') && ...
+               isa(jcontainer,'com.mathworks.hg.peer.FigureComponentContainer')   % there are 2 FigureComponentContainers - only process one...
+
+                % restart coordinate system, to exclude menu & toolbar areas
+                positions(thisIdx,:) = positions(thisIdx,:) - [jcontainer.getRootPane.getX, jcontainer.getRootPane.getY];
+                figureComponentFound = true;
+            end
+        elseif level==1
+            positions(thisIdx,:) = positions(thisIdx,:) + positions(parent,:);
+        else
+            % level 0 - initialize flags used later
+            figureComponentFound = false;
+            menuRootFound = false;
+        end
+        parentId = length(parentIdx);
+
+        % Traverse Menu items, unless the 'nomenu' option was requested
+        if ~nomenu
+            try
+                for child = 1 : getNumMenuComponents(jcontainer)
+                    traverseContainer(jcontainer.getMenuComponent(child-1),level+1,parentId);
+                end
+            catch
+                % Probably not a Menu container, but maybe a top-level JMenu, so discard duplicates
+                %if isa(handles(end).java,'javax.swing.JMenuBar')
+                if ~menuRootFound && strcmp(class(java(handles(end))),'javax.swing.JMenuBar')  %faster...
+                    if removeDuplicateNode(thisIdx)
+                        menuRootFound = true;
+                        return;
+                    end
+                end
+            end
+        end
+
+        % Now recursively process all this node's children (if any), except menu items if so requested
+        %if isa(jcontainer,'java.awt.Container')
+        try  % try-catch is faster than checking isa(jcontainer,'java.awt.Container')...
+            %if jcontainer.getComponentCount,  jcontainer.getComponents,  end
+            if ~nomenu || menuBarFoundFlag || isempty(strfind(class(jcontainer),'FigureMenuBar'))
+                lastChildComponent = java.lang.Object;
+                child = 0;
+                while (child < jcontainer.getComponentCount)
+                    childComponent = jcontainer.getComponent(child);
+                    % Looping over menus sometimes causes jcontainer to get mixed up (probably a JITC bug), so identify & fix
+                    if isequal(childComponent,lastChildComponent)
+                        child = child + 1;
+                        childComponent = jcontainer.getComponent(child);
+                    end
+                    lastChildComponent = childComponent;
+                    %disp([repmat(' ',1,level) '=> ' num2str(child) ': ' char(class(childComponent))])
+                    traverseContainer(childComponent,level+1,parentId);
+                    child = child + 1;
+                end
+            else
+                menuBarFoundFlag = true;  % use this flag to skip further testing for FigureMenuBar
+            end
+        catch
+            % do nothing - probably not a container
+            %dispError
+        end
+
+        % ...and yet another type of child traversal...
+        try
+            if ~isdeployed  % prevent 'File is empty' messages in compiled apps
+                jc = jcontainer.java;
+            else
+                jc = jcontainer;
+            end
+            for child = 1 : jc.getChildCount
+                traverseContainer(jc.getChildAt(child-1),level+1,parentId);
+            end
+        catch
+            % do nothing - probably not a container
+            %dispError
+        end
+
+        % TODO: Add axis (plot) component handles
+    end
+
+    %% Get the XY location of a Java component
+    function xy = getXY(jcontainer)
+            % Note: getX/getY are better than get(..,'X') (mem leaks),
+            % ^^^^  but sometimes they fail and revert to getx.m ...
+            % Note2: try awtinvoke() catch is faster than checking ismethod()...
+            % Note3: using AWTUtilities.invokeAndWait() directly is even faster than awtinvoke()...
+            try %if ismethod(jcontainer,'getX')
+                %positions(thisIdx,:) = [jcontainer.getX, jcontainer.getY];
+                cls = getClass(jcontainer);
+                location = com.mathworks.jmi.AWTUtilities.invokeAndWait(jcontainer,getMethod(cls,'getLocation',[]),[]);
+                x = location.getX;
+                y = location.getY;
+            catch %else
+                try
+                    x = com.mathworks.jmi.AWTUtilities.invokeAndWait(jcontainer,getMethod(cls,'getX',[]),[]);
+                    y = com.mathworks.jmi.AWTUtilities.invokeAndWait(jcontainer,getMethod(cls,'getY',[]),[]);
+                catch
+                    try
+                        x = awtinvoke(jcontainer,'getX()');
+                        y = awtinvoke(jcontainer,'getY()');
+                    catch
+                        x = get(jcontainer,'X');
+                        y = get(jcontainer,'Y');
+                    end
+                end
+            end
+            %positions(thisIdx,:) = [x, y];
+            xy = [x,y];
+    end
+
+    %% Get the number of menu sub-elements
+    function numMenuComponents  = getNumMenuComponents(jcontainer)
+
+        % The following line will raise an Exception for anything except menus
+        numMenuComponents = jcontainer.getMenuComponentCount;
+
+        % No error so far, so this must be a menu container...
+        % Note: Menu subitems are not visible until the top-level (root) menu gets initial focus...
+        % Try several alternatives, until we get a non-empty menu (or not...)
+        % TODO: Improve performance - this takes WAY too long...
+        if jcontainer.isTopLevelMenu && (numMenuComponents==0)
+            jcontainer.requestFocus;
+            numMenuComponents = jcontainer.getMenuComponentCount;
+            if (numMenuComponents == 0)
+                drawnow; pause(0.001);
+                numMenuComponents = jcontainer.getMenuComponentCount;
+                if (numMenuComponents == 0)
+                    jcontainer.setSelected(true);
+                    numMenuComponents = jcontainer.getMenuComponentCount;
+                    if (numMenuComponents == 0)
+                        drawnow; pause(0.001);
+                        numMenuComponents = jcontainer.getMenuComponentCount;
+                        if (numMenuComponents == 0)
+                            jcontainer.doClick;  % needed in order to populate the sub-menu components
+                            numMenuComponents = jcontainer.getMenuComponentCount;
+                            if (numMenuComponents == 0)
+                                drawnow; %pause(0.001);
+                                numMenuComponents = jcontainer.getMenuComponentCount;
+                                jcontainer.doClick;  % close menu by re-clicking...
+                                if (numMenuComponents == 0)
+                                    drawnow; %pause(0.001);
+                                    numMenuComponents = jcontainer.getMenuComponentCount;
+                                end
+                            else
+                                % ok - found sub-items
+                                % Note: no need to close menu since this will be done when focus moves to the FindJObj window
+                                %jcontainer.doClick;  % close menu by re-clicking...
+                            end
+                        end
+                    end
+                    jcontainer.setSelected(false);  % de-select the menu
+                end
+            end
+        end
+    end
+
+    %% Remove a specific tree node's data
+    function nodeRemovedFlag = removeDuplicateNode(thisIdx)
+        nodeRemovedFlag = false;
+        for idx = 1 : thisIdx-1
+            if isequal(handles(idx),handles(thisIdx))
+                levels(thisIdx) = [];
+                parentIdx(thisIdx) = [];
+                handles(thisIdx) = [];
+                positions(thisIdx,:) = [];
+                %sizes(thisIdx,:) = [];
+                nodeRemovedFlag = true;
+                return;
+            end
+        end
+    end
+
+    %% Process optional args
+    function processArgs(varargin)
+
+        % Initialize
+        invertFlag = false;
+        listing = '';
+
+        % Loop over all optional args
+        while ~isempty(varargin) && ~isempty(handles)
+
+            % Process the arg (and all its params)
+            foundIdx = 1 : length(handles);
+            if iscell(varargin{1}),  varargin{1} = varargin{1}{1};  end
+            if ~isempty(varargin{1}) && varargin{1}(1)=='-'
+                varargin{1}(1) = [];
+            end
+            switch lower(varargin{1})
+                case 'not'
+                    invertFlag = true;
+                case 'position'
+                    [varargin,foundIdx] = processPositionArgs(varargin{:});
+                    if invertFlag,  foundIdx = ~foundIdx;  invertFlag = false;  end
+                case 'size'
+                    [varargin,foundIdx] = processSizeArgs(varargin{:});
+                    if invertFlag,  foundIdx = ~foundIdx;  invertFlag = false;  end
+                case 'class'
+                    [varargin,foundIdx] = processClassArgs(varargin{:});
+                    if invertFlag,  foundIdx = ~foundIdx;  invertFlag = false;  end
+                case 'property'
+                    [varargin,foundIdx] = processPropertyArgs(varargin{:});
+                    if invertFlag,  foundIdx = ~foundIdx;  invertFlag = false;  end
+                case 'depth'
+                    [varargin,foundIdx] = processDepthArgs(varargin{:});
+                    if invertFlag,  foundIdx = ~foundIdx;  invertFlag = false;  end
+                case 'flat'
+                    varargin = {'depth',0, varargin{min(2:end):end}};
+                    [varargin,foundIdx] = processDepthArgs(varargin{:});
+                    if invertFlag,  foundIdx = ~foundIdx;  invertFlag = false;  end
+                case {'print','list'}
+                    [varargin,listing] = processPrintArgs(varargin{:});
+                case {'persist','nomenu','debug'}
+                    % ignore - already handled in main function above
+                otherwise
+                    error('YMA:findjobj:IllegalOption',['Option ' num2str(varargin{1}) ' is not a valid option. Type ''help ' mfilename ''' for the full options list.']);
+            end
+
+            % If only parent-child pairs found
+            foundIdx = find(foundIdx);
+            if ~isempty(foundIdx) && isequal(parentIdx(foundIdx(2:2:end)),foundIdx(1:2:end))
+                % Return just the children (the parent panels are uninteresting)
+                foundIdx(1:2:end) = [];
+            end
+            
+            % If several possible handles were found and the first is the container of the second
+            if length(foundIdx) == 2 && isequal(handles(foundIdx(1)).java, handles(foundIdx(2)).getParent)
+                % Discard the uninteresting component container
+                foundIdx(1) = [];
+            end
+
+            % Filter the results
+            selectedIdx = selectedIdx(foundIdx);
+            handles   = handles(foundIdx);
+            levels    = levels(foundIdx);
+            parentIdx = parentIdx(foundIdx);
+            positions = positions(foundIdx,:);
+
+            % Remove this arg and proceed to the next one
+            varargin(1) = [];
+
+        end  % Loop over all args
+    end
+
+    %% Process 'print' option
+    function [varargin,listing] = processPrintArgs(varargin)
+        if length(varargin)<2 || ischar(varargin{2})
+            % No second arg given, so use the first available element
+            listingContainer = handles(1);  %#ok - used in evalc below
+        else
+            % Get the element to print from the specified second arg
+            if isnumeric(varargin{2}) && (varargin{2} == fix(varargin{2}))  % isinteger doesn't work on doubles...
+                if (varargin{2} > 0) && (varargin{2} <= length(handles))
+                    listingContainer = handles(varargin{2});  %#ok - used in evalc below
+                elseif varargin{2} > 0
+                    error('YMA:findjobj:IllegalPrintFilter','Print filter index %g > number of available elements (%d)',varargin{2},length(handles));
+                else
+                    error('YMA:findjobj:IllegalPrintFilter','Print filter must be a java handle or positive numeric index into handles');
+                end
+            elseif ismethod(varargin{2},'list')
+                listingContainer = varargin{2};  %#ok - used in evalc below
+            else
+                error('YMA:findjobj:IllegalPrintFilter','Print filter must be a java handle or numeric index into handles');
+            end
+            varargin(2) = [];
+        end
+
+        % use evalc() to capture output into a Matlab variable
+        %listing = evalc('listingContainer.list');
+
+        % Better solution: loop over all handles and process them one by one
+        listing = cell(length(handles),1);
+        for componentIdx = 1 : length(handles)
+            listing{componentIdx} = [repmat(' ',1,levels(componentIdx)) char(handles(componentIdx).toString)];
+        end
+    end
+
+    %% Process 'position' option
+    function [varargin,foundIdx] = processPositionArgs(varargin)
+        if length(varargin)>1
+            positionFilter = varargin{2};
+            %if (isjava(positionFilter) || iscom(positionFilter)) && ismethod(positionFilter,'getLocation')
+            try  % try-catch is faster...
+                % Java/COM object passed - get its position
+                positionFilter = positionFilter.getLocation;
+                filterXY = [positionFilter.getX, positionFilter.getY];
+            catch
+                if ~isscalar(positionFilter)
+                    % position vector passed
+                    if (length(positionFilter)>=2) && isnumeric(positionFilter)
+                        % Remember that java coordinates start at top-left corner, Matlab coords start at bottom left...
+                        %positionFilter = java.awt.Point(positionFilter(1), container.getHeight - positionFilter(2));
+                        filterXY = [container.getX + positionFilter(1), container.getY + contentSize(2) - positionFilter(2)];
+
+                        % Check for full Matlab position vector (x,y,w,h)
+                        %if (length(positionFilter)==4)
+                        %    varargin{end+1} = 'size';
+                        %    varargin{end+1} = fix(positionFilter(3:4));
+                        %end
+                    else
+                        error('YMA:findjobj:IllegalPositionFilter','Position filter must be a java UI component, or X,Y pair');
+                    end
+                elseif length(varargin)>2
+                    % x,y passed as separate arg values
+                    if isnumeric(positionFilter) && isnumeric(varargin{3})
+                        % Remember that java coordinates start at top-left corner, Matlab coords start at bottom left...
+                        %positionFilter = java.awt.Point(positionFilter, container.getHeight - varargin{3});
+                        filterXY = [container.getX + positionFilter, container.getY + contentSize(2) - varargin{3}];
+                        varargin(3) = [];
+                    else
+                        error('YMA:findjobj:IllegalPositionFilter','Position filter must be a java UI component, or X,Y pair');
+                    end
+                else
+                    error('YMA:findjobj:IllegalPositionFilter','Position filter must be a java UI component, or X,Y pair');
+                end
+            end
+
+            % Compute the required element positions in order to be eligible for a more detailed examination
+            % Note: based on the following constraints: 0 <= abs(elementX-filterX) + abs(elementY+elementH-filterY) < 7
+            baseDeltas = [positions(:,1)-filterXY(1), positions(:,2)-filterXY(2)];  % faster than repmat()...
+            %baseHeight = - baseDeltas(:,2);% -abs(baseDeltas(:,1));
+            %minHeight = baseHeight - 7;
+            %maxHeight = baseHeight + 7;
+            %foundIdx = ~arrayfun(@(b)(invoke(b,'contains',positionFilter)),handles);  % ARGH! - disallowed by Matlab!
+            %foundIdx = repmat(false,1,length(handles));
+            %foundIdx(length(handles)) = false;  % faster than repmat()...
+            foundIdx = (abs(baseDeltas(:,1)) < 7) & (abs(baseDeltas(:,2)) < 7); % & (minHeight >= 0);
+            %fi = find(foundIdx);
+            %for componentIdx = 1 : length(fi)
+                %foundIdx(componentIdx) = handles(componentIdx).getBounds.contains(positionFilter);
+
+                % Search for a point no farther than 7 pixels away (prevents rounding errors)
+                %foundIdx(componentIdx) = handles(componentIdx).getLocationOnScreen.distanceSq(positionFilter) < 50;  % fails for invisible components...
+
+                %p = java.awt.Point(positions(componentIdx,1), positions(componentIdx,2) + handles(componentIdx).getHeight);
+                %foundIdx(componentIdx) = p.distanceSq(positionFilter) < 50;
+
+                %foundIdx(componentIdx) = sum(([baseDeltas(componentIdx,1),baseDeltas(componentIdx,2)+handles(componentIdx).getHeight]).^2) < 50;
+
+                % Following is the fastest method found to date: only eligible elements are checked in detailed
+            %    elementHeight = handles(fi(componentIdx)).getHeight;
+            %    foundIdx(fi(componentIdx)) = elementHeight > minHeight(fi(componentIdx)) && ...
+            %                                 elementHeight < maxHeight(fi(componentIdx));
+                %disp([componentIdx,elementHeight,minHeight(fi(componentIdx)),maxHeight(fi(componentIdx)),foundIdx(fi(componentIdx))])
+            %end
+
+            varargin(2) = [];
+        else
+            foundIdx = [];
+        end
+    end
+
+    %% Process 'size' option
+    function [varargin,foundIdx] = processSizeArgs(varargin)
+        if length(varargin)>1
+            sizeFilter = lower(varargin{2});
+            %if (isjava(sizeFilter) || iscom(sizeFilter)) && ismethod(sizeFilter,'getSize')
+            try  % try-catch is faster...
+                % Java/COM object passed - get its size
+                sizeFilter = sizeFilter.getSize;
+                filterWidth  = sizeFilter.getWidth;
+                filterHeight = sizeFilter.getHeight;
+            catch
+                if ~isscalar(sizeFilter)
+                    % size vector passed
+                    if (length(sizeFilter)>=2) && isnumeric(sizeFilter)
+                        %sizeFilter = java.awt.Dimension(sizeFilter(1),sizeFilter(2));
+                        filterWidth  = sizeFilter(1);
+                        filterHeight = sizeFilter(2);
+                    else
+                        error('YMA:findjobj:IllegalSizeFilter','Size filter must be a java UI component, or W,H pair');
+                    end
+                elseif length(varargin)>2
+                    % w,h passed as separate arg values
+                    if isnumeric(sizeFilter) && isnumeric(varargin{3})
+                        %sizeFilter = java.awt.Dimension(sizeFilter,varargin{3});
+                        filterWidth  = sizeFilter;
+                        filterHeight = varargin{3};
+                        varargin(3) = [];
+                    else
+                        error('YMA:findjobj:IllegalSizeFilter','Size filter must be a java UI component, or W,H pair');
+                    end
+                else
+                    error('YMA:findjobj:IllegalSizeFilter','Size filter must be a java UI component, or W,H pair');
+                end
+            end
+            %foundIdx = ~arrayfun(@(b)(invoke(b,'contains',sizeFilter)),handles);  % ARGH! - disallowed by Matlab!
+            foundIdx(length(handles)) = false;  % faster than repmat()...
+            for componentIdx = 1 : length(handles)
+                %foundIdx(componentIdx) = handles(componentIdx).getSize.equals(sizeFilter);
+                % Allow a 2-pixel tollerance to account for non-integer pixel sizes
+                foundIdx(componentIdx) = abs(handles(componentIdx).getWidth  - filterWidth)  <= 3 && ...  % faster than getSize.equals()
+                                         abs(handles(componentIdx).getHeight - filterHeight) <= 3;
+            end
+            varargin(2) = [];
+        else
+            foundIdx = [];
+        end
+    end
+
+    %% Process 'class' option
+    function [varargin,foundIdx] = processClassArgs(varargin)
+        if length(varargin)>1
+            classFilter = varargin{2};
+            %if ismethod(classFilter,'getClass')
+            try  % try-catch is faster...
+                classFilter = char(classFilter.getClass);
+            catch
+                if ~ischar(classFilter)
+                    error('YMA:findjobj:IllegalClassFilter','Class filter must be a java object, class or string');
+                end
+            end
+
+            % Now convert all java classes to java.lang.Strings and compare to the requested filter string
+            try
+                foundIdx(length(handles)) = false;  % faster than repmat()...
+                jClassFilter = java.lang.String(classFilter).toLowerCase;
+                for componentIdx = 1 : length(handles)
+                    % Note: JVM 1.5's String.contains() appears slightly slower and is available only since Matlab 7.2
+                    foundIdx(componentIdx) = handles(componentIdx).getClass.toString.toLowerCase.indexOf(jClassFilter) >= 0;
+                end
+            catch
+                % Simple processing: slower since it does extra processing within opaque.char()
+                for componentIdx = 1 : length(handles)
+                    % Note: using @toChar is faster but returns java String, not a Matlab char
+                    foundIdx(componentIdx) = ~isempty(regexpi(char(handles(componentIdx).getClass),classFilter));
+                end
+            end
+
+            varargin(2) = [];
+        else
+            foundIdx = [];
+        end
+    end
+
+    %% Process 'property' option
+    function [varargin,foundIdx] = processPropertyArgs(varargin)
+        if length(varargin)>1
+            propertyName = varargin{2};
+            if iscell(propertyName)
+                if length(propertyName) == 2
+                    propertyVal  = propertyName{2};
+                    propertyName = propertyName{1};
+                elseif length(propertyName) == 1
+                    propertyName = propertyName{1};
+                else
+                    error('YMA:findjobj:IllegalPropertyFilter','Property filter must be a string (case insensitive name of property) or cell array {propName,propValue}');
+                end
+            end
+            if ~ischar(propertyName)
+                error('YMA:findjobj:IllegalPropertyFilter','Property filter must be a string (case insensitive name of property) or cell array {propName,propValue}');
+            end
+            propertyName = lower(propertyName);
+            %foundIdx = arrayfun(@(h)isprop(h,propertyName),handles);  % ARGH! - disallowed by Matlab!
+            foundIdx(length(handles)) = false;  % faster than repmat()...
+
+            % Split processing depending on whether a specific property value was requested (ugly but faster...)
+            if exist('propertyVal','var')
+                for componentIdx = 1 : length(handles)
+                    try
+                        % Find out whether this element has the specified property
+                        % Note: findprop() and its return value schema.prop are undocumented and unsupported!
+                        prop = findprop(handles(componentIdx),propertyName);  % faster than isprop() & enables partial property names
+
+                        % If found, compare it to the actual element's property value
+                        foundIdx(componentIdx) = ~isempty(prop) && isequal(get(handles(componentIdx),prop.Name),propertyVal);
+                    catch
+                        % Some Java classes have a write-only property (like LabelPeer with 'Text'), so we end up here
+                        % In these cases, simply assume that the property value doesn't match and continue
+                        foundIdx(componentIdx) = false;
+                    end
+                end
+            else
+                for componentIdx = 1 : length(handles)
+                    try
+                        % Find out whether this element has the specified property
+                        % Note: findprop() and its return value schema.prop are undocumented and unsupported!
+                        foundIdx(componentIdx) = ~isempty(findprop(handles(componentIdx),propertyName));
+                    catch
+                        foundIdx(componentIdx) = false;
+                    end
+                end
+            end
+            varargin(2) = [];
+        else
+            foundIdx = [];
+        end
+    end
+
+    %% Process 'depth' option
+    function [varargin,foundIdx] = processDepthArgs(varargin)
+        if length(varargin)>1
+            level = varargin{2};
+            if ~isnumeric(level)
+                error('YMA:findjobj:IllegalDepthFilter','Depth filter must be a number (=maximal element depth)');
+            end
+            foundIdx = (levels <= level);
+            varargin(2) = [];
+        else
+            foundIdx = [];
+        end
+    end
+
+    %% Convert property data into a string
+    function data = charizeData(data)
+        if isa(data,'com.mathworks.hg.types.HGCallback')
+            data = get(data,'Callback');
+        end
+        if ~ischar(data) && ~isa(data,'java.lang.String')
+            newData = strtrim(evalc('disp(data)'));
+            try
+                newData = regexprep(newData,'  +',' ');
+                newData = regexprep(newData,'Columns \d+ through \d+\s','');
+                newData = regexprep(newData,'Column \d+\s','');
+            catch
+                %never mind...
+            end
+            if iscell(data)
+                newData = ['{ ' newData ' }'];
+            elseif isempty(data)
+                newData = '';
+            elseif isnumeric(data) || islogical(data) || any(ishandle(data)) || numel(data) > 1 %&& ~isscalar(data)
+                newData = ['[' newData ']'];
+            end
+            data = newData;
+        elseif ~isempty(data)
+            data = ['''' data ''''];
+        end
+    end  % charizeData
+
+    %% Prepare a hierarchical callbacks table data
+    function setProp(list,name,value,category)
+        prop = eval('com.jidesoft.grid.DefaultProperty();');  % prevent JIDE alert by run-time (not load-time) evaluation
+        prop.setName(name);
+        prop.setType(java.lang.String('').getClass);
+        prop.setValue(value);
+        prop.setEditable(true);
+        prop.setExpert(true);
+        %prop.setCategory(['<html><b><u><font color="blue">' category ' callbacks']);
+        prop.setCategory([category ' callbacks']);
+        list.add(prop);
+    end  % getTreeData
+
+    %% Prepare a hierarchical callbacks table data
+    function list = getTreeData(data)
+        list = java.util.ArrayList();
+        names = regexprep(data,'([A-Z][a-z]+).*','$1');
+        %hash = java.util.Hashtable;
+        others = {};
+        for propIdx = 1 : size(data,1)
+            if (propIdx < size(data,1) && strcmp(names{propIdx},names{propIdx+1})) || ...
+               (propIdx > 1            && strcmp(names{propIdx},names{propIdx-1}))
+                % Child callback property
+                setProp(list,data{propIdx,1},data{propIdx,2},names{propIdx});
+            else
+                % Singular callback property => Add to 'Other' category at bottom of the list
+                others(end+1,:) = data(propIdx,:);  %#ok
+            end
+        end
+        for propIdx = 1 : size(others,1)
+            setProp(list,others{propIdx,1},others{propIdx,2},'Other');
+        end
+    end  % getTreeData
+
+    %% Get callbacks table data
+    function [cbData, cbHeaders, cbTableEnabled] = getCbsData(obj, stripStdCbsFlag)
+        % Initialize
+        cbData = {'(no callbacks)'};
+        cbHeaders = {'Callback name'};
+        cbTableEnabled = false;
+        cbNames = {};
+
+        try
+            try
+                classHdl = classhandle(handle(obj));
+                cbNames = get(classHdl.Events,'Name');
+                if ~isempty(cbNames) && ~iscom(obj)  %only java-based please...
+                    cbNames = strcat(cbNames,'Callback');
+                end
+                propNames = get(classHdl.Properties,'Name');
+            catch
+                % Try to interpret as an MCOS class object
+                try
+                    oldWarn = warning('off','MATLAB:structOnObject');
+                    dataFields = struct(obj);
+                    warning(oldWarn);
+                catch
+                    dataFields = get(obj);
+                end
+                propNames = fieldnames(dataFields);
+            end
+            propCbIdx = [];
+            if ischar(propNames),  propNames={propNames};  end  % handle case of a single callback
+            if ~isempty(propNames)
+                propCbIdx = find(~cellfun(@isempty,regexp(propNames,'(Fcn|Callback)$')));
+                cbNames = unique([cbNames; propNames(propCbIdx)]);  %#ok logical is faster but less debuggable...
+            end
+            if ~isempty(cbNames)
+                if stripStdCbsFlag
+                    cbNames = stripStdCbs(cbNames);
+                end
+                if iscell(cbNames)
+                    cbNames = sort(cbNames);
+                    if size(cbNames,1) < size(cbNames,2)
+                        cbNames = cbNames';
+                    end
+                end
+                hgHandleFlag = 0;  try hgHandleFlag = ishghandle(obj); catch, end  %#ok
+                try
+                    obj = handle(obj,'CallbackProperties');
+                catch
+                    hgHandleFlag = 1;
+                end
+                if hgHandleFlag
+                    % HG handles don't allow CallbackProperties - search only for *Fcn
+                    %cbNames = propNames(propCbIdx);
+                end
+                if iscom(obj)
+                    cbs = obj.eventlisteners;
+                    if ~isempty(cbs)
+                        cbNamesRegistered = cbs(:,1);
+                        cbData = setdiff(cbNames,cbNamesRegistered);
+                        %cbData = charizeData(cbData);
+                        if size(cbData,2) > size(cbData(1))
+                            cbData = cbData';
+                        end
+                        cbData = [cbData, cellstr(repmat(' ',length(cbData),1))];
+                        cbData = [cbData; cbs];
+                        [sortedNames, sortedIdx] = sort(cbData(:,1));
+                        sortedCbs = cellfun(@charizeData,cbData(sortedIdx,2),'un',0);
+                        cbData = [sortedNames, sortedCbs];
+                    else
+                        cbData = [cbNames, cellstr(repmat(' ',length(cbNames),1))];
+                    end
+                elseif iscell(cbNames)
+                    cbNames = sort(cbNames);
+                    %cbData = [cbNames, get(obj,cbNames)'];
+                    cbData = cbNames;
+                    oldWarn = warning('off','MATLAB:hg:JavaSetHGProperty');
+                    warning('off','MATLAB:hg:PossibleDeprecatedJavaSetHGProperty');
+                    for idx = 1 : length(cbNames)
+                        try
+                            cbData{idx,2} = charizeData(get(obj,cbNames{idx}));
+                        catch
+                            cbData{idx,2} = '(callback value inaccessible)';
+                        end
+                    end
+                    warning(oldWarn);
+                else  % only one event callback
+                    %cbData = {cbNames, get(obj,cbNames)'};
+                    %cbData{1,2} = charizeData(cbData{1,2});
+                    try
+                        cbData = {cbNames, charizeData(get(obj,cbNames))};
+                    catch
+                        cbData = {cbNames, '(callback value inaccessible)'};
+                    end
+                end
+                cbHeaders = {'Callback name','Callback value'};
+                cbTableEnabled = true;
+            end
+        catch
+            % never mind - use default (empty) data
+        end
+    end  % getCbsData
+
+    %% Get relative (0.0-1.0) divider location
+    function divLocation = getRalativeDivlocation(jDiv)
+        divLocation = jDiv.getDividerLocation;
+        if divLocation > 1  % i.e. [pixels]
+            visibleRect = jDiv.getVisibleRect;
+            if jDiv.getOrientation == 0  % vertical
+                start = visibleRect.getY;
+                extent = visibleRect.getHeight - start;
+            else
+                start = visibleRect.getX;
+                extent = visibleRect.getWidth - start;
+            end
+            divLocation = (divLocation - start) / extent;
+        end
+    end  % getRalativeDivlocation
+
+    %% Try to set a treenode icon based on a container's icon
+    function setTreeNodeIcon(treenode,container)
+        try
+            iconImage = [];
+            iconImage = container.getIcon;
+            if ~isempty(findprop(handle(iconImage),'Image'))  % get(iconImage,'Image') is easier but leaks memory...
+                iconImage = iconImage.getImage;
+            else
+                a=b; %#ok cause an error
+            end
+        catch
+            try
+                iconImage = container.getIconImage;
+            catch
+                try
+                    if ~isempty(iconImage)
+                        ge = java.awt.GraphicsEnvironment.getLocalGraphicsEnvironment;
+                        gd = ge.getDefaultScreenDevice;
+                        gc = gd.getDefaultConfiguration;
+                        image = gc.createCompatibleImage(iconImage.getIconWidth, iconImage.getIconHeight);  % a BufferedImage object
+                        g = image.createGraphics;
+                        iconImage.paintIcon([], g, 0, 0);
+                        g.dispose;
+                        iconImage = image;
+                    end
+                catch
+                    % never mind...
+                end
+            end
+        end
+        if ~isempty(iconImage)
+            iconImage = setIconSize(iconImage);
+            treenode.setIcon(iconImage);
+        end
+    end  % setTreeNodeIcon
+
+    %% Present the object hierarchy tree
+    function presentObjectTree()
+        persistent lastVersionCheck
+        if isempty(lastVersionCheck),  lastVersionCheck = now-1;  end
+
+        import java.awt.*
+        import javax.swing.*
+        hTreeFig = findall(0,'tag','findjobjFig');
+        iconpath = [matlabroot, '/toolbox/matlab/icons/'];
+        cbHideStd = 0;  % Initial state of the cbHideStdCbs checkbox
+        if isempty(hTreeFig)
+            % Prepare the figure
+            hTreeFig = figure('tag','findjobjFig','menuBar','none','toolBar','none','Name','FindJObj','NumberTitle','off','handleVisibility','off','IntegerHandle','off');
+            figIcon = ImageIcon([iconpath 'tool_legend.gif']);
+            drawnow;
+            try
+                mde = com.mathworks.mde.desk.MLDesktop.getInstance;
+                jTreeFig = mde.getClient('FindJObj').getTopLevelAncestor;
+                jTreeFig.setIcon(figIcon);
+            catch
+                warning('off','MATLAB:HandleGraphics:ObsoletedProperty:JavaFrame');  % R2008b compatibility
+                jTreeFig = get(hTreeFig,'JavaFrame');
+                jTreeFig.setFigureIcon(figIcon);
+            end
+            vsplitPaneLocation = 0.8;
+            hsplitPaneLocation = 0.5;
+        else
+            % Remember cbHideStdCbs checkbox & dividers state for later
+            userdata = get(hTreeFig, 'userdata');
+            try cbHideStd = userdata.cbHideStdCbs.isSelected; catch, end  %#ok
+            try
+                vsplitPaneLocation = getRalativeDivlocation(userdata.vsplitPane);
+                hsplitPaneLocation = getRalativeDivlocation(userdata.hsplitPane);
+            catch
+                vsplitPaneLocation = 0.8;
+                hsplitPaneLocation = 0.5;
+            end
+
+            % Clear the figure and redraw
+            clf(hTreeFig);
+            figure(hTreeFig);   % bring to front
+        end
+
+        % Traverse all HG children, if root container was a HG handle
+        if ishghandle(origContainer) %&& ~isequal(origContainer,container)
+            traverseHGContainer(origContainer,0,0);
+        end
+
+        % Prepare the tree pane
+        warning('off','MATLAB:uitreenode:MigratingFunction');  % R2008b compatibility
+        warning('off','MATLAB:uitreenode:DeprecatedFunction'); % R2008a compatibility
+        tree_h = com.mathworks.hg.peer.UITreePeer;
+        try tree_h = javaObjectEDT(tree_h); catch, end
+        tree_hh = handle(tree_h,'CallbackProperties');
+        hasChildren = sum(allParents==1) > 1;
+        icon = [iconpath 'upfolder.gif'];
+        [rootName, rootTitle] = getNodeName(container);
+        try
+            root = uitreenode('v0', handle(container), rootName, icon, ~hasChildren);
+        catch  % old matlab version don't have the 'v0' option
+            root = uitreenode(handle(container), rootName, icon, ~hasChildren);
+        end
+        setTreeNodeIcon(root,container);  % constructor must accept a char icon unfortunately, so need to do this afterwards...
+        if ~isempty(rootTitle)
+            set(hTreeFig, 'Name',['FindJObj - ' char(rootTitle)]);
+        end
+        nodedata.idx = 1;
+        nodedata.obj = container;
+        set(root,'userdata',nodedata);
+        root.setUserObject(container);
+        setappdata(root,'childHandle',container);
+        tree_h.setRoot(root);
+        treePane = tree_h.getScrollPane;
+        treePane.setMinimumSize(Dimension(50,50));
+        jTreeObj = treePane.getViewport.getComponent(0);
+        jTreeObj.setShowsRootHandles(true)
+        jTreeObj.getSelectionModel.setSelectionMode(javax.swing.tree.TreeSelectionModel.DISCONTIGUOUS_TREE_SELECTION);
+        %jTreeObj.setVisible(0);
+        %jTreeObj.getCellRenderer.setLeafIcon([]);
+        %jTreeObj.getCellRenderer.setOpenIcon(figIcon);
+        %jTreeObj.getCellRenderer.setClosedIcon([]);
+        treePanel = JPanel(BorderLayout);
+        treePanel.add(treePane, BorderLayout.CENTER);
+        progressBar = JProgressBar(0);
+        progressBar.setMaximum(length(allHandles) + length(hg_handles));  % = # of all nodes
+        treePanel.add(progressBar, BorderLayout.SOUTH);
+
+        % Prepare the image pane
+%disable for now, until we get it working...
+%{
+        try
+            hFig = ancestor(origContainer,'figure');
+            [cdata, cm] = getframe(hFig);  %#ok cm unused
+            tempfname = [tempname '.png'];
+            imwrite(cdata,tempfname);  % don't know how to pass directly to BufferedImage, so use disk...
+            jImg = javax.imageio.ImageIO.read(java.io.File(tempfname));
+            try delete(tempfname);  catch  end
+            imgPanel = JPanel();
+            leftPanel = JSplitPane(JSplitPane.VERTICAL_SPLIT, treePanel, imgPanel);
+            leftPanel.setOneTouchExpandable(true);
+            leftPanel.setContinuousLayout(true);
+            leftPanel.setResizeWeight(0.8);
+        catch
+            leftPanel = treePanel;
+        end
+%}
+        leftPanel = treePanel;
+
+        % Prepare the inspector pane
+        classNameLabel = JLabel(['      ' char(class(container))]);
+        classNameLabel.setForeground(Color.blue);
+        updateNodeTooltip(container, classNameLabel);
+        inspectorPanel = JPanel(BorderLayout);
+        inspectorPanel.add(classNameLabel, BorderLayout.NORTH);
+        % TODO: Maybe uncomment the following when we add the HG tree - in the meantime it's unused (java properties are un-groupable)
+        %objReg = com.mathworks.services.ObjectRegistry.getLayoutRegistry;
+        %toolBar = awtinvoke('com.mathworks.mlwidgets.inspector.PropertyView$ToolBarStyle','valueOf(Ljava.lang.String;)','GROUPTOOLBAR');
+        %inspectorPane = com.mathworks.mlwidgets.inspector.PropertyView(objReg, toolBar);
+        inspectorPane = com.mathworks.mlwidgets.inspector.PropertyView;
+        identifiers = disableDbstopError;  %#ok "dbstop if error" causes inspect.m to croak due to a bug - so workaround
+        inspectorPane.setObject(container);
+        inspectorPane.setAutoUpdate(true);
+        % TODO: Add property listeners
+        % TODO: Display additional props
+        inspectorTable = inspectorPane;
+        try
+            while ~isa(inspectorTable,'javax.swing.JTable')
+                inspectorTable = inspectorTable.getComponent(0);
+            end
+        catch
+            % R2010a
+            inspectorTable = inspectorPane.getComponent(0).getScrollPane.getViewport.getComponent(0);
+        end
+        toolTipText = 'hover mouse over the red classname above to see the full list of properties';
+        inspectorTable.setToolTipText(toolTipText);
+        jideTableUtils = [];
+        try
+            % Try JIDE features - see http://www.jidesoft.com/products/JIDE_Grids_Developer_Guide.pdf
+            com.mathworks.mwswing.MJUtilities.initJIDE;
+            jideTableUtils = eval('com.jidesoft.grid.TableUtils;');  % prevent JIDE alert by run-time (not load-time) evaluation
+            jideTableUtils.autoResizeAllColumns(inspectorTable);
+            inspectorTable.setRowAutoResizes(true);
+            inspectorTable.getModel.setShowExpert(1);
+        catch
+            % JIDE is probably unavailable - never mind...
+        end
+        inspectorPanel.add(inspectorPane, BorderLayout.CENTER);
+        % TODO: Add data update listeners
+
+        % Prepare the callbacks pane
+        callbacksPanel = JPanel(BorderLayout);
+        stripStdCbsFlag = true;  % initial value
+        [cbData, cbHeaders, cbTableEnabled] = getCbsData(container, stripStdCbsFlag);
+        %{
+        %classHdl = classhandle(handle(container));
+        %eventNames = get(classHdl.Events,'Name');
+        %if ~isempty(eventNames)
+        %    cbNames = sort(strcat(eventNames,'Callback'));
+        %    try
+        %        cbData = [cbNames, get(container,cbNames)'];
+        %    catch
+        %        % R2010a
+        %        cbData = cbNames;
+        %        if isempty(cbData)
+        %            cbData = {};
+        %        elseif ~iscell(cbData)
+        %            cbData = {cbData};
+        %        end
+        %        for idx = 1 : length(cbNames)
+        %            cbData{idx,2} = get(container,cbNames{idx});
+        %        end
+        %    end
+        %    cbTableEnabled = true;
+        %else
+        %    cbData = {'(no callbacks)',''};
+        %    cbTableEnabled = false;
+        %end
+        %cbHeaders = {'Callback name','Callback value'};
+        %}
+        try
+            % Use JideTable if available on this system
+            %callbacksTableModel = javax.swing.table.DefaultTableModel(cbData,cbHeaders);  %#ok
+            %callbacksTable = eval('com.jidesoft.grid.PropertyTable(callbacksTableModel);');  % prevent JIDE alert by run-time (not load-time) evaluation
+            try
+                list = getTreeData(cbData);  %#ok
+                model = eval('com.jidesoft.grid.PropertyTableModel(list);');  %#ok prevent JIDE alert by run-time (not load-time) evaluation
+
+                % Auto-expand if only one category
+                if model.getRowCount==1   % length(model.getCategories)==1 fails for some unknown reason...
+                    model.expandFirstLevel;
+                end
+
+                %callbacksTable = eval('com.jidesoft.grid.TreeTable(model);');  %#ok prevent JIDE alert by run-time (not load-time) evaluation
+                callbacksTable = eval('com.jidesoft.grid.PropertyTable(model);');  %#ok prevent JIDE alert by run-time (not load-time) evaluation
+
+                %callbacksTable.expandFirstLevel;
+                callbacksTable.setShowsRootHandles(true);
+                callbacksTable.setShowTreeLines(false);
+                %callbacksTable.setShowNonEditable(0);  %=SHOW_NONEDITABLE_NEITHER
+                callbacksPane = eval('com.jidesoft.grid.PropertyPane(callbacksTable);');  % prevent JIDE alert by run-time (not load-time) evaluation
+                callbacksPane.setShowDescription(false);
+            catch
+                callbacksTable = eval('com.jidesoft.grid.TreeTable(cbData,cbHeaders);');  % prevent JIDE alert by run-time (not load-time) evaluation
+            end
+            callbacksTable.setRowAutoResizes(true);
+            callbacksTable.setColumnAutoResizable(true);
+            callbacksTable.setColumnResizable(true);
+            jideTableUtils.autoResizeAllColumns(callbacksTable);
+            callbacksTable.setTableHeader([]);  % hide the column headers since now we can resize columns with the gridline
+            callbacksLabel = JLabel(' Callbacks:');  % The column headers are replaced with a header label
+            callbacksLabel.setForeground(Color.blue);
+            %callbacksPanel.add(callbacksLabel, BorderLayout.NORTH);
+
+            % Add checkbox to show/hide standard callbacks
+            callbacksTopPanel = JPanel;
+            callbacksTopPanel.setLayout(BoxLayout(callbacksTopPanel, BoxLayout.LINE_AXIS));
+            callbacksTopPanel.add(callbacksLabel);
+            callbacksTopPanel.add(Box.createHorizontalGlue);
+            jcb = JCheckBox('Hide standard callbacks', cbHideStd);
+            set(handle(jcb,'CallbackProperties'), 'ActionPerformedCallback',{@cbHideStdCbs_Callback,callbacksTable});
+            try
+                set(jcb, 'userdata',callbacksTable, 'tooltip','Hide standard Swing callbacks - only component-specific callbacks will be displayed');
+            catch
+                jcb.setToolTipText('Hide standard Swing callbacks - only component-specific callbacks will be displayed');
+                %setappdata(jcb,'userdata',callbacksTable);
+            end
+            callbacksTopPanel.add(jcb);
+            callbacksPanel.add(callbacksTopPanel, BorderLayout.NORTH);
+        catch
+            % Otherwise, use a standard Swing JTable (keep the headers to enable resizing)
+            callbacksTable = JTable(cbData,cbHeaders);
+        end
+        cbToolTipText = 'Callbacks may be ''strings'', @funcHandle or {@funcHandle,arg1,...}';
+        callbacksTable.setToolTipText(cbToolTipText);
+        callbacksTable.setGridColor(inspectorTable.getGridColor);
+        cbNameTextField = JTextField;
+        cbNameTextField.setEditable(false);  % ensure that the callback names are not modified...
+        cbNameCellEditor = DefaultCellEditor(cbNameTextField);
+        cbNameCellEditor.setClickCountToStart(intmax);  % i.e, never enter edit mode...
+        callbacksTable.getColumnModel.getColumn(0).setCellEditor(cbNameCellEditor);
+        if ~cbTableEnabled
+            try callbacksTable.getColumnModel.getColumn(1).setCellEditor(cbNameCellEditor); catch, end
+        end
+        hModel = callbacksTable.getModel;
+        set(handle(hModel,'CallbackProperties'), 'TableChangedCallback',{@tbCallbacksChanged,container,callbacksTable});
+        %set(hModel, 'UserData',container);
+        try
+            cbScrollPane = callbacksPane; %JScrollPane(callbacksPane);
+            %cbScrollPane.setHorizontalScrollBarPolicy(cbScrollPane.HORIZONTAL_SCROLLBAR_NEVER);
+        catch
+            cbScrollPane = JScrollPane(callbacksTable);
+            cbScrollPane.setVerticalScrollBarPolicy(cbScrollPane.VERTICAL_SCROLLBAR_AS_NEEDED);
+        end
+        callbacksPanel.add(cbScrollPane, BorderLayout.CENTER);
+        callbacksPanel.setToolTipText(cbToolTipText);
+
+        % Prepare the top-bottom JSplitPanes
+        vsplitPane = JSplitPane(JSplitPane.VERTICAL_SPLIT, inspectorPanel, callbacksPanel);
+        vsplitPane.setOneTouchExpandable(true);
+        vsplitPane.setContinuousLayout(true);
+        vsplitPane.setResizeWeight(0.8);
+
+        % Prepare the left-right JSplitPane
+        hsplitPane = JSplitPane(JSplitPane.HORIZONTAL_SPLIT, leftPanel, vsplitPane);
+        hsplitPane.setOneTouchExpandable(true);
+        hsplitPane.setContinuousLayout(true);
+        hsplitPane.setResizeWeight(0.6);
+        pos = getpixelposition(hTreeFig);
+
+        % Prepare the bottom pane with all buttons
+        lowerPanel = JPanel(FlowLayout);
+        blogUrlLabel = '<a href="http://UndocumentedMatlab.com">Undocumented<br>Matlab.com</a>';
+        jWebsite = createJButton(blogUrlLabel, @btWebsite_Callback, 'Visit the UndocumentedMatlab.com blog');
+        jWebsite.setContentAreaFilled(0);
+        lowerPanel.add(jWebsite);
+        lowerPanel.add(createJButton('Refresh<br>tree',        {@btRefresh_Callback, origContainer, hTreeFig}, 'Rescan the component tree, from the root down'));
+        lowerPanel.add(createJButton('Export to<br>workspace', {@btExport_Callback,  jTreeObj, classNameLabel}, 'Export the selected component handles to workspace variable findjobj_hdls'));
+        lowerPanel.add(createJButton('Request<br>focus',       {@btFocus_Callback,   jTreeObj, root}, 'Set the focus on the first selected component'));
+        lowerPanel.add(createJButton('Inspect<br>object',      {@btInspect_Callback, jTreeObj, root}, 'View the signature of all methods supported by the first selected component'));
+        lowerPanel.add(createJButton('Check for<br>updates',   {@btCheckFex_Callback}, 'Check the MathWorks FileExchange for the latest version of FindJObj'));
+
+        % Display everything on-screen
+        globalPanel = JPanel(BorderLayout);
+        globalPanel.add(hsplitPane, BorderLayout.CENTER);
+        globalPanel.add(lowerPanel, BorderLayout.SOUTH);
+        [obj, hcontainer] = javacomponent(globalPanel, [0,0,pos(3:4)], hTreeFig);
+        set(hcontainer,'units','normalized');
+        drawnow;
+        hsplitPane.setDividerLocation(hsplitPaneLocation);  % this only works after the JSplitPane is displayed...
+        vsplitPane.setDividerLocation(vsplitPaneLocation);  % this only works after the JSplitPane is displayed...
+        %restoreDbstopError(identifiers);
+
+        % Refresh & resize the screenshot thumbnail
+%disable for now, until we get it working...
+%{
+        try
+            hAx = axes('Parent',hTreeFig, 'units','pixels', 'position',[10,10,250,150], 'visible','off');
+            axis(hAx,'image');
+            image(cdata,'Parent',hAx);
+            axis(hAx,'off');
+            set(hAx,'UserData',cdata);
+            set(imgPanel, 'ComponentResizedCallback',{@resizeImg, hAx}, 'UserData',lowerPanel);
+            imgPanel.getGraphics.drawImage(jImg, 0, 0, []);
+        catch
+            % Never mind...
+        end
+%}
+        % If all handles were selected (i.e., none were filtered) then only select the first
+        if (length(selectedIdx) == length(allHandles)) && ~isempty(selectedIdx)
+            selectedIdx = 1;
+        end
+
+        % Store handles for callback use
+        userdata.handles = allHandles;
+        userdata.levels  = allLevels;
+        userdata.parents = allParents;
+        userdata.hg_handles = hg_handles;
+        userdata.hg_levels  = hg_levels;
+        userdata.hg_parents = hg_parentIdx;
+        userdata.initialIdx = selectedIdx;
+        userdata.userSelected = false;  % Indicates the user has modified the initial selections
+        userdata.inInit = true;
+        userdata.jTree = jTreeObj;
+        userdata.jTreePeer = tree_h;
+        userdata.vsplitPane = vsplitPane;
+        userdata.hsplitPane = hsplitPane;
+        userdata.classNameLabel = classNameLabel;
+        userdata.inspectorPane = inspectorPane;
+        userdata.callbacksTable = callbacksTable;
+        userdata.jideTableUtils = jideTableUtils;
+        try
+            userdata.cbHideStdCbs = jcb;
+        catch
+            userdata.cbHideStdCbs = [];
+        end
+
+        % Update userdata for use in callbacks
+        try
+            set(tree_h,'userdata',userdata);
+        catch
+            setappdata(handle(tree_h),'userdata',userdata);
+        end
+        try
+            set(callbacksTable,'userdata',userdata);
+        catch
+            setappdata(callbacksTable,'userdata',userdata);
+        end
+        set(hTreeFig,'userdata',userdata);
+
+        % Select the root node if requested
+        % Note: we must do so here since all other nodes except the root are processed by expandNode
+        if any(selectedIdx==1)
+            tree_h.setSelectedNode(root);
+        end
+
+        % Set the initial cbHideStdCbs state
+        try
+            if jcb.isSelected
+                drawnow;
+                evd.getSource.isSelected = jcb.isSelected;
+                cbHideStdCbs_Callback(jcb,evd,callbacksTable);
+            end
+        catch
+            % never mind...
+        end
+
+        % Set the callback functions
+        set(tree_hh, 'NodeExpandedCallback', {@nodeExpanded, tree_h});
+        set(tree_hh, 'NodeSelectedCallback', {@nodeSelected, tree_h});
+
+        % Set the tree mouse-click callback
+        % Note: default actions (expand/collapse) will still be performed?
+        % Note: MousePressedCallback is better than MouseClickedCallback
+        %       since it fires immediately when mouse button is pressed,
+        %       without waiting for its release, as MouseClickedCallback does
+        handleTree = tree_h.getScrollPane;
+        jTreeObj = handleTree.getViewport.getComponent(0);
+        jTreeObjh = handle(jTreeObj,'CallbackProperties');
+        set(jTreeObjh, 'MousePressedCallback', {@treeMousePressedCallback,tree_h});  % context (right-click) menu
+        set(jTreeObjh, 'MouseMovedCallback',   @treeMouseMovedCallback);    % mouse hover tooltips
+
+        % Update userdata
+        userdata.inInit = false;
+        try
+            set(tree_h,'userdata',userdata);
+        catch
+            setappdata(handle(tree_h),'userdata',userdata);
+        end
+        set(hTreeFig,'userdata',userdata);
+
+        % Pre-expand all rows
+        %treePane.setVisible(false);
+        expandNode(progressBar, jTreeObj, tree_h, root, 0);
+        %treePane.setVisible(true);
+        %jTreeObj.setVisible(1);
+
+        % Hide the progressbar now that we've finished expanding all rows
+        try
+            hsplitPane.getLeftComponent.setTopComponent(treePane);
+        catch
+            % Probably not a vSplitPane on the left...
+            hsplitPane.setLeftComponent(treePane);
+        end
+        hsplitPane.setDividerLocation(hsplitPaneLocation);  % need to do it again...
+
+        % Set keyboard focus on the tree
+        jTreeObj.requestFocus;
+        drawnow;
+
+        % Check for a newer version (only in non-deployed mode, and only twice a day)
+        if ~isdeployed && now-lastVersionCheck > 0.5
+            checkVersion();
+            lastVersionCheck = now;
+        end
+
+        % Reset the last error
+        lasterr('');  %#ok
+    end
+
+    %% Rresize image pane
+    function resizeImg(varargin)  %#ok - unused (TODO: waiting for img placement fix...)
+        try
+            hPanel = varargin{1};
+            hAx    = varargin{3};
+            lowerPanel = get(hPanel,'UserData');
+            newJPos = cell2mat(get(hPanel,{'X','Y','Width','Height'}));
+            newMPos = [1,get(lowerPanel,'Height'),newJPos(3:4)];
+            set(hAx, 'units','pixels', 'position',newMPos, 'Visible','on');
+            uistack(hAx,'top');  % no good...
+            set(hPanel,'Opaque','off');  % also no good...
+        catch
+            % Never mind...
+            dispError
+        end
+        return;
+    end
+
+    %% "dbstop if error" causes inspect.m to croak due to a bug - so workaround by temporarily disabling this dbstop
+    function identifiers = disableDbstopError
+        dbStat = dbstatus;
+        idx = find(strcmp({dbStat.cond},'error'));
+        identifiers = [dbStat(idx).identifier];
+        if ~isempty(idx)
+            dbclear if error;
+            msgbox('''dbstop if error'' had to be disabled due to a Matlab bug that would have caused Matlab to crash.', 'FindJObj', 'warn');
+        end
+    end
+
+    %% Restore any previous "dbstop if error"
+    function restoreDbstopError(identifiers)  %#ok
+        for itemIdx = 1 : length(identifiers)
+            eval(['dbstop if error ' identifiers{itemIdx}]);
+        end
+    end
+
+    %% Recursively expand all nodes (except toolbar/menubar) in startup
+    function expandNode(progressBar, tree, tree_h, parentNode, parentRow)
+        try
+            if nargin < 5
+                parentPath = javax.swing.tree.TreePath(parentNode.getPath);
+                parentRow = tree.getRowForPath(parentPath);
+            end
+            tree.expandRow(parentRow);
+            progressBar.setValue(progressBar.getValue+1);
+            numChildren = parentNode.getChildCount;
+            if (numChildren == 0)
+                pause(0.0002);  % as short as possible...
+                drawnow;
+            end
+            nodesToUnExpand = {'FigureMenuBar','MLMenuBar','MJToolBar','Box','uimenu','uitoolbar','ScrollBar'};
+            numChildren = parentNode.getChildCount;
+            for childIdx = 0 : numChildren-1
+                childNode = parentNode.getChildAt(childIdx);
+
+                % Pre-select the node based upon the user's FINDJOBJ filters
+                try
+                    nodedata = get(childNode, 'userdata');
+                    try
+                        userdata = get(tree_h, 'userdata');
+                    catch
+                        userdata = getappdata(handle(tree_h), 'userdata');
+                    end
+                    %fprintf('%d - %s\n',nodedata.idx,char(nodedata.obj))
+                    if ~ishghandle(nodedata.obj) && ~userdata.userSelected && any(userdata.initialIdx == nodedata.idx)
+                        pause(0.0002);  % as short as possible...
+                        drawnow;
+                        if isempty(tree_h.getSelectedNodes)
+                            tree_h.setSelectedNode(childNode);
+                        else
+                            newSelectedNodes = [tree_h.getSelectedNodes, childNode];
+                            tree_h.setSelectedNodes(newSelectedNodes);
+                        end
+                    end
+                catch
+                    % never mind...
+                    dispError
+                end
+
+                % Expand child node if not leaf & not toolbar/menubar
+                if childNode.isLeafNode
+
+                    % This is a leaf node, so simply update the progress-bar
+                    progressBar.setValue(progressBar.getValue+1);
+
+                else
+                    % Expand all non-leaves
+                    expandNode(progressBar, tree, tree_h, childNode);
+
+                    % Re-collapse toolbar/menubar etc., and also invisible containers
+                    % Note: if we simply did nothing, progressbar would not have been updated...
+                    try
+                        childHandle = getappdata(childNode,'childHandle');  %=childNode.getUserObject
+                        visible = childHandle.isVisible;
+                    catch
+                        visible = 1;
+                    end
+                    visible = visible && isempty(findstr(get(childNode,'Name'),'color="gray"'));
+                    %if any(strcmp(childNode.getName,nodesToUnExpand))
+                    %name = char(childNode.getName);
+                    if any(cellfun(@(s)~isempty(strmatch(s,char(childNode.getName))),nodesToUnExpand)) || ~visible
+                        childPath = javax.swing.tree.TreePath(childNode.getPath);
+                        childRow = tree.getRowForPath(childPath);
+                        tree.collapseRow(childRow);
+                    end
+                end
+            end
+        catch
+            % never mind...
+            dispError
+        end
+    end
+
+    %% Create utility buttons
+    function hButton = createJButton(nameStr, handler, toolTipText)
+        try
+            jButton = javax.swing.JButton(['<html><body><center>' nameStr]);
+            jButton.setCursor(java.awt.Cursor.getPredefinedCursor(java.awt.Cursor.HAND_CURSOR));
+            jButton.setToolTipText(toolTipText);
+            try
+                minSize = jButton.getMinimumSize;
+            catch
+                minSize = jButton.getMinimumSize;  % for HG2 - strange indeed that this is needed!
+            end
+            jButton.setMinimumSize(java.awt.Dimension(minSize.getWidth,35));
+            hButton = handle(jButton,'CallbackProperties');
+            set(hButton,'ActionPerformedCallback',handler);
+        catch
+            % Never mind...
+            a= 1;
+        end
+    end
+
+    %% Flash a component off/on for the specified duration
+    % note: starts with 'on'; if numTimes is odd then ends with 'on', otherwise with 'off'
+    function flashComponent(jComps,delaySecs,numTimes)
+        persistent redBorder redBorderPanels
+        try
+            % Handle callback data from right-click (context-menu)
+            if iscell(numTimes)
+                [jComps,delaySecs,numTimes] = deal(numTimes{:});
+            end
+
+            if isempty(redBorder)  % reuse if possible
+                redBorder = javax.swing.border.LineBorder(java.awt.Color.red,2,0);
+            end
+            for compIdx = 1 : length(jComps)
+                try
+                    oldBorder{compIdx} = jComps(compIdx).getBorder;  %#ok grow
+                catch
+                    oldBorder{compIdx} = [];  %#ok grow
+                end
+                isSettable(compIdx) = ismethod(jComps(compIdx),'setBorder');  %#ok grow
+                if isSettable(compIdx)
+                    try
+                        % some components prevent border modification:
+                        oldBorderFlag = jComps(compIdx).isBorderPainted;
+                        if ~oldBorderFlag
+                            jComps(compIdx).setBorderPainted(1);
+                            isSettable(compIdx) = jComps(compIdx).isBorderPainted;  %#ok grow
+                            jComps(compIdx).setBorderPainted(oldBorderFlag);
+                        end
+                    catch
+                        % do nothing...
+                    end
+                end
+                if compIdx > length(redBorderPanels)
+                    redBorderPanels{compIdx} = javax.swing.JPanel;
+                    redBorderPanels{compIdx}.setBorder(redBorder);
+                    redBorderPanels{compIdx}.setOpaque(0);  % transparent interior, red border
+                end
+                try
+                    redBorderPanels{compIdx}.setBounds(jComps(compIdx).getBounds);
+                catch
+                    % never mind - might be an HG handle
+                end
+            end
+            for idx = 1 : 2*numTimes
+                if idx>1,  pause(delaySecs);  end  % don't pause at start
+                visible = mod(idx,2);
+                for compIdx = 1 : length(jComps)
+                    try
+                        jComp = jComps(compIdx);
+
+                        % Prevent Matlab crash (java buffer overflow...)
+                        if isa(jComp,'com.mathworks.mwswing.desk.DTSplitPane') || ...
+                           isa(jComp,'com.mathworks.mwswing.MJSplitPane')
+                            continue;
+
+                        % HG handles are highlighted by setting their 'Selected' property
+                        elseif isa(jComp,'uimenu') || isa(jComp,'matlab.ui.container.Menu')
+                            if visible
+                                oldColor = get(jComp,'ForegroundColor');
+                                setappdata(jComp,'findjobj_oldColor',oldColor);
+                                set(jComp,'ForegroundColor','red');
+                            else
+                                oldColor = getappdata(jComp,'findjobj_oldColor');
+                                set(jComp,'ForegroundColor',oldColor);
+                                rmappdata(jComp,'ForegroundColor');
+                            end
+
+                        elseif ishghandle(jComp)
+                            if visible
+                                set(jComp,'Selected','on');
+                            else
+                                set(jComp,'Selected','off');
+                            end
+
+                        else %if isjava(jComp)
+
+                            jParent = jComps(compIdx).getParent;
+
+                            % Most Java components allow modifying their borders
+                            if isSettable(compIdx)
+                                if visible
+                                    jComp.setBorder(redBorder);
+                                    try jComp.setBorderPainted(1); catch, end  %#ok
+                                else %if ~isempty(oldBorder{compIdx})
+                                    jComp.setBorder(oldBorder{compIdx});
+                                end
+                                jComp.repaint;
+
+                            % The other Java components are highlighted by a transparent red-border
+                            % panel that is placed on top of them in their parent's space
+                            elseif ~isempty(jParent)
+                                if visible
+                                    jParent.add(redBorderPanels{compIdx});
+                                    jParent.setComponentZOrder(redBorderPanels{compIdx},0);
+                                else
+                                    jParent.remove(redBorderPanels{compIdx});
+                                end
+                                jParent.repaint
+                            end
+                        end
+                    catch
+                        % never mind - try the next component (if any)
+                    end
+                end
+                drawnow;
+            end
+        catch
+            % never mind...
+            dispError;
+        end
+        return;  % debug point
+    end  % flashComponent
+
+    %% Select tree node
+    function nodeSelected(src, evd, tree)  %#ok
+        try
+            if iscell(tree)
+                [src,node] = deal(tree{:});
+            else
+                node = evd.getCurrentNode;
+            end
+            %nodeHandle = node.getUserObject;
+            nodedata = get(node,'userdata');
+            nodeHandle = nodedata.obj;
+            try
+                userdata = get(src,'userdata');
+            catch
+                try
+                    userdata = getappdata(java(src),'userdata');
+                catch
+                    userdata = getappdata(src,'userdata');
+                end
+                if isempty(userdata)
+                    try userdata = get(java(src),'userdata'); catch, end
+                end
+            end
+            if ~isempty(nodeHandle) && ~isempty(userdata)
+                numSelections  = userdata.jTree.getSelectionCount;
+                selectionPaths = userdata.jTree.getSelectionPaths;
+                if (numSelections == 1)
+                    % Indicate that the user has modified the initial selection (except if this was an initial auto-selected node)
+                    if ~userdata.inInit
+                        userdata.userSelected = true;
+                        try
+                            set(src,'userdata',userdata);
+                        catch
+                            try
+                                setappdata(java(src),'userdata',userdata);
+                            catch
+                                setappdata(src,'userdata',userdata);
+                            end
+                        end
+                    end
+
+                    % Update the fully-qualified class name label
+                    numInitialIdx = length(userdata.initialIdx);
+                    thisHandle = nodeHandle;
+                    try
+                        if ~ishghandle(thisHandle)
+                            thisHandle = java(nodeHandle);
+                        end
+                    catch
+                        % never mind...
+                    end
+                    if ~userdata.inInit || (numInitialIdx == 1)
+                        userdata.classNameLabel.setText(['      ' char(class(thisHandle))]);
+                    else
+                        userdata.classNameLabel.setText([' ' num2str(numInitialIdx) 'x handles (some handles hidden by unexpanded tree nodes)']);
+                    end
+                    if ishghandle(thisHandle)
+                        userdata.classNameLabel.setText(userdata.classNameLabel.getText.concat(' (HG handle)'));
+                    end
+                    userdata.inspectorPane.dispose;  % remove props listeners - doesn't work...
+                    updateNodeTooltip(nodeHandle, userdata.classNameLabel);
+
+                    % Update the data properties inspector pane
+                    % Note: we can't simply use the evd nodeHandle, because this node might have been DE-selected with only one other node left selected...
+                    %nodeHandle = selectionPaths(1).getLastPathComponent.getUserObject;
+                    nodedata = get(selectionPaths(1).getLastPathComponent,'userdata');
+                    nodeHandle = nodedata.obj;
+                    %identifiers = disableDbstopError;  % "dbstop if error" causes inspect.m to croak due to a bug - so workaround
+                    userdata.inspectorPane.setObject(thisHandle);
+
+                    % Update the callbacks table
+                    try
+                        stripStdCbsFlag = getappdata(userdata.callbacksTable,'hideStdCbs');
+                        [cbData, cbHeaders, cbTableEnabled] = getCbsData(nodeHandle, stripStdCbsFlag);  %#ok cbTableEnabled unused
+                        try
+                            % Use JideTable if available on this system
+                            list = getTreeData(cbData);  %#ok
+                            callbacksTableModel = eval('com.jidesoft.grid.PropertyTableModel(list);');  %#ok prevent JIDE alert by run-time (not load-time) evaluation
+                            
+                            % Expand if only one category
+                            if length(callbacksTableModel.getCategories)==1
+                                callbacksTableModel.expandFirstLevel;
+                            end
+                        catch
+                            callbacksTableModel = javax.swing.table.DefaultTableModel(cbData,cbHeaders);
+                        end
+                        set(handle(callbacksTableModel,'CallbackProperties'), 'TableChangedCallback',{@tbCallbacksChanged,nodeHandle,userdata.callbacksTable});
+                        %set(callbacksTableModel, 'UserData',nodeHandle);
+                        userdata.callbacksTable.setModel(callbacksTableModel)
+                        userdata.callbacksTable.setRowAutoResizes(true);
+                        userdata.jideTableUtils.autoResizeAllColumns(userdata.callbacksTable);
+                    catch
+                        % never mind...
+                        %dispError
+                    end
+                    pause(0.005);
+                    drawnow;
+                    %restoreDbstopError(identifiers);
+
+                    % Highlight the selected object (if visible)
+                    flashComponent(nodeHandle,0.2,3);
+
+                elseif (numSelections > 1)  % Multiple selections
+
+                    % Get the list of all selected nodes
+                    jArray = javaArray('java.lang.Object', numSelections);
+                    toolTipStr = '<html>';
+                    sameClassFlag = true;
+                    for idx = 1 : numSelections
+                        %jArray(idx) = selectionPaths(idx).getLastPathComponent.getUserObject;
+                        nodedata = get(selectionPaths(idx).getLastPathComponent,'userdata');
+                        try
+                            if ishghandle(nodedata.obj)
+                                if idx==1
+                                    jArray = nodedata.obj;
+                                else
+                                    jArray(idx) = nodedata.obj;
+                                end
+                            else
+                                jArray(idx) = java(nodedata.obj);
+                            end
+                        catch
+                            jArray(idx) = nodedata.obj;
+                        end
+                        toolTipStr = [toolTipStr '&nbsp;' class(jArray(idx)) '&nbsp;'];  %#ok grow
+                        if (idx < numSelections),  toolTipStr = [toolTipStr '<br>'];  end  %#ok grow
+                        try
+                            if (idx > 1) && sameClassFlag && ~isequal(jArray(idx).getClass,jArray(1).getClass)
+                                sameClassFlag = false;
+                            end
+                        catch
+                            if (idx > 1) && sameClassFlag && ~isequal(class(jArray(idx)),class(jArray(1)))
+                                sameClassFlag = false;
+                            end
+                        end
+                    end
+                    toolTipStr = [toolTipStr '</html>'];
+
+                    % Update the fully-qualified class name label
+                    if sameClassFlag
+                        classNameStr = class(jArray(1));
+                    else
+                        classNameStr = 'handle';
+                    end
+                    if all(ishghandle(jArray))
+                        if strcmp(classNameStr,'handle')
+                            classNameStr = 'HG handles';
+                        else
+                            classNameStr = [classNameStr ' (HG handles)'];
+                        end
+                    end
+                    classNameStr = [' ' num2str(numSelections) 'x ' classNameStr];
+                    userdata.classNameLabel.setText(classNameStr);
+                    userdata.classNameLabel.setToolTipText(toolTipStr);
+
+                    % Update the data properties inspector pane
+                    %identifiers = disableDbstopError;  % "dbstop if error" causes inspect.m to croak due to a bug - so workaround
+                    if isjava(jArray)
+                        jjArray = jArray;
+                    else
+                        jjArray = javaArray('java.lang.Object', numSelections);
+                        for idx = 1 : numSelections
+                            jjArray(idx) = java(jArray(idx));
+                        end
+                    end
+                    userdata.inspectorPane.getRegistry.setSelected(jjArray, true);
+
+                    % Update the callbacks table
+                    try
+                        % Get intersecting callback names & values
+                        stripStdCbsFlag = getappdata(userdata.callbacksTable,'hideStdCbs');
+                        [cbData, cbHeaders, cbTableEnabled] = getCbsData(jArray(1), stripStdCbsFlag);  %#ok cbHeaders & cbTableEnabled unused
+                        if ~isempty(cbData)
+                            cbNames = cbData(:,1);
+                            for idx = 2 : length(jArray)
+                                [cbData2, cbHeaders2] = getCbsData(jArray(idx), stripStdCbsFlag);  %#ok cbHeaders2 unused
+                                if ~isempty(cbData2)
+                                    newCbNames = cbData2(:,1);
+                                    [cbNames, cbIdx, cb2Idx] = intersect(cbNames,newCbNames);  %#ok cb2Idx unused
+                                    cbData = cbData(cbIdx,:);
+                                    for cbIdx = 1 : length(cbNames)
+                                        newIdx = find(strcmp(cbNames{cbIdx},newCbNames));
+                                        if ~isequal(cbData2,cbData) && ~isequal(cbData2{newIdx,2}, cbData{cbIdx,2})
+                                            cbData{cbIdx,2} = '<different values>';
+                                        end
+                                    end
+                                else
+                                    cbData = cbData([],:);  %=empty cell array
+                                end
+                                if isempty(cbData)
+                                    break;
+                                end
+                            end
+                        end
+                        cbHeaders = {'Callback name','Callback value'};
+                        try
+                            % Use JideTable if available on this system
+                            list = getTreeData(cbData);  %#ok
+                            callbacksTableModel = eval('com.jidesoft.grid.PropertyTableModel(list);');  %#ok prevent JIDE alert by run-time (not load-time) evaluation
+                            
+                            % Expand if only one category
+                            if length(callbacksTableModel.getCategories)==1
+                                callbacksTableModel.expandFirstLevel;
+                            end
+                        catch
+                            callbacksTableModel = javax.swing.table.DefaultTableModel(cbData,cbHeaders);
+                        end
+                        set(handle(callbacksTableModel,'CallbackProperties'), 'TableChangedCallback',{@tbCallbacksChanged,jArray,userdata.callbacksTable});
+                        %set(callbacksTableModel, 'UserData',jArray);
+                        userdata.callbacksTable.setModel(callbacksTableModel)
+                        userdata.callbacksTable.setRowAutoResizes(true);
+                        userdata.jideTableUtils.autoResizeAllColumns(userdata.callbacksTable);
+                    catch
+                        % never mind...
+                        dispError
+                    end
+
+                    pause(0.005);
+                    drawnow;
+                    %restoreDbstopError(identifiers);
+
+                    % Highlight the selected objects (if visible)
+                    flashComponent(jArray,0.2,3);
+                end
+
+                % TODO: Auto-highlight selected object (?)
+                %nodeHandle.requestFocus;
+            end
+        catch
+            dispError
+        end
+    end
+
+    %% IFF utility function for annonymous cellfun funcs
+    function result = iff(test,trueVal,falseVal)  %#ok
+        try
+            if test
+                result = trueVal;
+            else
+                result = falseVal;
+            end
+        catch
+            result = false;
+        end
+    end
+
+    %% Get an HTML representation of the object's properties
+    function dataFieldsStr = getPropsHtml(nodeHandle, dataFields)
+        try
+            % Get a text representation of the fieldnames & values
+            undefinedStr = '';
+            hiddenStr = '';
+            dataFieldsStr = '';  % just in case the following croaks...
+            if isempty(dataFields)
+                return;
+            end
+            dataFieldsStr = evalc('disp(dataFields)');
+            if dataFieldsStr(end)==char(10),  dataFieldsStr=dataFieldsStr(1:end-1);  end
+
+            % Strip out callbacks
+            dataFieldsStr = regexprep(dataFieldsStr,'^\s*\w*Callback(Data)?:[^\n]*$','','lineanchors');
+
+            % Strip out internal HG2 mirror properties
+            dataFieldsStr = regexprep(dataFieldsStr,'^\s*\w*_I:[^\n]*$','','lineanchors');
+            dataFieldsStr = regexprep(dataFieldsStr,'\n\n+','\n');
+
+            % Sort the fieldnames
+            %fieldNames = fieldnames(dataFields);
+            try
+                [a,b,c,d] = regexp(dataFieldsStr,'(\w*): ');
+                fieldNames = strrep(d,': ','');
+            catch
+                fieldNames = fieldnames(dataFields);
+            end
+            try
+                [fieldNames, sortedIdx] = sort(fieldNames);
+                s = strsplit(dataFieldsStr, sprintf('\n'))';
+                dataFieldsStr = strjoin(s(sortedIdx), sprintf('\n'));
+            catch
+                % never mind... - ignore, leave unsorted
+            end
+
+            % Convert into a Matlab handle()
+            %nodeHandle = handle(nodeHandle);
+            try
+                % ensure this is a Matlab handle, not a java object
+                nodeHandle = handle(nodeHandle, 'CallbackProperties');
+            catch
+                try
+                    % HG handles don't allow CallbackProperties...
+                    nodeHandle = handle(nodeHandle);
+                catch
+                    % Some Matlab class objects simply cannot be converted into a handle()
+                end
+            end
+
+            % HTMLize tooltip data
+            % First, set the fields' font based on its read-write status
+            for fieldIdx = 1 : length(fieldNames)
+                thisFieldName = fieldNames{fieldIdx};
+                %accessFlags = get(findprop(nodeHandle,thisFieldName),'AccessFlags');
+                try
+                    hProp = findprop(nodeHandle,thisFieldName);
+                    accessFlags = get(hProp,'AccessFlags');
+                    visible = get(hProp,'Visible');
+                catch
+                    accessFlags = [];
+                    visible = 'on';
+                    try if hProp.Hidden, visible='off'; end, catch, end
+                end
+                %if isfield(accessFlags,'PublicSet') && strcmp(accessFlags.PublicSet,'on')
+                if (~isempty(hProp) && isprop(hProp,'SetAccess') && isequal(hProp.SetAccess,'public')) || ...  % isequal(...'public') and not strcmpi(...) because might be a cell array of classes
+                   (~isempty(accessFlags) && isfield(accessFlags,'PublicSet') && strcmpi(accessFlags.PublicSet,'on'))
+                    % Bolden read/write fields
+                    thisFieldFormat = ['<b>' thisFieldName '</b>:$2'];
+                %elseif ~isfield(accessFlags,'PublicSet')
+                elseif (isempty(hProp) || ~isprop(hProp,'SetAccess')) && ...
+                       (isempty(accessFlags) || ~isfield(accessFlags,'PublicSet'))
+                    % Undefined - probably a Matlab-defined field of com.mathworks.hg.peer.FigureFrameProxy...
+                    thisFieldFormat = ['<font color="blue">' thisFieldName '</font>:$2'];
+                    undefinedStr = ', <font color="blue">undefined</font>';
+                else % PublicSet=='off'
+                    % Gray-out & italicize any read-only fields
+                    thisFieldFormat = ['<font color="#C0C0C0">' thisFieldName '</font>:<font color="#C0C0C0">$2</font>'];
+                end
+                if strcmpi(visible,'off')
+                    %thisFieldFormat = ['<i>' thisFieldFormat '</i>']; %#ok<AGROW>
+                    thisFieldFormat = regexprep(thisFieldFormat, {'(.*):(.*)','<.?b>'}, {'<i>$1:<i>$2',''}); %'(.*):(.*)', '<i>$1:<i>$2');
+                    hiddenStr = ', <i>hidden</i>';
+                end
+                dataFieldsStr = regexprep(dataFieldsStr, ['([\s\n])' thisFieldName ':([^\n]*)'], ['$1' thisFieldFormat]);
+            end
+        catch
+            % never mind... - probably an ambiguous property name
+            %dispError
+        end
+
+        % Method 1: simple <br> list
+        %dataFieldsStr = strrep(dataFieldsStr,char(10),'&nbsp;<br>&nbsp;&nbsp;');
+
+        % Method 2: 2-column <table>
+        dataFieldsStr = regexprep(dataFieldsStr, '^\s*([^:]+:)([^\n]*)\n^\s*([^:]+:)([^\n]*)$', '<tr><td>&nbsp;$1</td><td>&nbsp;$2</td><td>&nbsp;&nbsp;&nbsp;&nbsp;$3</td><td>&nbsp;$4&nbsp;</td></tr>', 'lineanchors');
+        dataFieldsStr = regexprep(dataFieldsStr, '^[^<]\s*([^:]+:)([^\n]*)$', '<tr><td>&nbsp;$1</td><td>&nbsp;$2</td><td>&nbsp;</td><td>&nbsp;</td></tr>', 'lineanchors');
+        dataFieldsStr = ['(<b>documented</b>' undefinedStr hiddenStr ' &amp; <font color="#C0C0C0">read-only</font> fields)<p>&nbsp;&nbsp;<table cellpadding="0" cellspacing="0">' dataFieldsStr '</table>'];
+    end
+
+    %% Update tooltip string with a node's data
+    function updateNodeTooltip(nodeHandle, uiObject)
+        try
+            toolTipStr = class(nodeHandle);
+            dataFieldsStr = '';
+
+            % Add HG annotation if relevant
+            if ishghandle(nodeHandle)
+                hgStr = ' HG Handle';
+            else
+                hgStr = '';
+            end
+
+            % Prevent HG-Java warnings
+            oldWarn = warning('off','MATLAB:hg:JavaSetHGProperty');
+            warning('off','MATLAB:hg:PossibleDeprecatedJavaSetHGProperty');
+            warning('off','MATLAB:hg:Root');
+
+            % Note: don't bulk-get because (1) not all properties are returned & (2) some properties cause a Java exception
+            % Note2: the classhandle approach does not enable access to user-defined schema.props
+            ch = classhandle(handle(nodeHandle));
+            dataFields = [];
+            [sortedNames, sortedIdx] = sort(get(ch.Properties,'Name'));
+            for idx = 1 : length(sortedIdx)
+                sp = ch.Properties(sortedIdx(idx));
+                % TODO: some fields (see EOL comment below) generate a Java Exception from: com.mathworks.mlwidgets.inspector.PropertyRootNode$PropertyListener$1$1.run
+                if strcmp(sp.AccessFlags.PublicGet,'on') % && ~any(strcmp(sp.Name,{'FixedColors','ListboxTop','Extent'}))
+                    try
+                        dataFields.(sp.Name) = get(nodeHandle, sp.Name);
+                    catch
+                        dataFields.(sp.Name) = '<font color="red">Error!</font>';
+                    end
+                else
+                    dataFields.(sp.Name) = '(no public getter method)';
+                end
+            end
+            dataFieldsStr = getPropsHtml(nodeHandle, dataFields);
+        catch
+            % Probably a non-HG java object
+            try
+                % Note: the bulk-get approach enables access to user-defined schema-props, but not to some original classhandle Properties...
+                try
+                    oldWarn3 = warning('off','MATLAB:structOnObject');
+                    dataFields = struct(nodeHandle);
+                    warning(oldWarn3);
+                catch
+                    dataFields = get(nodeHandle);
+                end
+                dataFieldsStr = getPropsHtml(nodeHandle, dataFields);
+            catch
+                % Probably a missing property getter implementation
+                try
+                    % Inform the user - bail out on error
+                    err = lasterror;  %#ok
+                    dataFieldsStr = ['<p>' strrep(err.message, char(10), '<br>')];
+                catch
+                    % forget it...
+                end
+            end
+        end
+
+        % Restore warnings
+        try warning(oldWarn); catch, end
+
+        % Set the object tooltip
+        if ~isempty(dataFieldsStr)
+            toolTipStr = ['<html>&nbsp;<b><font color="blue">' char(toolTipStr) '</font></b>' hgStr ':&nbsp;' dataFieldsStr '</html>'];
+        end
+        uiObject.setToolTipText(toolTipStr);
+    end
+
+    %% Expand tree node
+    function nodeExpanded(src, evd, tree)  %#ok
+        % tree = handle(src);
+        % evdsrc = evd.getSource;
+        evdnode = evd.getCurrentNode;
+
+        if ~tree.isLoaded(evdnode)
+
+            % Get the list of children TreeNodes
+            childnodes = getChildrenNodes(tree, evdnode);
+
+            % Add the HG sub-tree (unless already included in the first tree)
+            childHandle = getappdata(evdnode.handle,'childHandle');  %=evdnode.getUserObject
+            if evdnode.isRoot && ~isempty(hg_handles) && ~isequal(hg_handles(1).java, childHandle)
+                childnodes = [childnodes, getChildrenNodes(tree, evdnode, true)];
+            end
+
+            % If we have a single child handle, wrap it within a javaArray for tree.add() to "swallow"
+            if (length(childnodes) == 1)
+                chnodes = childnodes;
+                childnodes = javaArray('com.mathworks.hg.peer.UITreeNode', 1);
+                childnodes(1) = java(chnodes);
+            end
+
+            % Add child nodes to the current node
+            tree.add(evdnode, childnodes);
+            tree.setLoaded(evdnode, true);
+        end
+    end
+
+    %% Get an icon image no larger than 16x16 pixels
+    function iconImage = setIconSize(iconImage)
+        try
+            iconWidth  = iconImage.getWidth;
+            iconHeight = iconImage.getHeight;
+            if iconWidth > 16
+                newHeight = fix(iconHeight * 16 / iconWidth);
+                iconImage = iconImage.getScaledInstance(16,newHeight,iconImage.SCALE_SMOOTH);
+            elseif iconHeight > 16
+                newWidth = fix(iconWidth * 16 / iconHeight);
+                iconImage = iconImage.getScaledInstance(newWidth,16,iconImage.SCALE_SMOOTH);
+            end
+        catch
+            % never mind... - return original icon
+        end
+    end  % setIconSize
+
+    %% Get list of children nodes
+    function nodes = getChildrenNodes(tree, parentNode, isRootHGNode)
+        try
+            iconpath = [matlabroot, '/toolbox/matlab/icons/'];
+            nodes = handle([]);
+            try
+                userdata = get(tree,'userdata');
+            catch
+                userdata = getappdata(handle(tree),'userdata');
+            end
+            hdls = userdata.handles;
+            nodedata = get(parentNode,'userdata');
+            if nargin < 3
+                %isJavaNode = ~ishghandle(parentNode.getUserObject);
+                isJavaNode = ~ishghandle(nodedata.obj);
+                isRootHGNode = false;
+            else
+                isJavaNode = ~isRootHGNode;
+            end
+
+            % Search for this parent node in the list of all nodes
+            parents = userdata.parents;
+            nodeIdx = nodedata.idx;
+
+            if isJavaNode && isempty(nodeIdx)  % Failback, in case userdata doesn't work for some reason...
+                for hIdx = 1 : length(hdls)
+                    %if isequal(handle(parentNode.getUserObject), hdls(hIdx))
+                    if isequal(handle(nodedata.obj), hdls(hIdx))
+                        nodeIdx = hIdx;
+                        break;
+                    end
+                end
+            end
+            if ~isJavaNode
+                if isRootHGNode  % =root HG node
+                    thisChildHandle = userdata.hg_handles(1);
+                    childName = getNodeName(thisChildHandle);
+                    hasGrandChildren = any(parents==1);
+                    icon = [];
+                    if hasGrandChildren && length(hg_handles)>1
+                        childName = childName.concat(' - HG root container');
+                        icon = [iconpath 'figureicon.gif'];
+                    end
+                    try
+                        nodes = uitreenode('v0', thisChildHandle, childName, icon, ~hasGrandChildren);
+                    catch  % old matlab version don't have the 'v0' option
+                        try
+                            nodes = uitreenode(thisChildHandle, childName, icon, ~hasGrandChildren);
+                        catch
+                            % probably an invalid handle - ignore...
+                        end
+                    end
+
+                    % Add the handler to the node's internal data
+                    % Note: could also use 'userdata', but setUserObject() is recommended for TreeNodes
+                    % Note2: however, setUserObject() sets a java *BeenAdapter object for HG handles instead of the required original class, so use setappdata
+                    %nodes.setUserObject(thisChildHandle);
+                    setappdata(nodes,'childHandle',thisChildHandle);
+                    nodedata.idx = 1;
+                    nodedata.obj = thisChildHandle;
+                    set(nodes,'userdata',nodedata);
+                    return;
+                else  % non-root HG node
+                    parents = userdata.hg_parents;
+                    hdls    = userdata.hg_handles;
+                end  % if isRootHGNode
+            end  % if ~isJavaNode
+
+            % If this node was found, get the list of its children
+            if ~isempty(nodeIdx)
+                %childIdx = setdiff(find(parents==nodeIdx),nodeIdx);
+                childIdx = find(parents==nodeIdx);
+                childIdx(childIdx==nodeIdx) = [];  % faster...
+                numChildren = length(childIdx);
+                for cIdx = 1 : numChildren
+                    thisChildIdx = childIdx(cIdx);
+                    try thisChildHandle = hdls(thisChildIdx); catch, continue, end
+                    childName = getNodeName(thisChildHandle);
+                    try
+                        visible = thisChildHandle.Visible;
+                        if visible
+                            try visible = thisChildHandle.Width > 0; catch, end  %#ok
+                        end
+                        if ~visible
+                            childName = ['<HTML><i><font color="gray">' char(childName) '</font></i></html>'];  %#ok grow
+                        end
+                    catch
+                        % never mind...
+                    end
+                    hasGrandChildren = any(parents==thisChildIdx);
+                    try
+                        isaLabel = isa(thisChildHandle.java,'javax.swing.JLabel');
+                    catch
+                        isaLabel = 0;
+                    end
+                    if hasGrandChildren && ~any(strcmp(class(thisChildHandle),{'axes'}))
+                        icon = [iconpath 'foldericon.gif'];
+                    elseif isaLabel
+                        icon = [iconpath 'tool_text.gif'];
+                    else
+                        icon = [];
+                    end
+                    try
+                        nodes(cIdx) = uitreenode('v0', thisChildHandle, childName, icon, ~hasGrandChildren);
+                    catch  % old matlab version don't have the 'v0' option
+                        try
+                            nodes(cIdx) = uitreenode(thisChildHandle, childName, icon, ~hasGrandChildren);
+                        catch
+                            % probably an invalid handle - ignore...
+                        end
+                    end
+
+                    % Use existing object icon, if available
+                    try
+                        setTreeNodeIcon(nodes(cIdx),thisChildHandle);
+                    catch
+                        % probably an invalid handle - ignore...
+                    end
+
+                    % Pre-select the node based upon the user's FINDJOBJ filters
+                    try
+                        if isJavaNode && ~userdata.userSelected && any(userdata.initialIdx == thisChildIdx)
+                            pause(0.0002);  % as short as possible...
+                            drawnow;
+                            if isempty(tree.getSelectedNodes)
+                                tree.setSelectedNode(nodes(cIdx));
+                            else
+                                newSelectedNodes = [tree.getSelectedNodes, nodes(cIdx).java];
+                                tree.setSelectedNodes(newSelectedNodes);
+                            end
+                        end
+                    catch
+                        % never mind...
+                    end
+
+                    % Add the handler to the node's internal data
+                    % Note: could also use 'userdata', but setUserObject() is recommended for TreeNodes
+                    % Note2: however, setUserObject() sets a java *BeenAdapter object for HG handles instead of the required original class, so use setappdata
+                    % Note3: the following will error if invalid handle - ignore
+                    try
+                        if isJavaNode
+                            thisChildHandle = thisChildHandle.java;
+                        end
+                        %nodes(cIdx).setUserObject(thisChildHandle);
+                        setappdata(nodes(cIdx),'childHandle',thisChildHandle);
+                        nodedata.idx = thisChildIdx;
+                        nodedata.obj = thisChildHandle;
+                        set(nodes(cIdx),'userdata',nodedata);
+                    catch
+                        % never mind (probably an invalid handle) - leave unchanged (like a leaf)
+                    end
+                end
+            end
+        catch
+            % Never mind - leave unchanged (like a leaf)
+            %error('YMA:findjobj:UnknownNodeType', 'Error expanding component tree node');
+            dispError
+        end
+    end
+
+    %% Get a node's name
+    function [nodeName, nodeTitle] = getNodeName(hndl,charsLimit)
+        try
+            % Initialize (just in case one of the succeding lines croaks)
+            nodeName = '';
+            nodeTitle = '';
+            if ~ismethod(hndl,'getClass')
+                try
+                    nodeName = class(hndl);
+                catch
+                    nodeName = hndl.type;  % last-ditch try...
+                end
+            else
+                nodeName = hndl.getClass.getSimpleName;
+            end
+
+            % Strip away the package name, leaving only the regular classname
+            if ~isempty(nodeName) && ischar(nodeName)
+                nodeName = java.lang.String(nodeName);
+                nodeName = nodeName.substring(nodeName.lastIndexOf('.')+1);
+            end
+            if (nodeName.length == 0)
+                % fix case of anonymous internal classes, that do not have SimpleNames
+                try
+                    nodeName = hndl.getClass.getName;
+                    nodeName = nodeName.substring(nodeName.lastIndexOf('.')+1);
+                catch
+                    % never mind - leave unchanged...
+                end
+            end
+
+            % Get any unique identifying string (if available in one of several fields)
+            labelsToCheck = {'label','title','text','string','displayname','toolTipText','TooltipString','actionCommand','name','Tag','style'}; %,'UIClassID'};
+            nodeTitle = '';
+            strField = '';  %#ok - used for debugging
+            while ((~isa(nodeTitle,'java.lang.String') && ~ischar(nodeTitle)) || isempty(nodeTitle)) && ~isempty(labelsToCheck)
+                try
+                    nodeTitle = get(hndl,labelsToCheck{1});
+                    strField = labelsToCheck{1};  %#ok - used for debugging
+                catch
+                    % never mind - probably missing prop, so skip to next one
+                end
+                labelsToCheck(1) = [];
+            end
+            if length(nodeTitle) ~= numel(nodeTitle)
+                % Multi-line - convert to a long single line
+                nodeTitle = nodeTitle';
+                nodeTitle = nodeTitle(:)';
+            end
+            if isempty(char(nodeTitle))
+                % No title - check whether this is an HG label whose text is gettable
+                try
+                    location = hndl.getLocationOnScreen;
+                    pos = [location.getX, location.getY, hndl.getWidth, hndl.getHeight];
+                    %dist = sum((labelPositions-repmat(pos,size(labelPositions,1),[1,1,1,1])).^2, 2);
+                    dist = sum((labelPositions-repmat(pos,[size(labelPositions,1),1])).^2, 2);
+                    [minVal,minIdx] = min(dist);
+                    % Allow max distance of 8 = 2^2+2^2 (i.e. X&Y off by up to 2 pixels, W&H exact)
+                    if minVal <= 8  % 8=2^2+2^2
+                        nodeTitle = get(hg_labels(minIdx),'string');
+                        % Preserve the label handles & position for the tooltip & context-menu
+                        %hg_labels(minIdx) = [];
+                        %labelPositions(minIdx,:) = [];
+                    end
+                catch
+                    % never mind...
+                end
+            end
+            if nargin<2,  charsLimit = 25;  end
+            extraStr = regexprep(nodeTitle,{sprintf('(.{%d,%d}).*',charsLimit,min(charsLimit,length(nodeTitle)-1)),' +'},{'$1...',' '},'once');
+            if ~isempty(extraStr)
+                if ischar(extraStr)
+                    nodeName = nodeName.concat(' (''').concat(extraStr).concat(''')');
+                else
+                    nodeName = nodeName.concat(' (').concat(num2str(extraStr)).concat(')');
+                end
+                %nodeName = nodeName.concat(strField);
+            end
+        catch
+            % Never mind - use whatever we have so far
+            %dispError
+        end
+    end
+
+    %% Strip standard Swing callbacks from a list of events
+    function evNames = stripStdCbs(evNames)
+        try
+            stdEvents = {'AncestorAdded',  'AncestorMoved',    'AncestorRemoved', 'AncestorResized', ...
+                         'ComponentAdded', 'ComponentRemoved', 'ComponentHidden', ...
+                         'ComponentMoved', 'ComponentResized', 'ComponentShown', ...
+                         'FocusGained',    'FocusLost',        'HierarchyChanged', ...
+                         'KeyPressed',     'KeyReleased',      'KeyTyped', ...
+                         'MouseClicked',   'MouseDragged',     'MouseEntered',  'MouseExited', ...
+                         'MouseMoved',     'MousePressed',     'MouseReleased', 'MouseWheelMoved', ...
+                         'PropertyChange', 'VetoableChange',   ...
+                         'CaretPositionChanged',               'InputMethodTextChanged', ...
+                         'ButtonDown',     'Create',           'Delete'};
+            stdEvents = [stdEvents, strcat(stdEvents,'Callback'), strcat(stdEvents,'Fcn')];
+            evNames = setdiff(evNames,stdEvents)';
+        catch
+            % Never mind...
+            dispError
+        end
+    end
+
+    %% Callback function for <Hide standard callbacks> checkbox
+    function cbHideStdCbs_Callback(src, evd, callbacksTable, varargin)  %#ok
+        try
+            % Store the current checkbox value for later use
+            if nargin < 3
+                try
+                    callbacksTable = get(src,'userdata');
+                catch
+                    callbacksTable = getappdata(src,'userdata');
+                end
+            end
+            if evd.getSource.isSelected
+                setappdata(callbacksTable,'hideStdCbs',1);
+            else
+                setappdata(callbacksTable,'hideStdCbs',[]);
+            end
+
+            % Rescan the current node
+            try
+                userdata = get(callbacksTable,'userdata');
+            catch
+                userdata = getappdata(callbacksTable,'userdata');
+            end
+            nodepath = userdata.jTree.getSelectionModel.getSelectionPath;
+            try
+                ed.getCurrentNode = nodepath.getLastPathComponent;
+                nodeSelected(handle(userdata.jTreePeer),ed,[]);
+            catch
+                % ignore - probably no node selected
+            end
+        catch
+            % Never mind...
+            dispError
+        end
+    end
+
+    %% Callback function for <UndocumentedMatlab.com> button
+    function btWebsite_Callback(src, evd, varargin)  %#ok
+        try
+            web('http://UndocumentedMatlab.com','-browser');
+        catch
+            % Never mind...
+            dispError
+        end
+    end
+
+    %% Callback function for <Refresh data> button
+    function btRefresh_Callback(src, evd, varargin)  %#ok
+        try
+            % Set cursor shape to hourglass until we're done
+            hTreeFig = varargin{2};
+            set(hTreeFig,'Pointer','watch');
+            drawnow;
+            object = varargin{1};
+
+            % Re-invoke this utility to re-scan the container for all children
+            findjobj(object);
+        catch
+            % Never mind...
+        end
+
+        % Restore default cursor shape
+        set(hTreeFig,'Pointer','arrow');
+    end
+
+    %% Callback function for <Export> button
+    function btExport_Callback(src, evd, varargin)  %#ok
+        try
+            % Get the list of all selected nodes
+            if length(varargin) > 1
+                jTree = varargin{1};
+                numSelections  = jTree.getSelectionCount;
+                selectionPaths = jTree.getSelectionPaths;
+                hdls = handle([]);
+                for idx = 1 : numSelections
+                    %hdls(idx) = handle(selectionPaths(idx).getLastPathComponent.getUserObject);
+                    nodedata = get(selectionPaths(idx).getLastPathComponent,'userdata');
+                    try
+                        hdls(idx) = handle(nodedata.obj,'CallbackProperties');
+                    catch
+                        if idx==1  % probably due to HG2: can't convert object to handle([])
+                            hdls = nodedata.obj;
+                        else
+                            hdls(idx) = nodedata.obj;
+                        end
+                    end
+                end
+
+                % Assign the handles in the base workspace & inform user
+                assignin('base','findjobj_hdls',hdls);
+                classNameLabel = varargin{2};
+                msg = ['Exported ' char(classNameLabel.getText.trim) ' to base workspace variable findjobj_hdls'];
+            else
+                % Right-click (context-menu) callback
+                data = varargin{1};
+                obj = data{1};
+                varName = data{2};
+                if isempty(varName)
+                    varName = inputdlg('Enter workspace variable name','FindJObj');
+                    if isempty(varName),  return;  end  % bail out on <Cancel>
+                    varName = varName{1};
+                    if isempty(varName) || ~ischar(varName),  return;  end  % bail out on empty/null
+                    varName = genvarname(varName);
+                end
+                assignin('base',varName,handle(obj,'CallbackProperties'));
+                msg = ['Exported object to base workspace variable ' varName];
+            end
+            msgbox(msg,'FindJObj','help');
+        catch
+            % Never mind...
+            dispError
+        end
+    end
+
+    %% Callback function for <Request focus> button
+    function btFocus_Callback(src, evd, varargin)  %#ok
+        try
+            % Request focus for the specified object
+            object = getTopSelectedObject(varargin{:});
+            object.requestFocus;
+        catch
+            try
+                object = object.java.getPeer.requestFocus;
+                object.requestFocus;
+            catch
+                % Never mind...
+                %dispError
+            end
+        end
+    end
+
+    %% Callback function for <Inspect> button
+    function btInspect_Callback(src, evd, varargin)  %#ok
+        try
+            % Inspect the specified object
+            if length(varargin) == 1
+                object = varargin{1};
+            else
+                object = getTopSelectedObject(varargin{:});
+            end
+            if isempty(which('uiinspect'))
+
+                % If the user has not indicated NOT to be informed about UIInspect
+                if ~ispref('FindJObj','dontCheckUIInspect')
+
+                    % Ask the user whether to download UIINSPECT (YES, no, no & don't ask again)
+                    answer = questdlg({'The object inspector requires UIINSPECT from the MathWorks File Exchange. UIINSPECT was created by Yair Altman, like this FindJObj utility.','','Download & install UIINSPECT?'},'UIInspect','Yes','No','No & never ask again','Yes');
+                    switch answer
+                        case 'Yes'  % => Yes: download & install
+                            try
+                                % Download UIINSPECT
+                                baseUrl = 'http://www.mathworks.com/matlabcentral/fileexchange/17935';
+                                fileUrl = [baseUrl '?controller=file_infos&download=true'];
+                                %file = urlread(fileUrl);
+                                %file = regexprep(file,[char(13),char(10)],'\n');  %convert to OS-dependent EOL
+
+                                % Install...
+                                %newPath = fullfile(fileparts(which(mfilename)),'uiinspect.m');
+                                %fid = fopen(newPath,'wt');
+                                %fprintf(fid,'%s',file);
+                                %fclose(fid);
+                                [fpath,fname,fext] = fileparts(which(mfilename));
+                                zipFileName = fullfile(fpath,'uiinspect.zip');
+                                urlwrite(fileUrl,zipFileName);
+                                unzip(zipFileName,fpath);
+                                rehash;
+                            catch
+                                % Error downloading: inform the user
+                                msgbox(['Error in downloading: ' lasterr], 'UIInspect', 'warn');  %#ok
+                                web(baseUrl);
+                            end
+
+                            % ...and now run it...
+                            %pause(0.1); 
+                            drawnow;
+                            dummy = which('uiinspect');  %#ok used only to load into memory
+                            uiinspect(object);
+                            return;
+
+                        case 'No & never ask again'   % => No & don't ask again
+                            setpref('FindJObj','dontCheckUIInspect',1);
+
+                        otherwise
+                            % forget it...
+                    end
+                end
+                drawnow;
+
+                % No UIINSPECT available - run the good-ol' METHODSVIEW()...
+                methodsview(object);
+            else
+                uiinspect(object);
+            end
+        catch
+            try
+                if isjava(object)
+                    methodsview(object)
+                else
+                    methodsview(object.java);
+                end
+            catch
+                % Never mind...
+                dispError
+            end
+        end
+    end
+
+    %% Callback function for <Check for updates> button
+    function btCheckFex_Callback(src, evd, varargin)  %#ok
+        try
+            % Check the FileExchange for the latest version
+            web('http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=14317');
+        catch
+            % Never mind...
+            dispError
+        end
+    end
+
+    %% Check for existence of a newer version
+    function checkVersion()
+        try
+            % If the user has not indicated NOT to be informed
+            if ~ispref('FindJObj','dontCheckNewerVersion')
+
+                % Get the latest version date from the File Exchange webpage
+                baseUrl = 'http://www.mathworks.com/matlabcentral/fileexchange/';
+                webUrl = [baseUrl '14317'];  % 'loadFile.do?objectId=14317'];
+                webPage = urlread(webUrl);
+                modIdx = strfind(webPage,'>Updates<');
+                if ~isempty(modIdx)
+                    webPage = webPage(modIdx:end);
+                    % Note: regexp hangs if substr not found, so use strfind instead...
+                    %latestWebVersion = regexprep(webPage,'.*?>(20[\d-]+)</td>.*','$1');
+                    dateIdx = strfind(webPage,'class="date">');
+                    if ~isempty(dateIdx)
+                        latestDate = webPage(dateIdx(end)+13 : dateIdx(end)+23);
+                        try
+                            startIdx = dateIdx(end)+27;
+                            descStartIdx = startIdx + strfind(webPage(startIdx:startIdx+999),'<td>');
+                            descEndIdx   = startIdx + strfind(webPage(startIdx:startIdx+999),'</td>');
+                            descStr = webPage(descStartIdx(1)+3 : descEndIdx(1)-2);
+                            descStr = regexprep(descStr,'</?[pP]>','');
+                        catch
+                            descStr = '';
+                        end
+
+                        % Get this file's latest date
+                        thisFileName = which(mfilename);  %#ok
+                        try
+                            thisFileData = dir(thisFileName);
+                            try
+                                thisFileDatenum = thisFileData.datenum;
+                            catch  % old ML versions...
+                                thisFileDatenum = datenum(thisFileData.date);
+                            end
+                        catch
+                            thisFileText = evalc('type(thisFileName)');
+                            thisFileLatestDate = regexprep(thisFileText,'.*Change log:[\s%]+([\d-]+).*','$1');
+                            thisFileDatenum = datenum(thisFileLatestDate,'yyyy-mm-dd');
+                        end
+
+                        % If there's a newer version on the File Exchange webpage (allow 2 days grace period)
+                        if (thisFileDatenum < datenum(latestDate,'dd mmm yyyy')-2)
+
+                            % Ask the user whether to download the newer version (YES, no, no & don't ask again)
+                            msg = {['A newer version (' latestDate ') of FindJObj is available on the MathWorks File Exchange:'], '', ...
+                                   ['\color{blue}' descStr '\color{black}'], '', ...
+                                   'Download & install the new version?'};
+                            createStruct.Interpreter = 'tex';
+                            createStruct.Default = 'Yes';
+                            answer = questdlg(msg,'FindJObj','Yes','No','No & never ask again',createStruct);
+                            switch answer
+                                case 'Yes'  % => Yes: download & install newer file
+                                    try
+                                        %fileUrl = [baseUrl '/download.do?objectId=14317&fn=findjobj&fe=.m'];
+                                        fileUrl = [baseUrl '/14317?controller=file_infos&download=true'];
+                                        %file = urlread(fileUrl);
+                                        %file = regexprep(file,[char(13),char(10)],'\n');  %convert to OS-dependent EOL
+                                        %fid = fopen(thisFileName,'wt');
+                                        %fprintf(fid,'%s',file);
+                                        %fclose(fid);
+                                        [fpath,fname,fext] = fileparts(thisFileName);
+                                        zipFileName = fullfile(fpath,[fname '.zip']);
+                                        urlwrite(fileUrl,zipFileName);
+                                        unzip(zipFileName,fpath);
+                                        rehash;
+                                    catch
+                                        % Error downloading: inform the user
+                                        msgbox(['Error in downloading: ' lasterr], 'FindJObj', 'warn');  %#ok
+                                        web(webUrl);
+                                    end
+                                case 'No & never ask again'   % => No & don't ask again
+                                    setpref('FindJObj','dontCheckNewerVersion',1);
+                                otherwise
+                                    % forget it...
+                            end
+                        end
+                    end
+                else
+                    % Maybe webpage not fully loaded or changed format - bail out...
+                end
+            end
+        catch
+            % Never mind...
+        end
+    end
+
+    %% Get the first selected object (might not be the top one - depends on selection order)
+    function object = getTopSelectedObject(jTree, root)
+        try
+            object = [];
+            numSelections  = jTree.getSelectionCount;
+            if numSelections > 0
+                % Get the first object specified
+                %object = jTree.getSelectionPath.getLastPathComponent.getUserObject;
+                nodedata = get(jTree.getSelectionPath.getLastPathComponent,'userdata');
+            else
+                % Get the root object (container)
+                %object = root.getUserObject;
+                nodedata = get(root,'userdata');
+            end
+            object = nodedata.obj;
+        catch
+            % Never mind...
+            dispError
+        end
+    end
+
+    %% Update component callback upon callbacksTable data change
+    function tbCallbacksChanged(src, evd, object, table)
+        persistent hash
+        try
+            % exit if invalid handle or already in Callback
+            %if ~ishandle(src) || ~isempty(getappdata(src,'inCallback')) % || length(dbstack)>1  %exit also if not called from user action
+            if isempty(hash), hash = java.util.Hashtable;  end
+            if ~ishandle(src) || ~isempty(hash.get(src)) % || length(dbstack)>1  %exit also if not called from user action
+                return;
+            end
+            %setappdata(src,'inCallback',1);  % used to prevent endless recursion   % can't use getappdata(src,...) because it fails on R2010b!!!
+            hash.put(src,1);
+
+            % Update the object's callback with the modified value
+            modifiedColIdx = evd.getColumn;
+            modifiedRowIdx = evd.getFirstRow;
+            if modifiedRowIdx>=0 %&& modifiedColIdx==1  %sanity check - should always be true
+                %table = evd.getSource;
+                %object = get(src,'userdata');
+                modifiedRowIdx = table.getSelectedRow;  % overcome issues with hierarchical table
+                cbName = strtrim(table.getValueAt(modifiedRowIdx,0));
+                try
+                    cbValue = strtrim(char(table.getValueAt(modifiedRowIdx,1)));
+                    if ~isempty(cbValue) && ismember(cbValue(1),'{[@''')
+                        cbValue = eval(cbValue);
+                    end
+                    if (~ischar(cbValue) && ~isa(cbValue, 'function_handle') && (~iscell(cbValue) || iscom(object(1))))
+                        revertCbTableModification(table, modifiedRowIdx, modifiedColIdx, cbName, object, '');
+                    else
+                        for objIdx = 1 : length(object)
+                            obj = object(objIdx);
+                            if ~iscom(obj)
+                                try
+                                    try
+                                        if isjava(obj)
+                                            obj = handle(obj,'CallbackProperties');
+                                        end
+                                    catch
+                                        % never mind...
+                                    end
+                                    set(obj, cbName, cbValue);
+                                catch
+                                    try
+                                        set(handle(obj,'CallbackProperties'), cbName, cbValue);
+                                    catch
+                                        % never mind - probably a callback-group header
+                                    end
+                                end
+                            else
+                                cbs = obj.eventlisteners;
+                                if ~isempty(cbs)
+                                    cbs = cbs(strcmpi(cbs(:,1),cbName),:);
+                                    obj.unregisterevent(cbs);
+                                end
+                                if ~isempty(cbValue) && ~strcmp(cbName,'-')
+                                    obj.registerevent({cbName, cbValue});
+                                end
+                            end
+                        end
+                    end
+                catch
+                    revertCbTableModification(table, modifiedRowIdx, modifiedColIdx, cbName, object, lasterr)  %#ok
+                end
+            end
+        catch
+            % never mind...
+        end
+        %setappdata(src,'inCallback',[]);  % used to prevent endless recursion   % can't use setappdata(src,...) because it fails on R2010b!!!
+        hash.remove(src);
+    end
+
+    %% Revert Callback table modification
+    function revertCbTableModification(table, modifiedRowIdx, modifiedColIdx, cbName, object, errMsg)  %#ok
+        try
+            % Display a notification MsgBox
+            msg = 'Callbacks must be a ''string'', or a @function handle';
+            if ~iscom(object(1)),  msg = [msg ' or a {@func,args...} construct'];  end
+            if ~isempty(errMsg),  msg = {errMsg, '', msg};  end
+            msgbox(msg, ['Error setting ' cbName ' value'], 'warn');
+
+            % Revert to the current value
+            curValue = '';
+            try
+                if ~iscom(object(1))
+                    curValue = charizeData(get(object(1),cbName));
+                else
+                    cbs = object(1).eventlisteners;
+                    if ~isempty(cbs)
+                        cbs = cbs(strcmpi(cbs(:,1),cbName),:);
+                        curValue = charizeData(cbs(1,2));
+                    end
+                end
+            catch
+                % never mind... - clear the current value
+            end
+            table.setValueAt(curValue, modifiedRowIdx, modifiedColIdx);
+        catch
+            % never mind...
+        end
+    end  % revertCbTableModification
+
+    %% Get the Java positions of all HG text labels
+    function labelPositions = getLabelsJavaPos(container)
+        try
+            labelPositions = [];
+
+            % Ensure we have a figure handle
+            try
+                h = hFig;  %#ok unused
+            catch
+                hFig = getCurrentFigure;
+            end
+
+            % Get the figure's margins from the Matlab properties
+            hg_labels = findall(hFig, 'Style','text');
+            units = get(hFig,'units');
+            set(hFig,'units','pixels');
+            outerPos = get(hFig,'OuterPosition');
+            innerPos = get(hFig,'Position');
+            set(hFig,'units',units);
+            margins = abs(innerPos-outerPos);
+
+            figX = container.getX;        % =outerPos(1)
+            figY = container.getY;        % =outerPos(2)
+            %figW = container.getWidth;   % =outerPos(3)
+            figH = container.getHeight;   % =outerPos(4)
+
+            % Get the relevant screen pixel size
+            %monitorPositions = get(0,'MonitorPositions');
+            %for monitorIdx = size(monitorPositions,1) : -1 : 1
+            %    screenSize = monitorPositions(monitorIdx,:);
+            %    if (outerPos(1) >= screenSize(1)) % && (outerPos(1)+outerPos(3) <= screenSize(3))
+            %        break;
+            %    end
+            %end
+            %monitorBaseY = screenSize(4) - monitorPositions(1,4);
+
+            % Compute the labels' screen pixel position in Java units ([0,0] = top left)
+            for idx = 1 : length(hg_labels)
+                matlabPos = getpixelposition(hg_labels(idx),1);
+                javaX = figX + matlabPos(1) + margins(1);
+                javaY = figY + figH - matlabPos(2) - matlabPos(4) - margins(2);
+                labelPositions(idx,:) = [javaX, javaY, matlabPos(3:4)];  %#ok grow
+            end
+        catch
+            % never mind...
+            err = lasterror;  %#ok debug point
+        end
+    end
+
+    %% Traverse an HG container hierarchy and extract the HG elements within
+    function traverseHGContainer(hcontainer,level,parent)
+        try
+            % Record the data for this node
+            thisIdx = length(hg_levels) + 1;
+            hg_levels(thisIdx) = level;
+            hg_parentIdx(thisIdx) = parent;
+            try
+                hg_handles(thisIdx) = handle(hcontainer);
+            catch
+                hg_handles = handle(hcontainer);
+            end
+            parentId = length(hg_parentIdx);
+
+            % Now recursively process all this node's children (if any)
+            %if ishghandle(hcontainer)
+            try  % try-catch is faster than checking ishghandle(hcontainer)...
+                allChildren = allchild(handle(hcontainer));
+                for childIdx = 1 : length(allChildren)
+                    traverseHGContainer(allChildren(childIdx),level+1,parentId);
+                end
+            catch
+                % do nothing - probably not a container
+                %dispError
+            end
+
+            % TODO: Add axis (plot) component handles
+        catch
+            % forget it...
+        end
+    end
+
+    %% Debuggable "quiet" error-handling
+    function dispError
+        err = lasterror;  %#ok
+        msg = err.message;
+        for idx = 1 : length(err.stack)
+            filename = err.stack(idx).file;
+            if ~isempty(regexpi(filename,mfilename))
+                funcname = err.stack(idx).name;
+                line = num2str(err.stack(idx).line);
+                msg = [msg ' at <a href="matlab:opentoline(''' filename ''',' line ');">' funcname ' line #' line '</a>']; %#ok grow
+                break;
+            end
+        end
+        disp(msg);
+        return;  % debug point
+    end
+
+    %% ML 7.0 - compatible ischar() function
+    function flag = ischar(data)
+        try
+            flag = builtin('ischar',data);
+        catch
+            flag = isa(data,'char');
+        end
+    end
+
+    %% Set up the uitree context (right-click) menu
+    function jmenu = setTreeContextMenu(obj,node,tree_h)
+          % Prepare the context menu (note the use of HTML labels)
+          import javax.swing.*
+          titleStr = getNodeTitleStr(obj,node);
+          titleStr = regexprep(titleStr,'<hr>.*','');
+          menuItem0 = JMenuItem(titleStr);
+          menuItem0.setEnabled(false);
+          menuItem0.setArmed(false);
+          %menuItem1 = JMenuItem('Export handle to findjobj_hdls');
+          if isjava(obj), prefix = 'j';  else,  prefix = 'h';  end  %#ok<NOCOM>
+          varname = strrep([prefix strtok(char(node.getName))], '$','_');
+          varname = genvarname(varname);
+          varname(2) = upper(varname(2));  % ensure lowerCamelCase
+          menuItem1 = JMenuItem(['Export handle to ' varname]);
+          menuItem2 = JMenuItem('Export handle to...');
+          menuItem3 = JMenuItem('Request focus (bring to front)');
+          menuItem4 = JMenuItem('Flash component borders');
+          menuItem5 = JMenuItem('Display properties & callbacks');
+          menuItem6 = JMenuItem('Inspect object');
+
+          % Set the menu items' callbacks
+          set(handle(menuItem1,'CallbackProperties'), 'ActionPerformedCallback', {@btExport_Callback,{obj,varname}});
+          set(handle(menuItem2,'CallbackProperties'), 'ActionPerformedCallback', {@btExport_Callback,{obj,[]}});
+          set(handle(menuItem3,'CallbackProperties'), 'ActionPerformedCallback', {@requestFocus,obj});
+          set(handle(menuItem4,'CallbackProperties'), 'ActionPerformedCallback', {@flashComponent,{obj,0.2,3}});
+          set(handle(menuItem5,'CallbackProperties'), 'ActionPerformedCallback', {@nodeSelected,{tree_h,node}});
+          set(handle(menuItem6,'CallbackProperties'), 'ActionPerformedCallback', {@btInspect_Callback,obj});
+
+          % Add all menu items to the context menu (with internal separator)
+          jmenu = JPopupMenu;
+          jmenu.add(menuItem0);
+          jmenu.addSeparator;
+          handleValue=[];  try handleValue = double(obj); catch, end;  %#ok
+          if ~isempty(handleValue)
+              % For valid HG handles only
+              menuItem0a = JMenuItem('Copy handle value to clipboard');
+              set(handle(menuItem0a,'CallbackProperties'), 'ActionPerformedCallback', sprintf('clipboard(''copy'',%.99g)',handleValue));
+              jmenu.add(menuItem0a);
+          end
+          jmenu.add(menuItem1);
+          jmenu.add(menuItem2);
+          jmenu.addSeparator;
+          jmenu.add(menuItem3);
+          jmenu.add(menuItem4);
+          jmenu.add(menuItem5);
+          jmenu.add(menuItem6);
+    end  % setTreeContextMenu
+
+    %% Set the mouse-press callback
+    function treeMousePressedCallback(hTree, eventData, tree_h)  %#ok hTree is unused
+        if eventData.isMetaDown  % right-click is like a Meta-button
+            % Get the clicked node
+            clickX = eventData.getX;
+            clickY = eventData.getY;
+            jtree = eventData.getSource;
+            treePath = jtree.getPathForLocation(clickX, clickY);
+            try
+                % Modify the context menu based on the clicked node
+                node = treePath.getLastPathComponent;
+                userdata = get(node,'userdata');
+                obj = userdata.obj;
+                jmenu = setTreeContextMenu(obj,node,tree_h);
+
+                % TODO: remember to call jmenu.remove(item) in item callback
+                % or use the timer hack shown here to remove the item:
+                %    timerFcn = {@menuRemoveItem,jmenu,item};
+                %    start(timer('TimerFcn',timerFcn,'StartDelay',0.2));
+
+                % Display the (possibly-modified) context menu
+                jmenu.show(jtree, clickX, clickY);
+                jmenu.repaint;
+
+                % This is for debugging:
+                userdata.tree = jtree;
+                setappdata(gcf,'findjobj_hgtree',userdata)
+            catch
+                % clicked location is NOT on top of any node
+                % Note: can also be tested by isempty(treePath)
+            end
+        end
+    end  % treeMousePressedCallback
+
+    %% Remove the extra context menu item after display
+    function menuRemoveItem(hObj,eventData,jmenu,item) %#ok unused
+        jmenu.remove(item);
+    end  % menuRemoveItem
+
+    %% Get the title for the tooltip and context (right-click) menu
+    function nodeTitleStr = getNodeTitleStr(obj,node)
+        try
+            % Display the full classname and object name in the tooltip
+            %nodeName = char(node.getName);
+            %nodeName = strrep(nodeName, '<HTML><i><font color="gray">','');
+            %nodeName = strrep(nodeName, '</font></i></html>','');
+            nodeName = char(getNodeName(obj,99));
+            [objClass,objName] = strtok(nodeName);
+            objName = objName(3:end-1);  % strip leading ( and trailing )
+            if isempty(objName),  objName = '(none found)';  end
+            nodeName = char(node.getName);
+            objClass = char(obj.getClass.getName);
+            nodeTitleStr = sprintf('<html>Class name: <font color="blue">%s</font><br>Text/title: %s',objClass,objName);
+
+            % If the component is invisible, state this in the tooltip
+            if ~isempty(strfind(nodeName,'color="gray"'))
+                nodeTitleStr = [nodeTitleStr '<br><font color="gray"><i><b>*** Invisible ***</b></i></font>'];
+            end
+            nodeTitleStr = [nodeTitleStr '<hr>Right-click for context-menu'];
+        catch
+            % Possible not a Java object - try treating as an HG handle
+            try
+                handleValueStr = sprintf('#: <font color="blue"><b>%.99g<b></font>',double(obj));
+                try
+                    type = '';
+                    type = get(obj,'type');
+                    type(1) = upper(type(1));
+                catch
+                    if ~ishandle(obj)
+                        type = ['<font color="red"><b>Invalid <i>' char(node.getName) '</i>'];
+                        handleValueStr = '!!!</b></font><br>Perhaps this handle was deleted after this UIInspect tree was<br>already drawn. Try to refresh by selecting any valid node handle';
+                    end
+                end
+                nodeTitleStr = sprintf('<html>%s handle %s',type,handleValueStr);
+                try
+                    % If the component is invisible, state this in the tooltip
+                    if strcmp(get(obj,'Visible'),'off')
+                        nodeTitleStr = [nodeTitleStr '<br><center><font color="gray"><i>Invisible</i></font>'];
+                    end
+                catch
+                    % never mind...
+                end
+            catch
+                % never mind... - ignore
+            end
+        end
+    end  % getNodeTitleStr
+
+    %% Handle tree mouse movement callback - used to set the tooltip & context-menu
+    function treeMouseMovedCallback(hTree, eventData)
+          try
+              x = eventData.getX;
+              y = eventData.getY;
+              jtree = eventData.getSource;
+              treePath = jtree.getPathForLocation(x, y);
+              try
+                  % Set the tooltip string based on the hovered node
+                  node = treePath.getLastPathComponent;
+                  userdata = get(node,'userdata');
+                  obj = userdata.obj;
+                  tooltipStr = getNodeTitleStr(obj,node);
+                  set(hTree,'ToolTipText',tooltipStr)
+              catch
+                  % clicked location is NOT on top of any node
+                  % Note: can also be tested by isempty(treePath)
+              end
+          catch
+              dispError;
+          end
+          return;  % denug breakpoint
+    end  % treeMouseMovedCallback
+
+    %% Request focus for a specific object handle
+    function requestFocus(hTree, eventData, obj)  %#ok hTree & eventData are unused
+        % Ensure the object handle is valid
+        if isjava(obj)
+            obj.requestFocus;
+            return;
+        elseif ~ishandle(obj)
+            msgbox('The selected object does not appear to be a valid handle as defined by the ishandle() function. Perhaps this object was deleted after this hierarchy tree was already drawn. Refresh this tree by selecting a valid node handle and then retry.','FindJObj','warn');
+            beep;
+            return;
+        end
+
+        try
+            foundFlag = 0;
+            while ~foundFlag
+                if isempty(obj),  return;  end  % sanity check
+                type = get(obj,'type');
+                obj = double(obj);
+                foundFlag = any(strcmp(type,{'figure','axes','uicontrol'}));
+                if ~foundFlag
+                    obj = get(obj,'Parent');
+                end
+            end
+            feval(type,obj);
+        catch
+            % never mind...
+            dispError;
+        end
+    end  % requestFocus
+
+end  % FINDJOBJ
+
+% Fast implementation
+function jControl = findjobj_fast(hControl, jContainer)
+    try jControl = hControl.getTable; return, catch, end  % fast bail-out for old uitables
+    try jControl = hControl.JavaFrame.getGUIDEView; return, catch, end  % bail-out for HG2 matlab.ui.container.Panel
+    oldWarn = warning('off','MATLAB:HandleGraphics:ObsoletedProperty:JavaFrame');
+    if nargin < 2 || isempty(jContainer)
+        % Use a HG2 matlab.ui.container.Panel jContainer if the control's parent is a uipanel
+        try
+            hParent = get(hControl,'Parent');
+        catch
+            % Probably indicates an invalid/deleted/empty handle
+            jControl = [];
+            return
+        end
+        try jContainer = hParent.JavaFrame.getGUIDEView; catch, jContainer = []; end
+    end
+    if isempty(jContainer)
+        hFig = ancestor(hControl,'figure');
+        jf = get(hFig, 'JavaFrame');
+        jContainer = jf.getFigurePanelContainer.getComponent(0);
+    end
+    warning(oldWarn);
+    jControl = [];
+    counter = 20;  % 2018-09-21 speedup (100 x 0.001 => 20 x 0.005) - Martin Lehmann suggestion on FEX 2016-06-07
+    specialTooltipStr = '!@#$%^&*';
+    try  % Fix for R2018b suggested by Eddie (FEX comment 2018-09-19)
+        tooltipPropName = 'TooltipString';
+        oldTooltip = get(hControl,tooltipPropName);
+        set(hControl,tooltipPropName,specialTooltipStr);
+    catch
+        tooltipPropName = 'Tooltip';
+        oldTooltip = get(hControl,tooltipPropName);
+        set(hControl,tooltipPropName,specialTooltipStr);
+    end
+    while isempty(jControl) && counter>0
+        counter = counter - 1;
+        pause(0.005);
+        jControl = findTooltipIn(jContainer, specialTooltipStr);
+    end
+    set(hControl,tooltipPropName,oldTooltip);
+    try jControl.setToolTipText(oldTooltip); catch, end
+    try jControl = jControl.getParent.getView.getParent.getParent; catch, end  % return JScrollPane if exists
+end
+function jControl = findTooltipIn(jContainer, specialTooltipStr)
+    try
+        jControl = [];  % Fix suggested by H. Koch 11/4/2017
+        tooltipStr = jContainer.getToolTipText;
+        %if strcmp(char(tooltipStr),specialTooltipStr)
+        if ~isempty(tooltipStr) && tooltipStr.startsWith(specialTooltipStr)  % a bit faster
+            jControl = jContainer;
+        else
+            for idx = 1 : jContainer.getComponentCount
+                jControl = findTooltipIn(jContainer.getComponent(idx-1), specialTooltipStr);
+                if ~isempty(jControl), return; end
+            end
+        end
+    catch
+        % ignore
+    end
+end
+
+%% TODO TODO TODO
+%{
+- Enh: Improve interactive-GUI performance - esp. expandNode()
+- Enh: Add property listeners - same problem in MathWork's inspect.m
+- Enh: Display additional properties - same problem in MathWork's inspect.m
+- Enh: Add axis (plot, Graphics) component handles
+- Enh: Add figure thumbnail image below the java tree (& indicate corresponding jObject when selected)
+- Enh: scroll initially-selected node into view (problem because treenode has no pixel location)
+- Fix: java exceptions when getting some fields (com.mathworks.mlwidgets.inspector.PropertyRootNode$PropertyListener$1$1.run)
+- Fix: use EDT if available (especially in flashComponent)
+%}
\ No newline at end of file
diff --git a/Core/utils/getReportOctave.m b/+ocl/+utils/getReportOctave.m
similarity index 100%
rename from Core/utils/getReportOctave.m
rename to +ocl/+utils/getReportOctave.m
diff --git a/+ocl/+utils/info.m b/+ocl/+utils/info.m
new file mode 100644
index 00000000..a6bf6ab1
--- /dev/null
+++ b/+ocl/+utils/info.m
@@ -0,0 +1,23 @@
+% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
+% Redistribution is permitted under the 3-Clause BSD License terms. Please
+% ensure the above copyright notice is visible in any derived work.
+%
+function info(msg)
+
+N = 60; % max line width
+  
+l = length(msg);
+d = floor(l/N);
+
+new_msg = '';
+offset = 0;
+for k=1:d
+  
+  msg_part = msg( (k-1)*N+1 : k*N);
+  spaces = strfind(msg_part, ' ');
+  
+  new_offset = N - spaces(end);
+  new_msg = sprintf('%s %s %s', new_msg, msg( (k-1)*N-offset+1 : k*N-new_offset), newline);
+  offset = new_offset;
+end
+fprintf('%s %s %s', new_msg, msg( d*N-offset+1 : end), newline);
\ No newline at end of file
diff --git a/Core/utils/oclException.m b/+ocl/+utils/isFunHandle.m
similarity index 78%
rename from Core/utils/oclException.m
rename to +ocl/+utils/isFunHandle.m
index 498484de..27426405 100644
--- a/Core/utils/oclException.m
+++ b/+ocl/+utils/isFunHandle.m
@@ -2,5 +2,5 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function oclException(msg)
-  error(['oclException: ', msg])
\ No newline at end of file
+function r = isFunHandle(v)
+r = isa(v,'function_handle');
\ No newline at end of file
diff --git a/Core/utils/oclIsFunHandleOrEmpty.m b/+ocl/+utils/isFunHandleOrEmpty.m
similarity index 87%
rename from Core/utils/oclIsFunHandleOrEmpty.m
rename to +ocl/+utils/isFunHandleOrEmpty.m
index 829a8b62..ca06b705 100644
--- a/Core/utils/oclIsFunHandleOrEmpty.m
+++ b/+ocl/+utils/isFunHandleOrEmpty.m
@@ -2,5 +2,5 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function r = oclIsFunHandleOrEmpty(v)
+function r = isFunHandleOrEmpty(v)
 r = isa(v,'function_handle') || isempty(v);
diff --git a/Core/utils/isOctave.m b/+ocl/+utils/isOctave.m
similarity index 100%
rename from Core/utils/isOctave.m
rename to +ocl/+utils/isOctave.m
diff --git a/+ocl/+utils/isTestRun.m b/+ocl/+utils/isTestRun.m
index 888fb862..2e2a3fe6 100644
--- a/+ocl/+utils/isTestRun.m
+++ b/+ocl/+utils/isTestRun.m
@@ -3,6 +3,6 @@
 % ensure the above copyright notice is visible in any derived work.
 %
 function r = isTestRun()
-  global testRun
-  r = ~isempty(testRun) && testRun;
-end
+global testRun
+r = ~isempty(testRun) && testRun;
+
diff --git a/+ocl/+utils/setTestRun.m b/+ocl/+utils/setTestRun.m
index fa847dda..f474b693 100644
--- a/+ocl/+utils/setTestRun.m
+++ b/+ocl/+utils/setTestRun.m
@@ -1,4 +1,3 @@
 function setTestRun(val)
 global testRun
 testRun = val;
-end
\ No newline at end of file
diff --git a/+ocl/+utils/StartupOCL.m b/+ocl/+utils/startup.m
similarity index 61%
rename from +ocl/+utils/StartupOCL.m
rename to +ocl/+utils/startup.m
index cabee37c..38100de9 100644
--- a/+ocl/+utils/StartupOCL.m
+++ b/+ocl/+utils/startup.m
@@ -2,9 +2,8 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function StartupOCL(in)
-  % StartupOCL(workingDirLocation)
-  % StartupOCL(octaveClear)
+function startup(in)
+  % ocl.utils.startup(workingDirLocation)
   %
   % Startup script for OpenOCL
   % Adds required directories to the path. Sets up a folder for the results
@@ -14,26 +13,20 @@ function StartupOCL(in)
   %   workingDirLocation - path to location where the working directory
   %                        should be created.
 
-  oclPath  = fileparts(which('ocl'));
+  ocl_dir  = fileparts(which('ocl'));
 
-  if isempty(oclPath)
-    error('Can not find OpenOCL. Add root directory of OpenOCL to the path.')
+  if isempty(ocl_dir)
+    ocl.utils.error('Can not find OpenOCL. Add root directory of OpenOCL to the path.')
   end
 
-  workspaceLocation = fullfile(oclPath, 'Workspace');
-  octaveClear = false;
+  workspaceLocation = fullfile(ocl_dir, 'Workspace');
 
-  if nargin == 1 && (islogical(in)||isnumeric(in))
-    octaveClear = in;
-  elseif nargin == 1 && ischar(in)
+  if nargin == 1 && ischar(in)
     workspaceLocation = in;
   elseif nargin == 1
-    oclError('Invalid argument.')
+    ocl.utils.error('Invalid argument.')
   end
-
-  % add current directory to path
-  addpath(pwd);
-
+  
   % create folders for tests and autogenerated code
   testDir     = fullfile(workspaceLocation,'test');
   exportDir   = fullfile(workspaceLocation,'export');
@@ -41,32 +34,26 @@ function StartupOCL(in)
   [~,~] = mkdir(exportDir);
 
   % set environment variables for directories
-  setenv('OPENOCL_PATH', oclPath)
+  setenv('OPENOCL_PATH', ocl_dir)
   setenv('OPENOCL_TEST', testDir)
   setenv('OPENOCL_EXPORT', exportDir)
   setenv('OPENOCL_WORK', workspaceLocation)
 
   % setup directories
-  addpath(oclPath)
+  addpath(ocl_dir)
   addpath(exportDir)
-  addpath(fullfile(oclPath,'CasadiLibrary'))
   
-  addpath(fullfile(oclPath,'doc'))
-
-  addpath(fullfile(oclPath,'Core'))
-  addpath(fullfile(oclPath,'Core','Variables'))
-  addpath(fullfile(oclPath,'Core','Variables','Variable'))
-  addpath(fullfile(oclPath,'Core','utils'))
+  addpath(fullfile(ocl_dir,'doc'))
   
-  if ~exist(fullfile(oclPath,'Lib','casadi'), 'dir')
-    r = mkdir(fullfile(oclPath,'Lib','casadi'));
-    oclAssert(r, 'Could not create directory in Lib/casadi');
+  if ~exist(fullfile(ocl_dir,'Lib','casadi'), 'dir')
+    r = mkdir(fullfile(ocl_dir,'Lib','casadi'));
+    ocl.utils.assert(r, 'Could not create directory in Lib/casadi');
     casadiFound = false;
   else
     % check if casadi is already installed
-    addpath(fullfile(oclPath,'Lib'))
-    addpath(fullfile(oclPath,'Lib','casadi'))
-    casadiFound = checkCasadi(fullfile(oclPath,'Lib','casadi'));
+    addpath(fullfile(ocl_dir,'Lib'))
+    addpath(fullfile(ocl_dir,'Lib','casadi'))
+    casadiFound = checkCasadi(fullfile(ocl_dir,'Lib','casadi'));
   end
 
   % install casadi into Lib folder
@@ -76,111 +63,70 @@ function StartupOCL(in)
       % Windows, >=Matlab 2016a
       path = 'https://github.com/casadi/casadi/releases/download/3.4.5/';
       filename = 'casadi-windows-matlabR2016a-v3.4.5.zip';
-      downloadCasadi(oclPath, path, filename, fullfile(oclPath,'Lib','casadi'));
+      downloadCasadi(ocl_dir, path, filename, fullfile(ocl_dir,'Lib','casadi'));
     elseif ispc && verAtLeast('matlab','8.4')
       % Windows, >=Matlab 2014b
       path = 'https://github.com/casadi/casadi/releases/download/3.4.5/';
       filename = 'casadi-windows-matlabR2014b-v3.4.5.zip';
-      downloadCasadi(oclPath, path, filename, fullfile(oclPath,'Lib','casadi'));
+      downloadCasadi(ocl_dir, path, filename, fullfile(ocl_dir,'Lib','casadi'));
     elseif ispc && verAtLeast('matlab','8.3')
       % Windows, >=Matlab 2014a
       path = 'https://github.com/casadi/casadi/releases/download/3.4.5/';
       filename = 'casadi-windows-matlabR2014a-v3.4.5.zip';
-      downloadCasadi(oclPath, path, filename, fullfile(oclPath,'Lib','casadi'));
+      downloadCasadi(ocl_dir, path, filename, fullfile(ocl_dir,'Lib','casadi'));
     elseif ispc && verAtLeast('matlab','8.1')
       % Windows, >=Matlab 2013a
       path = 'https://github.com/casadi/casadi/releases/download/3.4.5/';
       filename = 'casadi-windows-matlabR2013a-v3.4.5.zip';
-      downloadCasadi(oclPath, path, filename, fullfile(oclPath,'Lib','casadi'));
+      downloadCasadi(ocl_dir, path, filename, fullfile(ocl_dir,'Lib','casadi'));
     
     elseif isunix && ~ismac && verAtLeast('matlab','8.4')
       % Linux, >=Matlab 2014b
       path = 'https://github.com/casadi/casadi/releases/download/3.4.5/';
       filename = 'casadi-linux-matlabR2014b-v3.4.5.tar.gz';
-      downloadCasadi(oclPath, path, filename, fullfile(oclPath,'Lib','casadi'));
+      downloadCasadi(ocl_dir, path, filename, fullfile(ocl_dir,'Lib','casadi'));
     elseif isunix && ~ismac && verAtLeast('matlab','8.3')
       % Linux, >=Matlab 2014a
       path = 'https://github.com/casadi/casadi/releases/download/3.4.5/';
       filename = 'casadi-linux-matlabR2014a-v3.4.5.tar.gz';
-      downloadCasadi(oclPath, path, filename, fullfile(oclPath,'Lib','casadi'));
+      downloadCasadi(ocl_dir, path, filename, fullfile(ocl_dir,'Lib','casadi'));
     
     elseif ismac && verAtLeast('matlab','8.5')
       % Mac, >=Matlab 2015a
       path = 'https://github.com/casadi/casadi/releases/download/3.4.5/';
       filename = 'casadi-osx-matlabR2015a-v3.4.5.tar.gz';
-      downloadCasadi(oclPath, path, filename, fullfile(oclPath,'Lib','casadi'));
+      downloadCasadi(ocl_dir, path, filename, fullfile(ocl_dir,'Lib','casadi'));
     elseif ismac && verAtLeast('matlab','8.4')
       % Mac, >=Matlab 2015a
       path = 'https://github.com/casadi/casadi/releases/download/3.4.5/';
       filename = 'casadi-osx-matlabR2014b-v3.4.5.tar.gz';
-      downloadCasadi(oclPath, path, filename, fullfile(oclPath,'Lib','casadi'));
+      downloadCasadi(ocl_dir, path, filename, fullfile(ocl_dir,'Lib','casadi'));
     elseif ismac && verAtLeast('matlab','8.3')
       % Mac, >=Matlab 2015a
       path = 'https://github.com/casadi/casadi/releases/download/3.4.5/';
       filename = 'casadi-osx-matlabR2014a-v3.4.5.tar.gz';
-      downloadCasadi(oclPath, path, filename, fullfile(oclPath,'Lib','casadi'));
+      downloadCasadi(ocl_dir, path, filename, fullfile(ocl_dir,'Lib','casadi'));
     else
-      oclInfo(['Could not set up CasADi for you system.', ...
+      ocl.utils.info(['Could not set up CasADi for you system.', ...
                'You need to install CasADi yourself and add it to your path.'])
     end
     % add Lib and Lib/casadi to path
-    addpath(fullfile(oclPath,'Lib'))
-    addpath(fullfile(oclPath,'Lib','casadi'))
+    addpath(fullfile(ocl_dir,'Lib'))
+    addpath(fullfile(ocl_dir,'Lib','casadi'))
   end
   
   casadiFound = checkCasadiWorking();
   if casadiFound
-    oclInfo('CasADi is up and running!')
+    ocl.utils.info('CasADi is up and running!')
   else
-    oclError('Go to https://web.casadi.org/get/ and setup CasADi.');
-  end
-
-  % remove properties function in Variable.m for Octave which gives a
-  % parse error
-  if isOctave()
-    variableDir = fullfile(oclPath,'Core','Variables','Variable');
-    %rmpath(variableDir);
-
-    vFilePath = fullfile(exportDir, 'Variable','Variable.m');
-    if ~exist(vFilePath,'file') || octaveClear
-      delete(fullfile(exportDir, 'Variable','V*.m'))
-      status = copyfile(variableDir,exportDir);
-      assert(status, 'Could not copy Variables folder');
-    end
-
-    vFileText = fileread(vFilePath);
-    searchPattern = 'function n = properties(self)';
-    replacePattern = 'function n = ppp(self)';
-    pIndex = strfind(vFileText,searchPattern);
-
-    if ~isempty(pIndex)
-      assert(length(pIndex)==1, ['Found multiple occurences of properties ',...
-                                 'function in Variable.m; Please reinstall ',...
-                                 'OpenOCL.'])
-      newText = strrep(vFileText,searchPattern,replacePattern);
-      fid=fopen(vFilePath,'w');
-      fwrite(fid, newText);
-      fclose(fid);
-    end
-    addpath(fullfile(exportDir,'Variable'));
-  end
-
-  % travis-ci
-  if isOctave()
-    args = argv();
-    if length(args)>0 && args{1} == '1'
-      nFails = runTests(1);
-      if nFails > 0
-        exit(nFails);
-      end
-    end
+    ocl.utils.error('Go to https://web.casadi.org/get/ and setup CasADi.');
   end
 
-  oclInfo('OpenOCL startup procedure finished successfully.')
+  ocl.utils.info('OpenOCL startup procedure finished successfully.')
   
 end
 
-function downloadCasadi(oclPath, path, filename, dest)
+function downloadCasadi(ocl_dir, path, filename, dest)
 
   if exist(fullfile(dest, 'CUSTOM_CASADI'), 'file') > 0
     fprintf(['You chose to your use your custom CasADi installation. If you changed \n', ...
@@ -206,23 +152,23 @@ function downloadCasadi(oclPath, path, filename, dest)
   
   if strcmp(m, 'n') || strcmp(m, 'no')
     try 
-      ocl.test.testVariable;
+      ocl.tests.variable;
       fid = fopen(fullfile(dest, 'CUSTOM_CASADI'),'w');
       fclose(fid);
       return;
     catch e
       warning(e.message)
-      oclError('You did not agree to download CasADi and your version is not compatible. Either run again or set-up a compatible CasADi version manually.');
+      ocl.utils.error('You did not agree to download CasADi and your version is not compatible. Either run again or set-up a compatible CasADi version manually.');
     end
   end
   
-  archive_destination = fullfile(oclPath, 'Workspace', filename);
+  archive_destination = fullfile(ocl_dir, 'Workspace', filename);
 
   if ~exist(archive_destination, 'file')
-    oclInfo('Downloading...')
+    ocl.utils.info('Downloading...')
     websave(archive_destination, [path,filename]);
   end
-  oclInfo('Extracting...')
+  ocl.utils.info('Extracting...')
   [~,~,ending] = fileparts(archive_destination);
   if strcmp(ending, '.zip')
     unzip(archive_destination, dest)
@@ -256,7 +202,7 @@ function downloadCasadi(oclPath, path, filename, dest)
     casadi.SX.sym('x');
     r = true;
   catch e
-    oclInfo(e);
+    ocl.utils.warning(e.message);
     casadiNotWorkingError();
   end
 end
@@ -266,7 +212,7 @@ function downloadCasadi(oclPath, path, filename, dest)
 end
 
 function casadiNotWorkingError
-  oclError(['Casadi installation in the path found but does not ', ...
+  ocl.utils.error(['Casadi installation not found or it does not ', ...
             'work properly. Try restarting Matlab. Remove all ', ...
             'casadi installations from your path. Run ocl.utils.clean. OpenOCL will ', ...
             'then install the correct casadi version for you.']);
diff --git a/Core/utils/oclWarning.m b/+ocl/+utils/warning.m
similarity index 83%
rename from Core/utils/oclWarning.m
rename to +ocl/+utils/warning.m
index fff88ccd..a1adea0d 100644
--- a/Core/utils/oclWarning.m
+++ b/+ocl/+utils/warning.m
@@ -2,7 +2,7 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function oclWarning(msg)
+function warning(msg)
   warning(msg)
   global oclHasWarnings
   oclHasWarnings = true;
\ No newline at end of file
diff --git a/Core/utils/oclWarningNotice.m b/+ocl/+utils/warningNotice.m
similarity index 76%
rename from Core/utils/oclWarningNotice.m
rename to +ocl/+utils/warningNotice.m
index 67087ca7..9fa311c3 100644
--- a/Core/utils/oclWarningNotice.m
+++ b/+ocl/+utils/warningNotice.m
@@ -2,10 +2,10 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function oclWarningNotice()
+function warningNotice()
   global oclHasWarnings
   if ~isempty(oclHasWarnings) && oclHasWarnings
-    oclWarning(['There have been warnings in OpenOCL. Check the output above for warnings. ', ...
+    ocl.utils.warning(['There have been warnings in OpenOCL. Check the output above for warnings. ', ...
                 'Resolve all warnings before you proceed as they ', ...
                 'point to potential issues.']);
     oclHasWarnings = false;
diff --git a/+ocl/+utils/workspacePath.m b/+ocl/+utils/workspacePath.m
new file mode 100644
index 00000000..680a51f2
--- /dev/null
+++ b/+ocl/+utils/workspacePath.m
@@ -0,0 +1,4 @@
+function r = workspacePath()
+
+oclPath  = fileparts(which('ocl'));
+r = fullfile(oclPath, 'Workspace');
\ No newline at end of file
diff --git a/+ocl/+utils/zerofh.m b/+ocl/+utils/zerofh.m
new file mode 100644
index 00000000..565a6c40
--- /dev/null
+++ b/+ocl/+utils/zerofh.m
@@ -0,0 +1,2 @@
+function r = zerofh()
+r = @(varargin)0;
\ No newline at end of file
diff --git a/+ocl/+utils/zerofun.m b/+ocl/+utils/zerofun.m
new file mode 100644
index 00000000..83e1bb3f
--- /dev/null
+++ b/+ocl/+utils/zerofun.m
@@ -0,0 +1,2 @@
+function r = zerofun(varargin)
+r = 0;
\ No newline at end of file
diff --git a/Core/OclAssignment.m b/+ocl/Assignment.m
similarity index 77%
rename from Core/OclAssignment.m
rename to +ocl/Assignment.m
index a7f3bb3c..6d064d2b 100644
--- a/Core/OclAssignment.m
+++ b/+ocl/Assignment.m
@@ -1,11 +1,11 @@
-classdef OclAssignment < handle
+classdef Assignment < handle
   
   properties
     varsList
   end
   
   methods
-    function self = OclAssignment(varsList)
+    function self = Assignment(varsList)
       self.varsList = varsList;
     end
     
@@ -22,14 +22,14 @@
       elseif length(vl) == 1
         [varargout{1:nargout}] = subsref(vl{1} ,s);
       else
-        oclError('Not supported.');
+        ocl.utils.error('Not supported.');
       end
       
     end
     
     function disp(self) 
       vs = self.varsList;
-      disp('OclAssignment with content:');
+      disp('ocl.Assignment with content:');
       disp('{');
       disp(' ');
       for k=1:length(vs)
diff --git a/+ocl/Collocation.m b/+ocl/Collocation.m
new file mode 100644
index 00000000..613b270f
--- /dev/null
+++ b/+ocl/Collocation.m
@@ -0,0 +1,78 @@
+% This class is derived from:
+%
+% An implementation of direct collocation
+% Joel Andersson, 2016
+% https://github.com/casadi/casadi/blob/master/docs/examples/matlab/direct_collocation.m
+%
+% CasADi -- A symbolic framework for dynamic optimization.
+% Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
+%                         K.U. Leuven. All rights reserved.
+% Copyright (C) 2011-2014 Greg Horn
+% Under GNU Lesser General Public License
+
+classdef Collocation < handle
+
+  properties
+
+    daefun
+    pathcostfun
+    
+    coefficients
+    coeff_eval
+    coeff_der
+    coeff_int
+
+    vars
+    num_x
+    num_z
+    num_u
+    num_p
+    num_t
+
+    num_i
+    
+    tau_root
+    order
+  end
+
+  methods
+
+    function self = Collocation(states, algvars, controls, parameters, statesOrder, daefh, pathcostsfh, d)
+
+      nx = length(states);
+      nz = length(algvars);
+      nu = length(controls);
+      np = length(parameters);
+      nt = d;
+      
+      v = ocl.types.Structure();
+      v.addRepeated({'states', 'algvars'}, {states, algvars}, d);
+
+      tau = [0 ocl.collocation.collocationPoints(d)];
+      
+      coeff = ocl.collocation.coefficients(tau);
+      
+      self.daefun = @(x,z,u,p) ocl.model.dae(daefh, states, algvars, controls, parameters, statesOrder, x, z, u, p);
+      self.pathcostfun = @(x,z,u,p) ocl.model.pathcosts(pathcostsfh, states, algvars, controls, parameters, x, z, u, p);
+      
+      self.coefficients = coeff;
+      self.coeff_eval = ocl.collocation.evalCoefficients(coeff, d, 1.0);
+      self.coeff_der = ocl.collocation.evalCoefficientsDerivative(coeff, tau, d);
+      self.coeff_int = ocl.collocation.evalCoefficientsIntegral(coeff, d, 1.0);
+
+      self.vars = v;
+      self.num_x = nx;
+      self.num_z = nz;
+      self.num_u = nu;
+      self.num_p = np;
+      self.num_t = nt;
+      
+      self.num_i = length(v);
+      
+      self.tau_root = tau;
+      self.order = d;
+      
+    end
+
+  end
+end
diff --git a/Core/OclConstraint.m b/+ocl/Constraint.m
old mode 100755
new mode 100644
similarity index 80%
rename from Core/OclConstraint.m
rename to +ocl/Constraint.m
index 59446bf9..762f6ee6
--- a/Core/OclConstraint.m
+++ b/+ocl/Constraint.m
@@ -2,8 +2,8 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-classdef OclConstraint < handle
-  %CONSTRAINT OclConstraint
+classdef Constraint < handle
+  %CONSTRAINT ocl.Constraint
   %   Create, store and access constraints with this class
   
   properties
@@ -14,24 +14,24 @@
   
   methods
     
-    function self = OclConstraint()
+    function self = Constraint()
       self.values = [];
       self.lowerBounds = [];
       self.upperBounds = [];
     end
     
     function setInitialCondition(self,varargin)
-      oclDeprecation('Using of setInitialCondition is deprecated. Just use add instead.');
+      ocl.utils.deprecation('Using of setInitialCondition is deprecated. Just use add instead.');
       self.add(varargin{:});
     end
     
     function addPathConstraint(self,varargin)
-      oclDeprecation('Using of addPathConstraint is deprecated. Just use add instead.');
+      ocl.utils.deprecation('Using of addPathConstraint is deprecated. Just use add instead.');
       self.add(varargin{:});
     end
     
     function addBoundaryCondition(self,varargin)
-      oclDeprecation('Using of addBoundaryCondition is deprecated. Just use add instead.');
+      ocl.utils.deprecation('Using of addBoundaryCondition is deprecated. Just use add instead.');
       self.add(varargin{:});
     end
     
@@ -57,9 +57,9 @@ function add(self,varargin)
     
     function addWithBounds(self,lb,expr,ub)
       
-      lb = Variable.getValueAsColumn(lb);
-      expr = Variable.getValueAsColumn(expr);
-      ub = Variable.getValueAsColumn(ub);
+      lb = ocl.Variable.getValueAsColumn(lb);
+      expr = ocl.Variable.getValueAsColumn(expr);
+      ub = ocl.Variable.getValueAsColumn(ub);
       
       self.lowerBounds  = [self.lowerBounds;lb];
       self.values       = [self.values;expr];
@@ -68,8 +68,8 @@ function addWithBounds(self,lb,expr,ub)
     
     function addWithOperator(self, lhs, op, rhs)
       
-      lhs = Variable.getValueAsColumn(lhs);
-      rhs = Variable.getValueAsColumn(rhs);
+      lhs = ocl.Variable.getValueAsColumn(lhs);
+      rhs = ocl.Variable.getValueAsColumn(rhs);
       
       % Create new constraint entry
       if strcmp(op,'==')
diff --git a/Core/OclCost.m b/+ocl/Cost.m
similarity index 75%
rename from Core/OclCost.m
rename to +ocl/Cost.m
index a89c76ec..7d1138bd 100644
--- a/Core/OclCost.m
+++ b/+ocl/Cost.m
@@ -2,20 +2,20 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-classdef OclCost < handle
+classdef Cost < handle
   properties
     value
   end
   
   methods
     
-    function self = OclCost()
+    function self = Cost()
       self.value = 0;
     end
     
     function add(self,val)
       % add(self,val)
-      self.value = self.value + Variable.getValueAsColumn(val);
+      self.value = self.value + ocl.Variable.getValueAsColumn(val);
     end
   end
 end
diff --git a/Core/OclDaeHandler.m b/+ocl/DaeHandler.m
similarity index 58%
rename from Core/OclDaeHandler.m
rename to +ocl/DaeHandler.m
index 3a66220d..2c42f3a0 100644
--- a/Core/OclDaeHandler.m
+++ b/+ocl/DaeHandler.m
@@ -1,4 +1,4 @@
-classdef OclDaeHandler < handle
+classdef DaeHandler < handle
   
   properties
     ode
@@ -7,20 +7,20 @@
   
   methods
     
-    function self = OclDaeHandler()
+    function self = DaeHandler()
       self.ode = struct;
       self.alg = [];
     end
   
      function setODE(self,id,eq)
       if isfield(self.ode, id)
-        oclException(['Ode for var ', id, ' already defined']);
+        ocl.utils.exception(['Ode for var ', id, ' already defined']);
       end
-      self.ode.(id) = Variable.getValueAsColumn(eq);
+      self.ode.(id) = ocl.Variable.getValueAsColumn(eq);
     end
 
     function setAlgEquation(self,eq)
-      self.alg = [self.alg;Variable.getValueAsColumn(eq)];
+      self.alg = [self.alg;ocl.Variable.getValueAsColumn(eq)];
     end
     
     function r = getOde(self, nx, statesOrder)
@@ -29,7 +29,7 @@ function setAlgEquation(self,eq)
       for k=1:length(statesOrder)
         id = statesOrder{k};
         if ~isfield(self.ode,id)
-          oclException(['Ode for state ', id, ' not defined.']);
+          ocl.utils.exception(['Ode for state ', id, ' not defined.']);
         end
         r{k} = self.ode.(id);
         self.ode = rmfield(self.ode, id);
@@ -37,19 +37,19 @@ function setAlgEquation(self,eq)
       r = vertcat(r{:});
       
       if length(r) ~= nx
-        oclException(['Number of ode equations does not match ',...
+        ocl.utils.exception(['Number of ode equations does not match ',...
                       'number of state variables.']);
       end
       
       if numel(fieldnames(self.ode)) > 0
-        oclException(['ODE for variables defined that do not exist.']);
+        ocl.utils.exception('ODE for variables defined that do not exist.');
       end
     end
     
     function alg = getAlg(self, nz)
       alg = self.alg;
       if length(alg) ~= nz
-        oclException(['Number of algebraic equations does not match ',...
+        ocl.utils.exception(['Number of algebraic equations does not match ',...
                       'number of algebraic variables.']);
       end
     end
diff --git a/+ocl/Options.m b/+ocl/Options.m
deleted file mode 100644
index 34da9920..00000000
--- a/+ocl/Options.m
+++ /dev/null
@@ -1,9 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-function r = Options(varargin)
-  ocl.utils.checkStartup()
-  oclDeprecation('ocl.Options will be removed in future versions.')
-  r = OclOptions(varargin{:});
-end
\ No newline at end of file
diff --git a/+ocl/Pointcost.m b/+ocl/Pointcost.m
deleted file mode 100644
index 5cabe178..00000000
--- a/+ocl/Pointcost.m
+++ /dev/null
@@ -1,17 +0,0 @@
-classdef Pointcost < handle
-
-  properties
-    point
-    fh
-  end
-  
-  methods
-  
-    function self = Pointcost(point, fh)
-      self.point = point;
-      self.fh = fh;
-    end
-    
-  end
-  
-end
\ No newline at end of file
diff --git a/+ocl/README.md b/+ocl/README.md
index f6323039..50e18241 100644
--- a/+ocl/README.md
+++ b/+ocl/README.md
@@ -1,4 +1,3 @@
-This folder contains the public API that can be called by the user. At the moment
-these function are wrappers to the Core functions. Eventually direct call to
-the Core functions will be deprecated, and only the API in the +ocl package
-should be called.
+The main package folder +ocl contains the public API that is seen by the user. 
+Function in the subfolders (subpackages) are usually not see by the users of OpenOCL 
+with the exception of the examples.
\ No newline at end of file
diff --git a/+ocl/Simulator.m b/+ocl/Simulator.m
index 541ad716..d3d245eb 100644
--- a/+ocl/Simulator.m
+++ b/+ocl/Simulator.m
@@ -2,8 +2,189 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function varargout = Simulator(varargin)
-  ocl.utils.checkStartup()
-  varargout = cell(nargout,1);
-  [varargout{:}] = OclSimulator(varargin{:});
-end
\ No newline at end of file
+classdef Simulator < handle
+
+  properties
+    integrator
+    icfun
+    daefun
+    
+    x_struct
+    z_struct
+    u_struct
+    p_struct
+
+    current_state
+    algebraic_guess
+    parameters
+  end
+
+  methods
+
+    function self = Simulator(varargin)
+      ocl.utils.checkStartup()
+      
+      p = ocl.utils.ArgumentParser;
+      p.addKeyword('vars', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+      p.addKeyword('dae', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+      p.addKeyword('ic', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+      
+      r = p.parse(varargin{:});
+
+      varsfh = r.vars;
+      daefh = r.dae;
+      icfh = r.ic;
+      
+      [x_struct, z_struct, u_struct, p_struct, ...
+          ~, ~, ~, ~, ...
+          x_order] = ocl.model.vars(varsfh);
+      
+      daefun = @(x,z,u,p) ocl.model.dae( ...
+        daefh, ...
+        x_struct, ...
+        z_struct, ...
+        u_struct, ...
+        p_struct, ...
+        x_order, x, z, u, p);
+      
+      icfun = @(x,p) ocl.model.ic( ...
+        icfh, ...
+        x_struct, ...
+        p_struct, ...
+        x, p);
+
+      nx = length(x_struct);
+      nz = length(z_struct);
+      nu = length(u_struct);
+      np = length(p_struct);
+
+      self.integrator = ocl.casadi.CasadiIntegrator( ...
+        nx, nz, nu, np, daefun);
+      
+      self.daefun = daefun;
+      self.icfun = icfun;
+      
+      self.x_struct = x_struct;
+      self.z_struct = z_struct;
+      self.u_struct = u_struct;
+      self.p_struct = p_struct;
+
+      self.current_state = [];
+      self.algebraic_guess = 0;
+      self.parameters = [];
+    end
+
+    function states = getStates(self)
+      states = ocl.Variable.create(self.x_struct,0);
+    end
+    
+    function z = getAlgebraicStates(self)
+      z = ocl.Variable.create(self.z_struct,0);
+    end
+    
+    function controls = getControls(self)
+      controls = ocl.Variable.create(self.u_struct,0);
+    end
+
+    function states = getParameters(self)
+      states = ocl.Variable.create(self.p_struct,0);
+    end
+
+    function [x] = reset(self,varargin)
+
+      p = ocl.utils.ArgumentParser;
+      p.addRequired('x0', @(el) isa(el, 'ocl.Variable') || isnumeric(el) );
+      p.addKeyword('parameters', [], @(el) isa(el, 'ocl.Variable') || isnumeric(el));
+
+      args = p.parse(varargin{:});
+
+      initialStates = args.x0;
+      if isnumeric(args.x0)
+        initialStates = self.getStates();
+        initialStates.set(args.x0);
+      end
+
+      params = args.parameters;
+      if isnumeric(args.parameters)
+        params = self.getParameters();
+        params.set(args.parameters);
+      end
+
+      nz = length(self.z_struct);
+
+      z = zeros(nz,1);
+      x0 = ocl.Variable.getValueAsColumn(initialStates);
+      p = ocl.Variable.getValueAsColumn(params);
+
+      if ~isempty(z)
+        [x0,z] = self.getConsistentIntitialCondition(x0,z,p);
+      end
+      
+      initialStates.set(x0);
+
+      self.current_state = x0;
+      self.algebraic_guess = z;
+      self.parameters = p;
+    end
+
+    function [states_out,algebraics_out] = step(self, controls_in, dt)
+
+      if isnumeric(controls_in)
+        controls = self.getControls();
+        controls.set(controls_in);
+      else
+        controls = controls_in;
+      end
+
+      ocl.utils.assert(~isempty(self.current_state), 'Call `initialize` before `step`.');
+
+      x = self.current_state;
+      z0 = self.algebraic_guess;
+      p = self.parameters;
+
+      u = ocl.Variable.getValueAsColumn(controls);
+
+      [x,z] = self.integrator.evaluate(x,z0,u,dt,p);
+
+      states_out = self.getStates();
+      states_out.set(x);
+
+      algebraics_out = self.getAlgebraicStates();
+      algebraics_out.set(z);
+
+      self.current_state = x;
+      self.algebraic_guess = z;
+
+    end
+
+    function [xOut,zOut] = getConsistentIntitialCondition(self,x,z,p)
+
+      xSymb  = casadi.SX.sym('x',size(x));
+      zSymb  = casadi.SX.sym('z',size(z));
+
+      ic = self.icfun(xSymb,p);
+      [~,alg] = self.daefun(xSymb,zSymb,0,p);
+
+      z = ones(size(z)) * rand;
+
+      optVars = [xSymb;zSymb];
+      x0      = [x;z];
+
+      statesErr = (xSymb-x);
+      cost = statesErr'*statesErr;
+
+      nlp    = struct('x', optVars, 'f', cost, 'g', vertcat(ic,alg));
+      solver = casadi.nlpsol('solver', 'ipopt', nlp);
+      sol    = solver('x0', x0, 'lbx', -inf, 'ubx', inf,'lbg', 0, 'ubg', 0);
+
+      sol = full(sol.x);
+      nx = size(x,1);
+
+      xOut = sol(1:nx);
+      zOut = sol(nx+1:end);
+
+    end
+
+  end
+
+end
diff --git a/+ocl/Solver.m b/+ocl/Solver.m
index 7ceb9dba..3046dffc 100644
--- a/+ocl/Solver.m
+++ b/+ocl/Solver.m
@@ -2,8 +2,207 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function varargout = Solver(varargin)
-  ocl.utils.checkStartup()
-  varargout = cell(nargout,1);
-  [varargout{:}] = OclSolver(varargin{:});
-end
\ No newline at end of file
+classdef Solver < handle
+
+  properties
+    solver
+    stageList
+  end
+
+  methods
+
+    function self = Solver(varargin)
+      % ocl.Solver(T, 'vars', @varsfun, 'dae', @daefun,
+      %            'pathcosts', @pathcostfun,
+      %            'gridcosts', @gridcostfun,
+      %            'gridconstraints', @gridconstraintsfun, casadi_options)
+      % ocl.Solver(stages, transitions, casadi_options)
+      
+      ocl.utils.checkStartup()
+
+      if nargin >= 1 && isnumeric(varargin{1}) && ( isscalar(varargin{1}) || isempty(varargin{1}) )
+        % ocl.Solver(T, 'vars', @varsfun, 'dae', @daefun,
+        %            'lagrangecost', @lagrangefun,
+        %            'pathcosts', @pathcostfun, options
+        
+        p = ocl.utils.ArgumentParser;
+
+        p.addRequired('T', @(el)isnumeric(el) || isempty(el) );
+        p.addKeyword('vars', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+        p.addKeyword('dae', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+        p.addKeyword('pathcosts', ocl.utils.zerofh, @ocl.utils.isFunHandle);
+        p.addKeyword('gridcosts', ocl.utils.zerofh, @ocl.utils.isFunHandle);
+        p.addKeyword('gridconstraints', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+        p.addKeyword('terminalcost', ocl.utils.zerofh, @ocl.utils.isFunHandle);
+        
+        p.addParameter('nlp_casadi_mx', false, @islogical);
+        p.addParameter('controls_regularization', true, @islogical);
+        p.addParameter('controls_regularization_value', 1e-6, @isnumeric);
+        
+        p.addParameter('casadi_options', ocl.casadi.CasadiOptions(), @(el) isstruct(el));
+        p.addParameter('N', 20, @isnumeric);
+        p.addParameter('d', 3, @isnumeric);
+        
+        r = p.parse(varargin{:});
+        
+        stageList = {ocl.Stage(r.T, r.vars, r.dae, r.pathcosts, r.gridcosts, r.gridconstraints, ...
+                               r.terminalcost, ...
+                               'N', r.N, 'd', r.d)};
+        transitionList = {};
+        
+        nlp_casadi_mx = r.nlp_casadi_mx;
+        controls_regularization = r.controls_regularization;
+        controls_regularization_value = r.controls_regularization_value;
+        
+        casadi_options = r.casadi_options;
+        
+      else
+        % ocl.Solver(stages, transitions, opt)
+        p = ocl.utils.ArgumentParser;
+
+        p.addKeyword('stages', {}, @(el) iscell(el) || isa(el, 'ocl.Stage'));
+        p.addKeyword('transitions', {}, @(el) iscell(el) || ishandle(el) );
+        
+        p.addParameter('nlp_casadi_mx', false, @islogical);
+        p.addParameter('controls_regularization', true, @islogical);
+        p.addParameter('controls_regularization_value', 1e-6, @isnumeric);
+        
+        p.addParameter('casadi_options', ocl.casadi.CasadiOptions(), @(el) isstruct(el));
+        
+        r = p.parse(varargin{:});
+        
+        stageList = r.stages;
+        transitionList = r.transitions;
+        
+        nlp_casadi_mx = r.nlp_casadi_mx;
+        controls_regularization = r.controls_regularization;
+        controls_regularization_value = r.controls_regularization_value;
+        
+        casadi_options = r.casadi_options;
+      end
+
+      solver = ocl.casadi.CasadiSolver(stageList, transitionList, ...
+                                       nlp_casadi_mx, controls_regularization, ...
+                                       controls_regularization_value, casadi_options);
+
+                                     
+      % set instance variables
+      self.stageList = stageList;
+      self.solver = solver;
+    end
+
+    function [sol_ass,times_ass,objective_ass,constraints_ass] = solve(self, ig)
+      % [sol, times] = solve()
+      % [sol, times] = solve(ig)
+
+      s = self.solver;
+      st_list = self.stageList;
+
+      if nargin==1
+        % ig InitialGuess
+        ig = self.solver.getInitialGuessWithUserData();
+      end
+      
+      ig_list = cell(length(st_list),1);
+      for k=1:length(st_list)
+        ig_list{k} = ig{k}.value;
+      end
+
+      [sol,times,objective,constraints] = s.solve(ig_list);
+
+      sol_ass = ocl.Assignment(sol);
+      times_ass = ocl.Assignment(times);
+      objective_ass = ocl.Assignment(objective);
+      constraints_ass = ocl.Assignment(constraints);
+
+      ocl.utils.warningNotice()
+    end
+
+    function r = timeMeasures(self)
+      r = self.solver.timeMeasures;
+    end
+
+    function ig = ig(self)
+      ig = self.getInitialGuess();
+    end
+
+    function igAssignment = getInitialGuess(self)
+      igList = self.solver.getInitialGuess();
+      igAssignment = ocl.Assignment(igList);
+    end
+    
+    function initialize(self, id, gridpoints, values, T)
+      
+      if nargin==5
+        gridpoints = gridpoints / T;
+      end
+      
+      if length(self.stageList) == 1
+        self.stageList{1}.initialize(id, gridpoints, values);
+      else
+        ocl.utils.error('For multi-stage problems, set the guess to the stages directly.')
+      end
+    end
+
+    function setParameter(self,id,varargin)
+      if length(self.stageList) == 1
+        self.stageList{1}.setParameterBounds(id, varargin{:});
+      else
+        ocl.utils.error('For multi-stage problems, set the bounds to the stages directly.')
+      end
+    end
+
+    function setBounds(self,id,varargin)
+      % setBounds(id,value)
+      % setBounds(id,lower,upper)
+      if length(self.stageList) == 1
+
+        % check if id is a state, control, algvar or parameter
+        if ocl.utils.fieldnamesContain(self.stageList{1}.x_struct.getNames(), id)
+          self.stageList{1}.setStateBounds(id, varargin{:});
+        elseif ocl.utils.fieldnamesContain(self.stageList{1}.z_struct.getNames(), id)
+          self.stageList{1}.setAlgvarBounds(id, varargin{:});
+        elseif ocl.utils.fieldnamesContain(self.stageList{1}.u_struct.getNames(), id)
+          self.stageList{1}.setControlBounds(id, varargin{:});
+        elseif ocl.utils.fieldnamesContain(self.stageList{1}.p_struct.getNames(), id)
+          self.stageList{1}.setParameterBounds(id, varargin{:});
+        else
+          ocl.utils.error(['You specified a bound for a variable that does not exist: ', id]);
+        end
+
+      else
+        ocl.utils.error('For multi-stage problems, set the bounds to the stages directly.')
+      end
+    end
+    
+    function setInitialState(self,id,value)
+      % setInitialState(id,value)
+      if length(self.stageList) == 1
+        self.stageList{1}.setInitialStateBounds(id, value);
+      else
+        ocl.utils.error('For multi-stage problems, set the bounds to the stages directly.')
+      end
+    end
+
+    function setInitialBounds(self,id,varargin)
+      % setInitialBounds(id,value)
+      % setInitialBounds(id,lower,upper)
+      if length(self.stageList) == 1
+        self.stageList{1}.setInitialStateBounds(id, varargin{:});
+      else
+        ocl.utils.error('For multi-stage problems, set the bounds to the stages directly.')
+      end
+    end
+
+    function setEndBounds(self,id,varargin)
+      % setEndBounds(id,value)
+      % setEndBounds(id,lower,upper)
+      if length(self.stageList) == 1
+        self.stageList{1}.setEndStateBounds(id, varargin{:});
+      else
+        ocl.utils.error('For multi-stage problems, set the bounds to the stages directlly.')
+      end
+    end
+
+  end
+end
diff --git a/+ocl/Stage.m b/+ocl/Stage.m
index a01e2a72..c28860a6 100644
--- a/+ocl/Stage.m
+++ b/+ocl/Stage.m
@@ -1,9 +1,197 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-function varargout = Stage(varargin)
-  ocl.utils.checkStartup()
-  varargout = cell(nargout,1);
-  [varargout{:}] = OclStage(varargin{:});
-end
\ No newline at end of file
+classdef Stage < handle
+
+  properties
+    T
+    
+    N
+    d
+    
+    H_norm
+    
+    daefh
+    pathcostsfh
+    gridcostsfh
+    gridconstraintsfh
+    terminalcostfh
+    
+    x_bounds
+
+    x0_bounds
+    xF_bounds
+    z_bounds
+    u_bounds
+    p_bounds
+    
+    nx
+    nz
+    nu
+    np
+    
+    x_struct
+    z_struct
+    u_struct
+    p_struct
+    x_order
+    
+    x_guess
+    z_guess
+    u_guess
+    p_guess
+    
+  end
+  
+  methods
+    
+    function self = Stage(T, varargin)
+      
+      ocl.utils.checkStartup()
+      
+      p = ocl.utils.ArgumentParser;
+      
+      p.addKeyword('vars', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+      p.addKeyword('dae', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+      p.addKeyword('pathcosts', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+      p.addKeyword('gridcosts', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+      p.addKeyword('gridconstraints', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+      p.addKeyword('terminalcost', ocl.utils.emptyfh, @ocl.utils.isFunHandle);
+      
+      p.addParameter('N', 20, @isnumeric);
+      p.addParameter('d', 3, @isnumeric);
+      
+      r = p.parse(varargin{:});
+      
+      varsfh = r.vars;
+      daefh = r.dae;
+      pathcostsfh = r.pathcosts;
+      gridcostsfh = r.gridcosts;  
+      gridconstraintsfh = r.gridconstraints;
+      terminalcostfh = r.terminalcost;
+      H_norm_in = r.N;
+      d_in = r.d;
+
+      % arguments consistency checks
+      ocl.utils.assert( (isscalar(T) || isempty(T)) && isreal(T), ... 
+        ['Invalid value for parameter T.', ocl.utils.docMessage()] );
+      
+      ocl.utils.assert( (isscalar(H_norm_in) || isnumeric(H_norm_in)) && isreal(H_norm_in), ...
+        ['Invalid value for parameter N.', ocl.utils.docMessage()] );
+      
+      if isscalar(H_norm_in)
+        H_norm_in = repmat(1/H_norm_in, 1, H_norm_in);
+      elseif abs(sum(H_norm_in)-1) > 1e-6 
+        H_norm_in = H_norm_in/sum(H_norm_in);
+        ocl.utils.warning(['Timesteps given in pararmeter N are not normalized! ', ...
+                    'N either be a scalar value or a normalized vector with the length ', ...
+                    'of the number of control grid. Check the documentation of N. ', ...
+                    'Make sure the timesteps sum up to 1, and contain the relative ', ...
+                    'length of the timesteps. OpenOCL normalizes the timesteps and proceeds.']);
+      end
+      
+      [x_struct, z_struct, u_struct, p_struct, ...
+          x_bounds_v, z_bounds_v, u_bounds_v, p_bounds_v, ...
+          x_order] = ocl.model.vars(varsfh);
+      
+      self.T = T;
+      self.H_norm = H_norm_in;
+      self.N = length(H_norm_in);
+      self.d = d_in;
+      
+      self.daefh = daefh;
+      self.pathcostsfh = pathcostsfh;
+      self.gridcostsfh = gridcostsfh;
+      self.gridconstraintsfh = gridconstraintsfh;
+      self.terminalcostfh = terminalcostfh;
+      
+      self.nx = length(x_struct);
+      self.nz = length(z_struct);
+      self.nu = length(u_struct);
+      self.np = length(p_struct);
+      
+      self.x_struct = x_struct;
+      self.z_struct = z_struct;
+      self.u_struct = u_struct;
+      self.p_struct = p_struct;
+      self.x_order = x_order;
+      
+      self.x_bounds = x_bounds_v;
+      self.x0_bounds = ocl.types.Bounds();
+      self.xF_bounds = ocl.types.Bounds();
+      self.z_bounds = z_bounds_v;
+      self.u_bounds = u_bounds_v;
+      self.p_bounds = p_bounds_v;
+      
+      self.x_guess = ocl.types.InitialGuess(x_struct);
+      self.z_guess = ocl.types.InitialGuess(z_struct);
+      self.u_guess = ocl.types.InitialGuess(u_struct);
+      self.p_guess = ocl.types.InitialGuess(p_struct);
+    end
+    
+    %%% Initial guess
+    function initialize(self, id, points, values, T)
+      % setGuess(id, points, values)
+      
+      points = ocl.Variable.getValue(points);
+      values = ocl.Variable.getValue(values);
+      
+      if nargin==5
+        points = points / T;
+      end
+
+      % check if id is a state, control, algvar or parameter
+      if ocl.utils.fieldnamesContain(self.x_struct.getNames(), id)
+        self.setStateGuess(id, points, values);
+      elseif ocl.utils.fieldnamesContain(self.z_struct.getNames(), id)
+        self.setAlgvarGuess(id, points, values);
+      elseif ocl.utils.fieldnamesContain(self.u_struct.getNames(), id)
+        self.setControlGuess(id, points, values);
+      elseif ocl.utils.fieldnamesContain(self.p_struct.getNames(), id)
+        self.setParameterGuess(id, points, values);
+      else
+        ocl.utils.warning(['You specified a guess for a variable that does not exist: ', id]);
+      end
+      
+    end
+    
+    function setStateGuess(self, id, points, values)
+      self.x_guess.set(id, points, values);
+    end
+    
+    function setAlgvarGuess(self, id, points, values)
+      self.z_guess.set(id, points, values);
+    end
+    
+    function setControlGuess(self, id, points, values)
+      self.u_guess.set(id, points, values);
+    end
+    
+    function setParameterGuess(self, id, points, values)
+      self.p_guess.set(id, points, values);
+    end
+    
+    %%% Bounds
+    function setStateBounds(self, id, varargin)
+      self.x_bounds.set(id, varargin{:});
+    end
+    
+    function setInitialStateBounds(self, id, varargin)
+      self.x0_bounds.set(id, varargin{:});
+    end
+    
+    function setEndStateBounds(self, id, varargin)
+      self.xF_bounds.set(id, varargin{:});
+    end
+    
+    function setAlgvarBounds(self, id, varargin)
+      self.z_bounds.set(id, varargin{:});
+    end
+    
+    function setControlBounds(self, id, varargin)
+      self.u_bounds.set(id, varargin{:});
+    end
+    
+    function setParameterBounds(self, id, varargin)
+      self.p_bounds.set(id, varargin{:});
+    end
+    
+  end
+end
diff --git a/+ocl/System.m b/+ocl/System.m
deleted file mode 100644
index 52d18de9..00000000
--- a/+ocl/System.m
+++ /dev/null
@@ -1,9 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-function varargout = System(varargin)
-  ocl.utils.checkStartup()
-  varargout = cell(nargout,1);
-  [varargout{:}] = OclSystem(varargin{:});
-end
\ No newline at end of file
diff --git a/Core/OclSysvarsHandler.m b/+ocl/VarHandler.m
similarity index 58%
rename from Core/OclSysvarsHandler.m
rename to +ocl/VarHandler.m
index 6582193d..0ad9eced 100644
--- a/Core/OclSysvarsHandler.m
+++ b/+ocl/VarHandler.m
@@ -1,32 +1,34 @@
-classdef OclSysvarsHandler < handle
+classdef VarHandler < handle
   
   properties
-    states
-    algvars
-    parameters
-    controls
+    x_struct
+    z_struct
+    u_struct
+    p_struct
     
-    stateBounds
-    algvarBounds
-    controlBounds
-    parameterBounds
+    x_bounds
+    z_bounds
+    u_bounds
+    p_bounds
     
-    statesOrder
+    x_order
   end
   
   methods
     
-    function self = OclSysvarsHandler()
-      self.statesOrder = {};
-      self.states = OclStructure();
-      self.algvars = OclStructure();
-      self.controls = OclStructure();
-      self.parameters = OclStructure();
+    function self = VarHandler()
+      
+      self.x_struct = ocl.types.Structure();
+      self.z_struct = ocl.types.Structure();
+      self.u_struct = ocl.types.Structure();
+      self.p_struct = ocl.types.Structure();
             
-      self.stateBounds = struct;
-      self.algvarBounds = struct;
-      self.controlBounds = struct;
-      self.parameterBounds = struct;
+      self.x_bounds = ocl.types.Bounds();
+      self.z_bounds = ocl.types.Bounds();
+      self.u_bounds = ocl.types.Bounds();
+      self.p_bounds = ocl.types.Bounds();
+      
+      self.x_order = {};
     end
     
     function addState(self,id,varargin)
@@ -43,10 +45,10 @@ function addState(self,id,varargin)
 
       id = p.Results.id;
 
-      self.states.add(id, p.Results.s);
-      self.stateBounds.(id) = OclBounds(p.Results.lb, p.Results.ub);
+      self.x_struct.add(id, p.Results.s);
+      self.x_bounds.set(id, p.Results.lb, p.Results.ub);
       
-      self.statesOrder{end+1} = id;
+      self.x_order{end+1} = id;
       
     end
     function addAlgVar(self,id,varargin)
@@ -62,8 +64,8 @@ function addAlgVar(self,id,varargin)
 
       id = p.Results.id;
 
-      self.algvars.add(id, p.Results.s);
-      self.algvarBounds.(id) = OclBounds(p.Results.lb, p.Results.ub);
+      self.z_struct.add(id, p.Results.s);
+      self.z_bounds.set(id, p.Results.lb, p.Results.ub);
     end
     function addControl(self,id,varargin)
       % addControl(id)
@@ -78,8 +80,8 @@ function addControl(self,id,varargin)
 
       id = p.Results.id;
 
-      self.controls.add(id,p.Results.s);
-      self.controlBounds.(id) = OclBounds(p.Results.lb, p.Results.ub);
+      self.u_struct.add(id,p.Results.s);
+      self.u_bounds.set(id, p.Results.lb, p.Results.ub);
     end
     function addParameter(self,id,varargin)
       % addParameter(id)
@@ -93,8 +95,8 @@ function addParameter(self,id,varargin)
 
       id = p.Results.id;
 
-      self.parameters.add(id,p.Results.s);
-      self.parameterBounds.(id) = OclBounds(p.Results.default);
+      self.p_struct.add(id,p.Results.s);
+      self.p_bounds.set(id, p.Results.default);
     end
   end
 end
\ No newline at end of file
diff --git a/Core/Variables/Variable/Variable.m b/+ocl/Variable.m
old mode 100755
new mode 100644
similarity index 62%
rename from Core/Variables/Variable/Variable.m
rename to +ocl/Variable.m
index a1195a2f..50764b91
--- a/Core/Variables/Variable/Variable.m
+++ b/+ocl/Variable.m
@@ -24,60 +24,58 @@
     %%% factory methods
     function var = create(type,value)
       if isnumeric(value)
-        var = Variable.createNumeric(type,value);
+        var = ocl.Variable.createNumeric(type,value);
       elseif isa(value,'casadi.MX') || isa(value,'casadi.SX')
-        var = CasadiVariable.createFromValue(type,value);
+        var = ocl.casadi.CasadiVariable.createFromValue(type,value);
       else
-        oclError('Not implemented for this type of variable.')
+        ocl.utils.error('Not implemented for this type of variable.')
       end
     end
     
     function var = createFromVar(type,pos,var)
-      if isa(var, 'CasadiVariable')
-        var = CasadiVariable(type,pos,var.mx,var.val);
-      elseif isa(var,'SymVariable')
-        var = SymVariable(type,pos,var.val);
+      if isa(var, 'ocl.casadi.CasadiVariable')
+        var = ocl.casadi.CasadiVariable(type,pos,var.mx,var.val);
       else
-        var = Variable(type,pos,var.val);
+        var = ocl.Variable(type,pos,var.val);
       end
     end
 
     function obj = Matrix(value)
       % obj = createMatrixLike(input,value)
-      t = OclMatrix(size(value));
-      obj = Variable.create(t,value);
+      t = ocl.types.Matrix(size(value));
+      obj = ocl.Variable.create(t,value);
     end
     
     function var = createNumeric(type,value)
         [N,M,K] = type.size();
-        v = OclValue(zeros(1,N,M,K));
+        v = ocl.types.Value(zeros(1,N,M,K));
         p = reshape(1:N*M*K,N,M,K);
-        var = Variable(type,p,v);
+        var = ocl.Variable(type,p,v);
         var.set(value);
     end
     
     function v = createFromHandleOne(fh, a, varargin)
-      a = Variable.getValue(a);
-      v = Variable.Matrix( fh(a, varargin{:}) );
+      a = ocl.Variable.getValue(a);
+      v = ocl.Variable.Matrix( fh(a, varargin{:}) );
     end
     
     function v = createFromHandleTwo(fh, a, b, varargin)
-      a = Variable.getValue(a);
-      b = Variable.getValue(b);
+      a = ocl.Variable.getValue(a);
+      b = ocl.Variable.getValue(b);
       if isnumeric(a) && ((isa(b,'casadi.SX')||isa(b,'casadi.MX')))
         a = casadi.DM(a);  
       end
-      v = Variable.Matrix(fh(a,b,varargin{:}));
+      v = ocl.Variable.Matrix(fh(a,b,varargin{:}));
     end
     %%% end factory methods   
     function value = getValue(value)
-      if isa(value,'Variable')
+      if isa(value,'ocl.Variable')
         value = value.value;
       end
     end
     
     function value = getValueAsColumn(value)
-      value = Variable.getValue(value);
+      value = ocl.Variable.getValue(value);
       value = value(:);
     end
   end % methods(static)
@@ -85,9 +83,9 @@
   methods
     function self = Variable(type,positions,val)
       narginchk(3,3);
-      assert(isa(type,'OclStructure'));
+      assert(isa(type,'ocl.types.Structure'));
       assert(isnumeric(positions));
-      assert(isa(val,'OclValue'));
+      assert(isa(val,'ocl.types.Value'));
       self.type = type;
       self.positions = positions;
       self.val = val;
@@ -97,7 +95,7 @@
       if nargin==1
         value = self.value;
         if isnumeric(value)
-          valueStr = mat2str(self.value,Variable.DISP_FLOAT_PREC);
+          valueStr = mat2str(self.value, ocl.Variable.DISP_FLOAT_PREC);
         else
           % cell array
           cell2str = cellfun(@(v)[mat2str(v),','],value, 'UniformOutput',false);
@@ -107,8 +105,8 @@
       end
       
       dotsStr = '';
-      if numel(valueStr) >= Variable.MAX_DISP_LENGTH 
-        valueStr = valueStr(1:Variable.MAX_DISP_LENGTH);
+      if numel(valueStr) >= ocl.Variable.MAX_DISP_LENGTH 
+        valueStr = valueStr(1:ocl.Variable.MAX_DISP_LENGTH);
         dotsStr = '...';
       end
       
@@ -174,7 +172,7 @@ function disp(self)
       elseif isempty(s)
         [varargout{1}] = self;
       else
-        oclError('Not supported.');
+        ocl.utils.error('Not supported.');
       end
     end % subsref
     
@@ -187,7 +185,7 @@ function disp(self)
       if numel(s)==1 && strcmp(s(1).type,'.') && ~isfield(self.type.children,s(1).subs)
         self = builtin('subsasgn',self,s,v);
       else
-        v = Variable.getValue(v);
+        v = ocl.Variable.getValue(v);
         subVar = subsref(self,s);
         subVar.set(v);
       end
@@ -198,7 +196,7 @@ function disp(self)
       n=1;
     end	
 
-    %%% delegate methods to OclValue
+    %%% delegate methods to ocl.types.Value
     function set(self,val,varargin)
       % set(value)
       % set(value,slice1,slice2,slice3)
@@ -225,13 +223,13 @@ function set(self,val,varargin)
     function r = get(self,id)
       % r = get(id)
       [t,p] = self.type.get(id,self.positions);
-      r = Variable.createFromVar(t,p,self);
+      r = ocl.Variable.createFromVar(t,p,self);
     end
     
     function r = slice(self,varargin)
       % r = get(dim1,dim2,dim3)
       p = self.positions(varargin{:});
-      r = Variable.createFromVar(self.type,p,self);
+      r = ocl.Variable.createFromVar(self.type,p,self);
     end
 
     function toJSON(self,path,name,varargin)
@@ -259,194 +257,194 @@ function toJSON(self,path,name,varargin)
     %%% operators
     % single argument
     function v = uplus(self)
-      v = Variable.createFromHandleOne(@(self,varargin)uplus(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)uplus(self),self);
     end
     function v = uminus(self)
-      v = Variable.createFromHandleOne(@(self,varargin)uminus(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)uminus(self),self);
     end
    
     function v = ctranspose(self)
-      oclWarning(['Complex transpose is not defined. Using matrix transpose ', ...
+      ocl.utils.warning(['Complex transpose is not defined. Using matrix transpose ', ...
                   'instead. Use the .'' operator instead on the '' operator!']);
       v = self.transpose();
     end
     function v = transpose(self)
-      v = Variable.createFromHandleOne(@(self,varargin)transpose(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)transpose(self),self);
     end
     
     function v = reshape(self,varargin)
-      v = Variable.createFromHandleOne(@(self,varargin)reshape(self,varargin{:}),self,varargin{:});
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)reshape(self,varargin{:}),self,varargin{:});
     end
     
     function v = triu(self)
-      v = Variable.createFromHandleOne(@(self,varargin)triu(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)triu(self),self);
     end
     
     function v = repmat(self,varargin)
-      v = Variable.createFromHandleOne(@(self,varargin)repmat(self,varargin{:}),self,varargin{:});
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)repmat(self,varargin{:}),self,varargin{:});
     end
     
     function v = sum(self)
-      v = Variable.createFromHandleOne(@(self,varargin)sum(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)sum(self),self);
     end
     
     function v = norm(self,varargin)
-      v = Variable.createFromHandleOne(@(self,varargin)norm(self,varargin{:}),self,varargin{:});
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)norm(self,varargin{:}),self,varargin{:});
     end
     
     function v = inv(self)
-      v = Variable.createFromHandleOne(@(self,varargin)inv(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)inv(self),self);
     end
     
     function v = det(self)
-      v = Variable.createFromHandleOne(@(self,varargin)det(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)det(self),self);
     end
     
     function v = trace(self)
-      v = Variable.createFromHandleOne(@(self,varargin)trace(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)trace(self),self);
     end
     
     function v = diag(self)
-      v = Variable.createFromHandleOne(@(self,varargin)diag(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)diag(self),self);
     end
     
     function v = abs(self)
-      v = Variable.createFromHandleOne(@(self,varargin)abs(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)abs(self),self);
     end
 
     function v = sqrt(self)
-      v = Variable.createFromHandleOne(@(self,varargin)sqrt(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)sqrt(self),self);
     end
     
     function v = sin(self)
-      v = Variable.createFromHandleOne(@(self,varargin)sin(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)sin(self),self);
     end
     
     function v = cos(self)
-      v = Variable.createFromHandleOne(@(self,varargin)cos(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)cos(self),self);
     end
     
     function v = tan(self)
-      v = Variable.createFromHandleOne(@(self,varargin)tan(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)tan(self),self);
     end
     
     function v = atan(self)
-      v = Variable.createFromHandleOne(@(self,varargin)atan(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)atan(self),self);
     end
     
     function v = asin(self)
-      v = Variable.createFromHandleOne(@(self,varargin)asin(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)asin(self),self);
     end
     
     function v = acos(self)
-      v = Variable.createFromHandleOne(@(self,varargin)acos(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)acos(self),self);
     end
     
     function v = tanh(self)
-      v = Variable.createFromHandleOne(@(self,varargin)tanh(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)tanh(self),self);
     end
     
     function v = cosh(self)
-      v = Variable.createFromHandleOne(@(self,varargin)cosh(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)cosh(self),self);
     end
     
     function v = sinh(self)
-      v = Variable.createFromHandleOne(@(self,varargin)sinh(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)sinh(self),self);
     end
     
     function v = atanh(self)
-      v = Variable.createFromHandleOne(@(self,varargin)atanh(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)atanh(self),self);
     end
     
     function v = asinh(self)
-      v = Variable.createFromHandleOne(@(self,varargin)asinh(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)asinh(self),self);
     end
     
     function v = acosh(self)
-      v = Variable.createFromHandleOne(@(self,varargin)acosh(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)acosh(self),self);
     end
     
     function v = exp(self)
-      v = Variable.createFromHandleOne(@(self,varargin)exp(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)exp(self),self);
     end
     
     function v = log(self)
-      v = Variable.createFromHandleOne(@(self,varargin)log(self),self);
+      v = ocl.Variable.createFromHandleOne(@(self,varargin)log(self),self);
     end
     
     % two arguments
     function v = mtimes(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)mtimes(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)mtimes(a,b),a,b);
     end
     
     function v = mpower(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)mpower(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)mpower(a,b),a,b);
     end
     
     function v = mldivide(a,b)
-      a = Variable.getValue(a);
-      b = Variable.getValue(b);
+      a = ocl.Variable.getValue(a);
+      b = ocl.Variable.getValue(b);
       if (numel(a) > 1) && (numel(b) > 1)
-        v = Variable.Matrix(solve(a,b));
+        v = ocl.Variable.Matrix(solve(a,b));
       else
-        v = Variable.Matrix(mldivide(a,b));
+        v = ocl.Variable.Matrix(mldivide(a,b));
       end
     end
     
     function v = mrdivide(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)mrdivide(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)mrdivide(a,b),a,b);
     end
     
     function v = cross(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)cross(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)cross(a,b),a,b);
     end
     
     function v = dot(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)dot(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)dot(a,b),a,b);
     end
     
     function v = polyval(p,a)
-      v = Variable.createFromHandleTwo(@(p,a,varargin)polyval(p,a),p,a);
+      v = ocl.Variable.createFromHandleTwo(@(p,a,varargin)polyval(p,a),p,a);
     end
     
     function v = jacobian(ex,arg)
-      v = Variable.createFromHandleTwo(@(ex,arg,varargin)jacobian(ex,arg),ex,arg);
+      v = ocl.Variable.createFromHandleTwo(@(ex,arg,varargin)jacobian(ex,arg),ex,arg);
     end
     
     function v = plus(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)plus(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)plus(a,b),a,b);
     end
     
     function v = minus(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)minus(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)minus(a,b),a,b);
     end
     
     function v = times(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)times(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)times(a,b),a,b);
     end
     
     function v = power(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)power(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)power(a,b),a,b);
     end
     
     function v = rdivide(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)rdivide(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)rdivide(a,b),a,b);
     end
     
     function v = ldivide(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)ldivide(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)ldivide(a,b),a,b);
     end
     
     function v = atan2(a,b)
-      v = Variable.createFromHandleTwo(@(a,b,varargin)atan2(a,b),a,b);
+      v = ocl.Variable.createFromHandleTwo(@(a,b,varargin)atan2(a,b),a,b);
     end
     
     % three arguments
     function r = jtimes(ex,arg,v)
-      ex = Variable.getValue(ex);
-      arg = Variable.getValue(arg);
-      v = Variable.getValue(v);
-      r = Variable.Matrix(jtimes(ex,arg,v));
+      ex = ocl.Variable.getValue(ex);
+      arg = ocl.Variable.getValue(arg);
+      v = ocl.Variable.getValue(v);
+      r = ocl.Variable.Matrix(jtimes(ex,arg,v));
     end
     
     % lists
@@ -454,18 +452,18 @@ function toJSON(self,path,name,varargin)
       N = numel(varargin);
       outValues = cell(1,N);
       for k=1:numel(varargin)
-        outValues{k} = Variable.getValue(varargin{k});
+        outValues{k} = ocl.Variable.getValue(varargin{k});
       end    
-      v = Variable.Matrix(horzcat(outValues{:}));
+      v = ocl.Variable.Matrix(horzcat(outValues{:}));
     end
     
     function v = vertcat(varargin)
       N = numel(varargin);
       outValues = cell(1,N);
       for k=1:numel(varargin)
-        outValues{k} = Variable.getValue(varargin{k});
+        outValues{k} = ocl.Variable.getValue(varargin{k});
       end
-      v = Variable.Matrix(vertcat(outValues{:}));
+      v = ocl.Variable.Matrix(vertcat(outValues{:}));
     end
     
     %%% element wise operations
@@ -474,7 +472,7 @@ function toJSON(self,path,name,varargin)
       % It is automatically renamed for Octave as properties is not 
       % allowed as a function name.
       %
-      % Tab completion in Matlab for custom variables
+      % Tab completion in Matlab for custom ocl.Variables
       n = [fieldnames(self);fieldnames(self.type.children)];	
     end
   end
diff --git a/+ocl/plot.m b/+ocl/plot.m
index 153a6ce1..bd54c6f0 100644
--- a/+ocl/plot.m
+++ b/+ocl/plot.m
@@ -2,8 +2,12 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function varargout = plot(varargin)
+function plot(x,y,varargin)
   ocl.utils.checkStartup()
-  varargout = cell(nargout,1);
-  [varargout{:}] = OclPlot(varargin{:});
+  
+  x = ocl.Variable.getValue(x);
+  y = ocl.Variable.getValue(y);
+  
+  plot(x,y,'LineWidth', 3, varargin{:})
+  
 end
\ No newline at end of file
diff --git a/+ocl/stairs.m b/+ocl/stairs.m
index 3ce2ccfd..a8278654 100644
--- a/+ocl/stairs.m
+++ b/+ocl/stairs.m
@@ -2,8 +2,11 @@
 % Redistribution is permitted under the 3-Clause BSD License terms. Please
 % ensure the above copyright notice is visible in any derived work.
 %
-function varargout = stairs(varargin)
+function stairs(x, y, varargin)
   ocl.utils.checkStartup()
-  varargout = cell(nargout,1);
-  [varargout{:}] = OclStairs(varargin{:});
+  
+  x = ocl.Variable.getValue(x);
+  y = ocl.Variable.getValue(y);
+  
+  stairs(x,y,'LineWidth', 3, varargin{:})
 end
\ No newline at end of file
diff --git a/.gitignore b/.gitignore
index 8d5f9f35..6dafb0e4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,9 +3,12 @@ jit_tmp.c
 *.asv
 *.m~
 *.mex*
+*.lib
 slprj/
 octave-workspace
 .autosave
 Workspace
 Lib/casadi
+Lib/acados
 doc/html
+build/
diff --git a/CasadiLibrary/CasadiFunction.m b/CasadiLibrary/CasadiFunction.m
deleted file mode 100755
index 53f7a2e7..00000000
--- a/CasadiLibrary/CasadiFunction.m
+++ /dev/null
@@ -1,72 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-classdef CasadiFunction < OclFunction
-  properties (Access = public)
-    casadiFun
-    numericOutputIndizes
-    numericOutputValues
-    mx
-  end
-  
-  methods
-    
-    function self = CasadiFunction(fun, jit, mx)
-      % CasadiFunction(function,jit)
-      % CasadiFunction(userFunction,jit)
-      self = self@OclFunction(fun.obj,fun.functionHandle,fun.inputSizes,fun.nOutputs);
-      
-      if isa(fun,'CasadiFunction')
-        self = fun;
-        return
-      end
-      
-      if nargin == 1
-        jit = false;
-        self.mx = false;
-      elseif nargin == 2
-        self.mx = false;
-      else
-        self.mx = mx;
-      end
-      
-      nInputs = length(self.inputSizes);
-      inputs = cell(1,nInputs);
-      for k=1:nInputs
-        s = self.inputSizes{k};
-        assert(length(s)==2 || s(3)==1)
-        if self.mx
-          inputs{k} = casadi.MX.sym('v',s(1:2));
-        else
-          inputs{k} = casadi.SX.sym('v',s(1:2));
-        end
-      end
-      
-      outputs = cell(1,self.nOutputs);
-      [outputs{:}] = fun.evaluate(inputs{:});
-      
-      % check for numeric/constant outputs
-      self.numericOutputIndizes = logical(cellfun(@isnumeric,outputs));
-      self.numericOutputValues = outputs(self.numericOutputIndizes);
-      
-      self.casadiFun = casadi.Function('fun',inputs,outputs,struct('jit',jit));
-    end
-    
-    function varargout = evaluate(self,varargin)
-      % evaluate casadi function
-      varargout = cell(1,self.nOutputs);
-      [varargout{:}] = self.casadiFun(varargin{:});
-      
-      % replace numerical outputs
-      varargout(self.numericOutputIndizes) = self.numericOutputValues;
-      
-      for k=1:length(varargout)
-        if isa(varargout{k},'casadi.DM')
-          varargout{k} = full(varargout{k});
-        end
-      end
-    end   
-  end
-end
-
diff --git a/CasadiLibrary/CasadiMapFunction.m b/CasadiLibrary/CasadiMapFunction.m
deleted file mode 100644
index 66da7040..00000000
--- a/CasadiLibrary/CasadiMapFunction.m
+++ /dev/null
@@ -1,41 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-classdef CasadiMapFunction < handle
-  
-  properties
-    N
-    map_fcn
-    numericOutputIndizes
-    numericOutputValues
-  end
-  
-  methods
-
-    function self = CasadiMapFunction(casadi_fcn, N)
-      self.N = N;
-      self.map_fcn = casadi_fcn.casadiFun.map(N,'openmp');
-      self.numericOutputIndizes = casadi_fcn.numericOutputIndizes;
-      
-      for i=1:numel(casadi_fcn.numericOutputValues)
-        % repmat output values
-        self.numericOutputValues{i}  = repmat(casadi_fcn.numericOutputValues{i},1,N);
-      end
-    end
-    
-    function varargout = evaluate(self,varargin)
-      varargout = cell(nargout,1);
-      [varargout{:}] = self.map_fcn(varargin{:});
-      
-      varargout(self.numericOutputIndizes) = self.numericOutputValues;
-      
-      for k=1:length(varargout)
-        if isa(varargout{k},'casadi.DM')
-          varargout{k} = full(varargout{k});
-        end
-      end
-
-    end
-  end
-end
\ No newline at end of file
diff --git a/CasadiLibrary/CasadiSolver.m b/CasadiLibrary/CasadiSolver.m
deleted file mode 100644
index 2f33f479..00000000
--- a/CasadiLibrary/CasadiSolver.m
+++ /dev/null
@@ -1,203 +0,0 @@
-classdef CasadiSolver < handle
-  
-  properties
-    timeMeasures
-    stageList
-    nlpData
-    gridpoints
-    gridpoints_integrator
-  end
-  
-  properties (Access = private)
-    controls_regularization
-    controls_regularization_value
-  end
-  
-  methods
-    
-    function self = CasadiSolver(stageList, transitionList, ...
-                                 nlp_casadi_mx, ...
-                                 controls_regularization, controls_regularization_value, ...
-                                 casadi_options)
-      
-      oclAssert(length(stageList)==length(transitionList)+1, ...
-                'You need to specify Ns-1 transitions for Ns stages.');
-              
-      self.controls_regularization = controls_regularization;
-      self.controls_regularization_value = controls_regularization_value;
-      
-      constructTotalTic = tic;
-      
-      % create variables as casadi symbolics
-      if nlp_casadi_mx
-        expr = @casadi.MX.sym;
-      else
-        expr = @casadi.SX.sym;
-      end
-      
-      vars = cell(length(stageList), 1);
-      costs = cell(length(stageList), 1);
-      constraints = cell(length(stageList), 1);
-      constraints_LB = cell(length(stageList), 1);
-      constraints_UB = cell(length(stageList), 1);
-      
-      gridpoints = cell(length(stageList), 1);
-      gridpoints_integrator = cell(length(stageList), 1);
-      
-      v_stage = [];
-      
-      for k=1:length(stageList)
-        stage = stageList{k};
-        
-        x = expr(['x','_s',mat2str(k)], stage.nx);
-        vi = expr(['vi','_s',mat2str(k)], stage.integrator.num_i);
-        u = expr(['u','_s',mat2str(k)], stage.nu);
-        h = expr(['h','_s',mat2str(k)]);
-        p = expr(['p','_s',mat2str(k)], stage.np);
-        
-        [statesEnd, cost_integr, equations, rel_times] = stage.integrator.integratorfun(x, vi, u, h, p);
-        integrator_fun = casadi.Function('sys', {x,vi,u,h,p}, {statesEnd, cost_integr, equations, rel_times});
-        
-        stage.integratormap = integrator_fun.map(stage.N,'serial');
-        
-        nv_stage = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.num_i, stage.nu, stage.np);
-        v_last_stage = v_stage;
-        v_stage = expr(['v','_s',mat2str(k)], nv_stage);
-          
-        [costs_stage,constraints_stage,constraints_LB_stage, ...
-          constraints_UB_stage] = ocl.simultaneous.equations(stage, v_stage, ...
-              controls_regularization, controls_regularization_value);
-        
-        transition_eq = [];
-        transition_lb = [];
-        transition_ub = [];
-        if k >= 2
-          x0s = ocl.simultaneous.getFirstState(stageList{k}, v_stage);
-          xfs = ocl.simultaneous.getLastState(stageList{k-1}, v_last_stage);
-          transition_fun = transitionList{k-1};
-          tansition_handler = OclConstraint();
-          
-          x0 = Variable.create(stageList{k}.states, x0s);
-          xf = Variable.create(stageList{k-1}.states, xfs);
-          
-          transition_fun(tansition_handler,x0,xf);
-          
-          transition_eq = tansition_handler.values;
-          transition_lb = tansition_handler.lowerBounds;
-          transition_ub = tansition_handler.upperBounds;
-        end
-        
-        vars{k} = v_stage;
-        costs{k} = costs_stage;
-        constraints{k} = vertcat(transition_eq, constraints_stage);
-        constraints_LB{k} = vertcat(transition_lb, constraints_LB_stage);
-        constraints_UB{k} = vertcat(transition_ub, constraints_UB_stage);
-        
-        gridpoints_integrator{k} = ocl.simultaneous.normalizedIntegratorTimes(stage);
-        gridpoints{k} = ocl.simultaneous.normalizedStateTimes(stage);
-      end
-      
-      v = vertcat(vars{:});
-      costs = sum([costs{:}]);
-      constraints = vertcat(constraints{:});
-      constraints_LB = vertcat(constraints_LB{:});
-      constraints_UB = vertcat(constraints_UB{:});
-      
-      % get struct with nlp for casadi
-      casadiNLP = struct;
-      casadiNLP.x = v;
-      casadiNLP.f = costs;
-      casadiNLP.g = constraints;
-      casadiNLP.p = [];
-      
-      constructSolverTic = tic;
-      casadiSolver = casadi.nlpsol('my_solver', 'ipopt', casadiNLP, casadi_options);
-      constructSolverTime = toc(constructSolverTic);
-
-      nlpData = struct;
-      nlpData.casadiNLP = casadiNLP;
-      nlpData.constraints_LB = constraints_LB;
-      nlpData.constraints_UB = constraints_UB;
-      nlpData.solver = casadiSolver;
-      
-      timeMeasures.constructTotal = toc(constructTotalTic);
-      timeMeasures.constructSolver = constructSolverTime;
-      
-      self.stageList = stageList;
-      self.nlpData = nlpData;
-      self.timeMeasures = timeMeasures;
-      self.gridpoints = gridpoints;
-      self.gridpoints_integrator = gridpoints_integrator;
-    end
-    
-    function [sol,times,objective,constraints] = solve(self,v0)
-      % solve(initialGuess)
-      
-      solveTotalTic = tic;
-      
-      pl = self.stageList;
-      uregu = self.controls_regularization;
-      uregu_value = self.controls_regularization_value;
-      
-      lbv = cell(length(pl),1);
-      ubv = cell(length(pl),1);
-      for k=1:length(pl)
-        [lbv_stage,ubv_stage] = ocl.simultaneous.getBounds(pl{k});
-        lbv{k} = lbv_stage;
-        ubv{k} = ubv_stage;
-      end
-      
-      v0 = vertcat(v0{:});
-      lbv = vertcat(lbv{:});
-      ubv = vertcat(ubv{:});
- 
-      args = struct;
-      args.lbg = self.nlpData.constraints_LB;
-      args.ubg = self.nlpData.constraints_UB;
-      args.p = [];
-      args.lbx = lbv;
-      args.ubx = ubv;
-      args.x0 = v0;
-      
-      % execute solver
-      solveCasadiTic = tic;
-      sol = self.nlpData.solver.call(args);
-      solveCasadiTime = toc(solveCasadiTic);
-      
-      if strcmp(self.nlpData.solver.stats().return_status, 'NonIpopt_Exception_Thrown')
-        oclWarning('Solver was interrupted by user.');
-      end
-      
-      sol_values = sol.x.full();
-      
-      sol = cell(length(pl),1);
-      i = 1;
-      for k=1:length(pl)
-        stage = pl{k};
-        nv_stage = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.num_i, stage.nu, stage.np);
-        sol{k} = sol_values(i:i+nv_stage-1);
-        i = i + nv_stage;
-      end
-      
-      nlpFunEvalTic = tic;
-      times = cell(length(pl),1);
-      objective = cell(length(pl),1);
-      constraints = cell(length(pl),1);
-      for k=1:length(pl)
-        stage = pl{k};
-        [objective{k},constraints{k},~,~,times{k}] = ocl.simultaneous.equations(stage, sol{k}, ...
-                                                                                uregu, ...
-                                                                                uregu_value);
-        objective{k} = full(objective{k});
-        constraints{k} = full(constraints{k});
-        times{k} = full(times{k});
-      end
-      nlpFunEvalTime = toc(nlpFunEvalTic);
-      
-      self.timeMeasures.solveTotal      = toc(solveTotalTic);
-      self.timeMeasures.solveCasadi     = solveCasadiTime;
-      self.timeMeasures.nlpFunEval      = nlpFunEvalTime;
-    end
-  end
-  
-end
diff --git a/Core/NLPSolver.m b/Core/NLPSolver.m
deleted file mode 100755
index 906c7083..00000000
--- a/Core/NLPSolver.m
+++ /dev/null
@@ -1,60 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-classdef NLPSolver < handle
-
-  properties
-    nlp
-    timeMeasures
-  end
-  
-  methods
-    
-    function self = NLPSolver()
-      self.timeMeasures = struct;
-    end
-    
-    function solve(~,varargin)
-      oclError('Not implemented. Call CasadiNLPSolver instead.');
-    end
-    
-    function ig = ig(self)
-      ig = self.getInitialGuess();
-    end
-    
-    function ig = getInitialGuess(self)
-      igTic = tic;
-      ig = self.nlp.getInitialGuess();
-      self.timeMeasures.initialGuess = toc(igTic);
-    end
-    
-    function setParameter(self, id, value)
-      % setParameter(id,value)
-      self.nlp.setParameter(id, value)
-    end
-    
-    function setBounds(self,varargin)
-      % setInitialBounds(id,value)
-      % setInitialBounds(id,lower,upper)
-      self.nlp.setBounds(varargin{:})
-    end
-    
-    function setInitialBounds(self,varargin)
-      % setInitialBounds(id,value)
-      % setInitialBounds(id,lower,upper)
-      self.nlp.setInitialBounds(varargin{:})
-    end
-    
-    function setEndBounds(self,varargin)
-      % setEndBounds(id,value)
-      % setEndBounds(id,lower,upper)
-      self.nlp.setEndBounds(varargin{:})
-    end    
-    
-    function solutionCallback(self,times,solution)
-      self.nlp.system.solutionCallback(times,solution);
-    end
-    
-  end
-end
diff --git a/Core/OclBounds.m b/Core/OclBounds.m
deleted file mode 100644
index 39b51cf6..00000000
--- a/Core/OclBounds.m
+++ /dev/null
@@ -1,15 +0,0 @@
-function b = OclBounds(varargin)
-  b = struct;
-  b.lower = -inf;
-  b.upper = inf;
-  
-  if nargin >= 1
-    b.lower = varargin{1};
-    b.upper = varargin{1};
-  end
-  
-  if nargin >= 2
-    b.upper = varargin{2};
-  end
-end
-
diff --git a/Core/OclCollocation.m b/Core/OclCollocation.m
deleted file mode 100644
index f366d404..00000000
--- a/Core/OclCollocation.m
+++ /dev/null
@@ -1,227 +0,0 @@
-% This class is derived from:
-%
-% An implementation of direct collocation
-% Joel Andersson, 2016
-% https://github.com/casadi/casadi/blob/master/docs/examples/matlab/direct_collocation.m
-%
-% CasADi -- A symbolic framework for dynamic optimization.
-% Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,
-%                         K.U. Leuven. All rights reserved.
-% Copyright (C) 2011-2014 Greg Horn
-% Under GNU Lesser General Public License
-
-classdef OclCollocation < handle
-
-  properties
-
-    states
-    algvars
-    controls
-    parameters
-
-    daefun
-    pathcostsfh
-
-    integratorBounds
-    stateBounds
-    algvarBounds
-
-    vars
-    num_x
-    num_z
-    num_u
-    num_p
-    num_t
-
-    num_i
-    
-    coefficients
-    coeff_eval
-    coeff_der
-    coeff_int
-    tau_root
-    order
-  end
-
-  methods
-
-    function self = OclCollocation(states, algvars, controls, parameters, daefun, pathcostsfh, order, ...
-                                   stateBounds, algvarBounds)
-
-      self.states = states;
-      self.algvars = algvars;
-      self.controls = controls;
-      self.parameters = parameters;
-
-      nx = prod(states.size());
-      nz = prod(algvars.size());
-      nu = prod(controls.size());
-      np = prod(parameters.size());
-      nt = order;
-
-      self.daefun = daefun;
-      self.pathcostsfh = pathcostsfh;
-      
-      tau = [0 ocl.collocation.collocationPoints(order)];
-      
-      coeff = ocl.collocation.coefficients(tau);
-      self.coeff_eval = ocl.collocation.evalCoefficients(coeff, order, 1.0);
-      self.coeff_der = ocl.collocation.evalCoefficientsDerivative(coeff, tau, order);
-      self.coeff_int = ocl.collocation.evalCoefficientsIntegral(coeff, order, 1.0);
-      
-      self.vars = OclStructure();
-      self.vars.addRepeated({'states', 'algvars'},...
-                            {states, algvars}, order);
-
-      si = self.vars.size();
-      ni = prod(si);
-
-      self.integratorBounds = OclBounds(-inf * ones(ni, 1), inf * ones(ni, 1));
-      self.stateBounds = OclBounds(-inf * ones(nx, 1), inf * ones(nx, 1));
-      self.algvarBounds = OclBounds(-inf * ones(nz, 1), inf * ones(nz, 1));
-
-      names = fieldnames(stateBounds);
-      for k=1:length(names)
-        id = names{k};
-        self.setStateBounds(id, stateBounds.(id).lower, stateBounds.(id).upper);
-      end
-
-      names = fieldnames(algvarBounds);
-      for k=1:length(names)
-        id = names{k};
-        self.setAlgvarBounds(id, algvarBounds.(id).lower, algvarBounds.(id).upper);
-      end
-      
-      self.num_x = nx;
-      self.num_z = nz;
-      self.num_u = nu;
-      self.num_p = np;
-      self.num_t = nt;
-      self.num_i = ni;
-      self.coefficients = coeff;
-      self.tau_root = tau;
-      self.order = order;
-    end
-
-    function [xF, costs, equations, rel_times] = ...
-          integratorfun(self, x0, vars, u, h, params)
-
-      C = self.coeff_der;
-      B = self.coeff_int;
-      
-      tau = self.tau_root;
-      d = self.order;
-      
-      nx = self.num_x;
-      nz = self.num_z;
-        
-      equations = cell(d,1);
-      J = 0;
-      
-      [x_indizes, z_indizes] = ocl.collocation.indizes(nx,nz,d);
-
-      % Loop over collocation points
-      rel_times = cell(d,1);
-      for j=1:d
-        
-        x_der = C(1,j+1)*x0;
-        for r=1:d
-          x_r = vars(x_indizes(:,r));
-          x_der = x_der + C(r+1,j+1)*x_r;
-        end
-        
-        x_j = vars(x_indizes(:,j));
-        z_j = vars(z_indizes(:,j));
-
-        [ode,alg] = self.daefun(x_j, z_j, u, params);
-
-        qj = self.pathcostfun(x_j, z_j,u,params);
-        
-        equations{j} = [h*ode-x_der; alg];
-        J = J + B(j+1)*qj*h;
-        
-        rel_times{j} = tau(j+1) * h;
-      end
-
-      costs = J;
-      equations = vertcat(equations{:});
-      rel_times = vertcat(rel_times{:});
-      
-      xF = ocl.collocation.getStateAtPoint(self, x0, vars, 1.0);
-      
-    end
-
-    function r = getInitialGuess(self, stateGuess, algvarGuess)
-      ig = Variable.create(self.vars, 0);
-      ig.states.set(stateGuess);
-      ig.algvars.set(algvarGuess);
-      r = ig.value;
-    end
-
-    function setStateBounds(self,id,varargin)
-      % integrator
-      lb = Variable.create(self.vars, self.integratorBounds.lower);
-      ub = Variable.create(self.vars, self.integratorBounds.upper);
-
-      bounds = OclBounds(varargin{:});
-
-      lb.get('states').get(id).set(bounds.lower);
-      ub.get('states').get(id).set(bounds.upper);
-
-      self.integratorBounds.lower = lb.value;
-      self.integratorBounds.upper = ub.value;
-      
-      % states
-      x_lb = Variable.create(self.states, self.stateBounds.lower);
-      x_ub = Variable.create(self.states, self.stateBounds.upper);
-
-      bounds = OclBounds(varargin{:});
-
-      x_lb.get(id).set(bounds.lower);
-      x_ub.get(id).set(bounds.upper);
-
-      self.stateBounds.lower = x_lb.value;
-      self.stateBounds.upper = x_ub.value;
-    end
-
-    function setAlgvarBounds(self,id,varargin)
-      % integrator vars 
-      lb = Variable.create(self.vars, self.integratorBounds.lower);
-      ub = Variable.create(self.vars, self.integratorBounds.upper);
-
-      bounds = OclBounds(varargin{:});
-
-      lb.get('algvars').get(id).set(bounds.lower);
-      ub.get('algvars').get(id).set(bounds.upper);
-
-      self.integratorBounds.lower = lb.value;
-      self.integratorBounds.upper = ub.value;
-      
-      % algvars
-      z_lb = Variable.create(self.algvars, self.algvarBounds.lower);
-      z_ub = Variable.create(self.algvars, self.algvarBounds.upper);
-
-      bounds = OclBounds(varargin{:});
-
-      z_lb.get(id).set(bounds.lower);
-      z_ub.get(id).set(bounds.upper);
-
-      self.algvarBounds.lower = z_lb.value;
-      self.algvarBounds.upper = z_ub.value;
-    end
-
-    function r = pathcostfun(self,x,z,u,p)
-      pcHandler = OclCost();
-
-      x = Variable.create(self.states,x);
-      z = Variable.create(self.algvars,z);
-      u = Variable.create(self.controls,u);
-      p = Variable.create(self.parameters,p);
-
-      self.pathcostsfh(pcHandler,x,z,u,p);
-
-      r = pcHandler.value;
-    end
-  end
-
-end
diff --git a/Core/OclOcpHandler.m b/Core/OclOcpHandler.m
deleted file mode 100755
index b3a8d5b0..00000000
--- a/Core/OclOcpHandler.m
+++ /dev/null
@@ -1,205 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-classdef OclOcpHandler < handle
-  properties (Access = public)
-    pathCostsFun
-    arrivalCostsFun
-    boundaryConditionsFun
-    pathConstraintsFun
-    discreteCostsFun
-    
-    bounds
-    initialBounds
-    endBounds
-    
-    T
-    H_norm
-    
-    options
-  end
-  
-  properties(Access = private)
-    ocp    
-    system
-    nlpVarsStruct
-  end
-
-  methods
-    function self = OclOcpHandler(T,system,ocp,options,H_norm)
-      self.ocp = ocp;
-      self.system = system;
-      self.options = options;
-      
-      N = options.nlp.controlIntervals;
-      
-      self.bounds = struct;
-      self.initialBounds = struct;
-      self.endBounds = struct;
-      
-      if nargin < 5
-        H_norm = repmat(1/N,1,N);
-      end
-      
-      if length(T) == 1
-        % T = final time
-        h = T/N;
-        self.setBounds('h',h);
-        self.T = T;
-        self.H_norm = H_norm;
-      elseif length(T) == N+1
-        % T = N+1 timepoints at states
-        h = (T(2:N+1)-T(1:N));
-        self.setBounds('h',h);
-        self.T = T(end);
-        self.H_norm = H_norm;
-      elseif length(T) == N
-        % T = N timesteps
-        h = T;
-        self.setBounds('h',h);
-        self.T = sum(h);
-        self.H_norm = H_norm;
-      elseif isempty(T)
-        % T = [] free end time
-        self.T = [];
-        self.H_norm = H_norm;
-        self.setBounds('h',0.001,inf);
-      else
-        oclError('Dimension of T does not match the number of control intervals.')
-      end
-      
-    end
-    
-    function setup(self)
-      % variable sizes
-      
-      self.system.setup();
-      
-      sx = self.system.statesStruct.size();
-      sz = self.system.algVarsStruct.size();
-      su = self.system.controlsStruct.size();
-      sp = self.system.parametersStruct.size();
-
-      fhPC = @(self,varargin) self.getPathCosts(varargin{:});
-      self.pathCostsFun = OclFunction(self, fhPC, {sx,sz,su,sp}, 1);
-      
-      fhAC = @(self,varargin) self.getArrivalCosts(varargin{:});
-      self.arrivalCostsFun = OclFunction(self, fhAC, {sx,sp}, 1);
-      
-      fhBC = @(self,varargin)self.getBoundaryConditions(varargin{:});
-      self.boundaryConditionsFun = OclFunction(self, fhBC, {sx,sx,sp}, 3);
-      
-      fhPConst = @(self,varargin)self.getPathConstraints(varargin{:});
-      self.pathConstraintsFun = OclFunction(self, fhPConst, {sx,sp}, 3);
-    end
-    
-    function setNlpVarsStruct(self,varsStruct)
-      self.nlpVarsStruct = varsStruct;
-      sv = varsStruct.size;
-      fhDC = @(self,varargin)self.getDiscreteCosts(varargin{:});
-      self.discreteCostsFun = OclFunction(self, fhDC, {sv}, 1);
-    end
-    
-    function setBounds(self,id,in3,in4)
-
-      self.bounds.(id) = struct;
-      if nargin==3
-        self.bounds.(id).lower = in3;
-        self.bounds.(id).upper = in3;
-      else
-        self.bounds.(id).lower = in3;
-        self.bounds.(id).upper = in4;
-      end
-    end
-    
-    function setInitialBounds(self,id,in3,in4)
-      % setInitialBounds(id,value)
-      % setInitialBounds(id,lower,upper)
-      self.initialBounds.(id) = struct;
-      if nargin==3
-        self.initialBounds.(id).lower = in3;
-        self.initialBounds.(id).upper = in3;
-      else
-        self.initialBounds.(id).lower = in3;
-        self.initialBounds.(id).upper = in4;
-      end
-    end
-    
-    function setEndBounds(self,id,in3,in4)
-      % setEndBounds(id,value)
-      % setEndBounds(id,lower,upper)
-      self.endBounds.(id) = struct;
-      if nargin==3
-        self.endBounds.(id).lower = in3;
-        self.endBounds.(id).upper = in3;
-      else
-        self.endBounds.(id).lower = in3;
-        self.endBounds.(id).upper = in4;
-      end
-    end  
-    
-    function r = getPathCosts(self,x,z,u,p)
-      pcHandler = OclCost(self.ocp);
-      
-      if self.options.controls_regularization
-        pcHandler.add(self.options.controls_regularization_value*(u.'*u));
-      end
-      
-      x = Variable.create(self.system.statesStruct,x);
-      z = Variable.create(self.system.algVarsStruct,z);
-      u = Variable.create(self.system.controlsStruct,u);
-      p = Variable.create(self.system.parametersStruct,p);
-      
-      self.ocp.fh.pathCosts(pcHandler,x,z,u,p);
-      
-      r = pcHandler.value;
-    end
-    
-    function r = getArrivalCosts(self,x,p)
-      acHandler = OclCost(self.ocp);
-      x = Variable.create(self.system.statesStruct,x);
-      p = Variable.create(self.system.parametersStruct,p);
-      
-      self.ocp.fh.arrivalCosts(acHandler,x,p);
-      
-      r = acHandler.value;
-    end
-    
-    function [val,lb,ub] = getPathConstraints(self,x,p)
-      pathConstraintHandler = OclConstraint(self.ocp);
-      x = Variable.create(self.system.statesStruct,x);
-      p = Variable.create(self.system.parametersStruct,p);
-      
-      self.ocp.fh.pathConstraints(pathConstraintHandler,x,p);
-      
-      val = pathConstraintHandler.values;
-      lb = pathConstraintHandler.lowerBounds;
-      ub = pathConstraintHandler.upperBounds;
-    end
-    
-    function [val,lb,ub] = getBoundaryConditions(self,x0,xF,p)
-      bcHandler = OclConstraint(self.ocp);
-      x0 = Variable.create(self.system.statesStruct,x0);
-      xF = Variable.create(self.system.statesStruct,xF);
-      p = Variable.create(self.system.parametersStruct,p);
-      
-      self.ocp.fh.boundaryConditions(bcHandler,x0,xF,p);
-      
-      val = bcHandler.values;
-      lb = bcHandler.lowerBounds;
-      ub = bcHandler.upperBounds;
-    end
-    
-    function r = getDiscreteCosts(self,v)
-      dcHandler = OclCost(self.ocp);
-      v = Variable.create(self.nlpVarsStruct,v);
-      
-      self.ocp.fh.discreteCosts(dcHandler,v);
-      
-      r = dcHandler.value;
-    end
-
-  end
-end
-
diff --git a/Core/OclOptions.m b/Core/OclOptions.m
deleted file mode 100644
index cb9a2ea2..00000000
--- a/Core/OclOptions.m
+++ /dev/null
@@ -1,25 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-function opt = OclOptions()
-
-  oclDeprecation('The use of OclOptions is deprecated, pass options directly to the stage or solver.')
-
-  opt = struct;
-  opt.solverInterface   = 'casadi';
-  opt.system_casadi_mx  = false;
-  opt.nlp_casadi_mx     = false;
-  opt.controls_regularization = true;
-  opt.controls_regularization_value = 1e-6;
-  opt.path_constraints_at_boundary = true;
-  opt.nlp = struct;
-  opt.nlp.discretization         = 'collocation';
-  opt.nlp.controlIntervals       = 20;
-  opt.nlp.collocationOrder       = 3;
-  opt.nlp.solver                 = 'ipopt';
-  opt.nlp.casadi = struct;
-  opt.nlp.ipopt = struct;
-  opt.nlp.ipopt.linear_solver = 'mumps';
-  opt.nlp.ipopt.hessian_approximation = 'exact';
-end
\ No newline at end of file
diff --git a/Core/OclSimulator.m b/Core/OclSimulator.m
deleted file mode 100644
index 4642a076..00000000
--- a/Core/OclSimulator.m
+++ /dev/null
@@ -1,243 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-classdef OclSimulator < handle
-
-  properties
-    integrator
-    system
-    options
-
-    current_state
-    algebraic_guess
-    parameters
-  end
-
-  methods
-
-    function self = OclSimulator(system,options)
-
-      if nargin==1
-        self.options = struct;
-      else
-        self.options = options;
-      end
-
-      self.integrator = CasadiIntegrator(system);
-      self.system = system;
-
-      self.current_state = [];
-      self.algebraic_guess = 0;
-      self.parameters = [];
-    end
-
-    function controlsVec = getControlsVec(self,N)
-        controlsVecStruct  = OclStructure();
-        controlsVecStruct.addRepeated({'u'},{self.system.controls},N);
-        controlsVec = Variable.create(controlsVecStruct,0);
-        controlsVec = controlsVec.u;
-    end
-
-    function controls = getControls(self)
-      controls = Variable.create(self.system.controls,0);
-    end
-
-    function states = getStates(self)
-      states = Variable.create(self.system.states,0);
-    end
-
-    function states = getParameters(self)
-      states = Variable.create(self.system.parameters,0);
-    end
-
-    function z = getAlgebraicStates(self)
-      z = Variable.create(self.system.algvars,0);
-    end
-
-    function [x] = reset(self,varargin)
-
-      p = ocl.utils.ArgumentParser;
-      p.addRequired('x0', @(el) isa(el, 'Variable') || isnumeric(el) );
-      p.addKeyword('parameters', [], @(el) isa(el, 'Variable') || isnumeric(el));
-      p.addKeyword('callback', false, @islogical);
-
-      args = p.parse(varargin{:});
-
-      initialStates = args.x0;
-      if isnumeric(args.x0)
-        initialStates = self.getStates();
-        initialStates.set(args.x0);
-      end
-
-      params = args.parameters;
-      if isnumeric(args.parameters)
-        params = self.getParameters();
-        params.set(args.parameters);
-      end
-
-      useCallback = args.callback;
-
-      nz = self.system.nz;
-
-      z = zeros(nz,1);
-      x0 = Variable.getValueAsColumn(initialStates);
-      p = Variable.getValueAsColumn(params);
-
-      [x,z] = self.getConsistentIntitialCondition(x0,z,p);
-      initialStates.set(x);
-
-      % setup callback
-      if useCallback && ~oclIsTestRun
-        self.system.callbacksetupfun();
-      end
-
-      self.current_state = x;
-      self.algebraic_guess = z;
-      self.parameters = p;
-    end
-
-    function [states_out,algebraics_out] = step(self, controls_in, dt)
-
-      if isnumeric(controls_in)
-        controls = self.getControls();
-        controls.set(controls_in);
-      else
-        controls = controls_in;
-      end
-
-      oclAssert(~isempty(self.current_state), 'Call `initialize` before `step`.');
-
-      x = self.current_state;
-      z0 = self.algebraic_guess;
-      p = self.parameters;
-
-      u = Variable.getValueAsColumn(controls);
-
-      [x,z] = self.integrator.evaluate(x,z0,u,dt,p);
-
-      states_out = self.getStates();
-      states_out.set(x);
-
-      algebraics_out = self.getAlgebraicStates();
-      algebraics_out.set(z);
-
-      self.current_state = x;
-      self.algebraic_guess = z;
-
-    end
-
-    function [statesVec,algVarsVec,controlsVec] = simulate(self,varargin)
-      % [statesVec,algVarsVec,controlsVec] = 
-      %     simulate(initialState, times, parameters=[], controlsVec=0, callback=false)
-
-      p = ocl.utils.ArgumentParser;
-      p.addRequired('x0', @(el) isa(el, 'Variable') || isnumeric(el) );
-      p.addRequired('times', @isnumeric)
-
-      p.addKeyword('controls', 0, @(el) isa(el, 'Variable') || isnumeric(el));
-      p.addKeyword('parameters', [], @(el) isa(el, 'Variable') || isnumeric(el));
-      p.addKeyword('callback', false, @islogical);
-
-      args = p.parse(varargin{:});
-
-      x0 = args.x0;
-      if isnumeric(args.x0)
-        x0 = self.getStates();
-        x0.set(x0_in);
-      end
-
-      times = args.times;
-
-      p = args.parameters;
-      if isnumeric(args.parameters)
-        p = self.getParameters();
-        p.set(args.parameters);
-      end
-
-      N = length(times)-1; % number of control intervals
-
-      controlsVec = args.controls;
-      if isnumeric(args.controls)
-        controlsVec = self.getControlsVec(N);
-        controlsVec.set(args.controls);
-      end
-
-      x0 = self.reset(x0, p);
-
-      useCallback = args.callback;
-
-      statesVecStruct = OclStructure();
-      statesVecStruct.addRepeated({'x'},{self.system.states},N+1);
-
-      algVarsVecStruct = OclStructure();
-      algVarsVecStruct.addRepeated({'z'},{self.system.algvars},N);
-
-      statesVec = Variable.create(statesVecStruct,0);
-      statesVec = statesVec.x;
-      algVarsVec = Variable.create(algVarsVecStruct,0);
-      algVarsVec = algVarsVec.z;
-      
-      statesVec(:,:,1).set(x0);
-
-      for k=1:N-1
-
-        x = self.current_state;
-        z = self.algebraic_guess;
-        p = self.parameters;
-
-        t = times(k+1)-times(k);
-        u = Variable.getValueAsColumn(controlsVec(:,:,k));
-
-        if useCallback && ~ocl.utils.isTestRun
-          u = self.system.callbackfun(x,z,u,times(k),times(k+1),p);
-        end
-
-        [x,z] = self.integrator.evaluate(x,z,u,t,p);
-
-        self.current_state = x;
-        self.algebraic_guess = z;
-
-        statesVec(:,:,k+1).set(x);
-        algVarsVec(:,:,k).set(z);
-        controlsVec(:,:,k).set(u);
-
-      end
-
-      if useCallback && ~ocl.utils.isTestRun
-        [~] = self.system.callbackfun(x,z,u,times(k),times(k+1),p);
-      end
-
-    end
-
-    function [xOut,zOut] = getConsistentIntitialCondition(self,x,z,p)
-
-      xSymb  = casadi.SX.sym('x',size(x));
-      zSymb  = casadi.SX.sym('z',size(z));
-
-      ic = self.system.icfun(xSymb,p);
-      [~,alg] = self.system.daefun(xSymb,zSymb,0,p);
-
-      z = ones(size(z)) * rand;
-
-      optVars = [xSymb;zSymb];
-      x0      = [x;z];
-
-      statesErr = (xSymb-x);
-      cost = statesErr'*statesErr;
-
-      nlp    = struct('x', optVars, 'f', cost, 'g', vertcat(ic,alg));
-      solver = casadi.nlpsol('solver', 'ipopt', nlp);
-      sol    = solver('x0', x0, 'lbx', -inf, 'ubx', inf,'lbg', 0, 'ubg', 0);
-
-      sol = full(sol.x);
-      nx = size(x,1);
-
-      xOut = sol(1:nx);
-      zOut = sol(nx+1:end);
-
-    end
-
-  end
-
-end
diff --git a/Core/OclSolver.m b/Core/OclSolver.m
deleted file mode 100644
index 3240b88f..00000000
--- a/Core/OclSolver.m
+++ /dev/null
@@ -1,271 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-classdef OclSolver < handle
-
-  properties
-    bounds
-    initialBounds
-    endBounds
-    igParameters
-
-    solver
-    stageList
-  end
-
-  methods
-
-    function self = OclSolver(varargin)
-      % OclSolver(T, 'vars', @varsfun, 'dae', @daefun,
-      %           'pathcosts', @pathcostfun,
-      %           'gridcosts', @gridcostfun,
-      %           'gridconstraints', @gridconstraintsfun, casadi_options)
-      % OclSolver(stages, transitions, casadi_options)
-
-      if isnumeric(varargin{1}) && isa(varargin{2}, 'OclSystem')
-        % OclSolver(T, system, ocp, options, H_norm)
-        oclDeprecation(['This way of creating the solver ', ...
-                        'is deprecated. It will be removed from version >5.01']);
-        oclError('OclSystem is not supported anymore!');
-      elseif nargin >= 1 && isnumeric(varargin{1}) && ( isscalar(varargin{1}) || isempty(varargin{1}) )
-        % OclSolver(T, 'vars', @varsfun, 'dae', @daefun,
-        %           'lagrangecost', @lagrangefun,
-        %           'pathcosts', @pathcostfun, options
-        
-        zerofh = @(varargin) 0;
-        emptyfh = @(varargin) [];
-        p = ocl.utils.ArgumentParser;
-
-        p.addRequired('T', @(el)isnumeric(el) || isempty(el) );
-        p.addKeyword('vars', emptyfh, @oclIsFunHandle);
-        p.addKeyword('dae', emptyfh, @oclIsFunHandle);
-        p.addKeyword('pathcosts', zerofh, @oclIsFunHandle);
-        p.addKeyword('gridcosts', zerofh, @oclIsFunHandle);
-        
-        p.addKeyword('gridconstraints', emptyfh, @oclIsFunHandle);
-        
-        p.addKeyword('callback', emptyfh, @oclIsFunHandle);
-        p.addKeyword('callback_setup', emptyfh, @oclIsFunHandle);
-        
-        p.addKeyword('pointcosts', {}, @(el) iscell(el) && (isempty(el) || isa(el{1}, 'ocl.Pointcost')));
-        
-        p.addParameter('nlp_casadi_mx', false, @islogical);
-        p.addParameter('controls_regularization', true, @islogical);
-        p.addParameter('controls_regularization_value', 1e-6, @isnumeric);
-        
-        p.addParameter('casadi_options', CasadiOptions(), @(el) isstruct(el));
-        p.addParameter('N', 20, @isnumeric);
-        p.addParameter('d', 3, @isnumeric);
-        
-        r = p.parse(varargin{:});
-        
-        stageList = {OclStage(r.T, r.vars, r.dae, r.pathcosts, r.gridcosts, r.gridconstraints, ...
-                              r.callback_setup, r.callback, r.pointcosts, 'N', r.N, 'd', r.d)};
-        transitionList = {};
-        
-        nlp_casadi_mx = r.nlp_casadi_mx;
-        controls_regularization = r.controls_regularization;
-        controls_regularization_value = r.controls_regularization_value;
-        
-        casadi_options = r.casadi_options;
-        
-      else
-        % OclSolver(stages, transitions, opt)
-        p = ocl.utils.ArgumentParser;
-
-        p.addKeyword('stages', {}, @(el) iscell(el) || isa(el, 'OclStage'));
-        p.addKeyword('transitions', {}, @(el) iscell(el) || ishandle(el) );
-        
-        p.addParameter('nlp_casadi_mx', false, @islogical);
-        p.addParameter('controls_regularization', true, @islogical);
-        p.addParameter('controls_regularization_value', 1e-6, @isnumeric);
-        
-        p.addParameter('casadi_options', CasadiOptions(), @(el) isstruct(el));
-        
-        r = p.parse(varargin{:});
-        
-        stageList = r.stages;
-        transitionList = r.transitions;
-        
-        nlp_casadi_mx = r.nlp_casadi_mx;
-        controls_regularization = r.controls_regularization;
-        controls_regularization_value = r.controls_regularization_value;
-        
-        casadi_options = r.casadi_options;
-      end
-
-      solver = CasadiSolver(stageList, transitionList, ...
-            nlp_casadi_mx, controls_regularization, controls_regularization_value, casadi_options);
-
-      % set instance variables
-      self.stageList = stageList;
-      self.solver = solver;
-    end
-
-    function r = jacobian_pattern(self, assignment)
-
-      s = self.solver;
-      st_list = self.stageList;
-
-      v = s.nlpData.casadiNLP.x;
-      g = s.nlpData.casadiNLP.g;
-      jac_fun = casadi.Function('j', {v}, {jacobian(g, v)});
-
-      values = cell(length(st_list),1);
-      for k=1:length(st_list)
-        values{k} = assignment{k}.value;
-      end
-      values = vertcat(values{:});
-      r = jac_fun(values);
-    end
-
-    function [sol_ass,times_ass,objective_ass,constraints_ass] = solve(self, ig)
-
-      s = self.solver;
-      st_list = self.stageList;
-      
-      if isa(ig, 'OclAssignment')
-        ig_list = cell(length(st_list),1);
-        for k=1:length(st_list)
-          ig_list{k} = ig{k}.value;
-        end
-      else
-        % ig InitialGuess
-        ig_list = cell(length(st_list),1);
-        for k=1:length(st_list)
-          ig_stage = ocl.simultaneous.getInitialGuessWithUserData(st_list{k}, ig{k});
-          ig_list{k} = ig_stage.value;
-        end
-      end
-
-      [sol,times,objective,constraints] = s.solve(ig_list);
-
-      sol_list = cell(length(st_list));
-      times_list = cell(length(st_list));
-      obj_list = cell(length(st_list));
-      con_list = cell(length(st_list));
-
-      for k=1:length(st_list)
-        stage = st_list{k};
-        vars_structure = ocl.simultaneous.variables(stage);
-        times_structure = ocl.simultaneous.times(stage);
-        sol_list{k} = Variable.create(vars_structure, sol{k});
-        times_list{k} = Variable.create(times_structure, times{k});
-        obj_list{k} = objective{k};
-        con_list{k} = constraints{k};
-      end
-
-      sol_ass = OclAssignment(sol_list);
-      times_ass = OclAssignment(times_list);
-      objective_ass = OclAssignment(obj_list);
-      constraints_ass = OclAssignment(con_list);
-
-      oclWarningNotice()
-    end
-
-    function r = timeMeasures(self)
-      r = self.solver.timeMeasures;
-    end
-    
-    function ig_list = initialGuess(self)
-      ig_list = cell(length(self.stageList), 1);
-      for k=1:length(ig_list)
-        ig_list{k} = ocl.InitialGuess(self.stageList{k}.states);
-      end
-    end
-
-    function ig = ig(self)
-      ig = self.getInitialGuess();
-    end
-
-    function igAssignment = getInitialGuess(self)
-
-      pl = self.stageList;
-
-      igList = cell(length(pl),1);
-      for k=1:length(pl)
-        stage = pl{k};
-        varsStruct = ocl.simultaneous.variables(stage);
-        ig = ocl.simultaneous.getInitialGuess(stage);
-        igList{k} = Variable.create(varsStruct, ig);
-      end
-
-      igAssignment = OclAssignment(igList);
-    end
-
-    function solutionCallback(self,times,solution)
-      
-      for st_idx=1:length(self.stageList)
-        sN = size(solution{st_idx}.states);
-        N = sN(3);
-
-        t = times{st_idx}.states;
-
-        self.stageList{st_idx}.callbacksetupfun()
-
-        for k=1:N-1
-          x = solution{st_idx}.states(:,:,k+1);
-          z = solution{st_idx}.integrator(:,:,k).algvars;
-          u = solution{st_idx}.controls(:,:,k);
-          p = solution{st_idx}.parameters(:,:,k);
-          self.stageList{st_idx}.callbackfh(x,z,u,t(:,:,k),t(:,:,k+1),p);
-        end
-      end
-      
-
-    end
-
-    function setParameter(self,id,varargin)
-      if length(self.stageList) == 1
-        self.stageList{1}.setParameterBounds(id, varargin{:});
-      else
-        oclError('For multi-stage problems, set the bounds to the stages directlly.')
-      end
-    end
-
-    function setBounds(self,id,varargin)
-      % setBounds(id,value)
-      % setBounds(id,lower,upper)
-      if length(self.stageList) == 1
-
-        % check if id is a state, control, algvar or parameter
-        if oclFieldnamesContain(self.stageList{1}.states.getNames(), id)
-          self.stageList{1}.setStateBounds(id, varargin{:});
-        elseif oclFieldnamesContain(self.stageList{1}.algvars.getNames(), id)
-          self.stageList{1}.setAlgvarBounds(id, varargin{:});
-        elseif oclFieldnamesContain(self.stageList{1}.controls.getNames(), id)
-          self.stageList{1}.setControlBounds(id, varargin{:});
-        elseif oclFieldnamesContain(self.stageList{1}.parameters.getNames(), id)
-          self.stageList{1}.setParameterBounds(id, varargin{:});
-        else
-          oclWarning(['You specified a bound for a variable that does not exist: ', id]);
-        end
-
-      else
-        oclError('For multi-stage problems, set the bounds to the stages directly.')
-      end
-    end
-
-    function setInitialBounds(self,id,varargin)
-      % setInitialBounds(id,value)
-      % setInitialBounds(id,lower,upper)
-      if length(self.stageList) == 1
-        self.stageList{1}.setInitialStateBounds(id, varargin{:});
-      else
-        oclError('For multi-stage problems, set the bounds to the stages directly.')
-      end
-    end
-
-    function setEndBounds(self,id,varargin)
-      % setEndBounds(id,value)
-      % setEndBounds(id,lower,upper)
-      if length(self.stageList) == 1
-        self.stageList{1}.setEndStateBounds(id, varargin{:});
-      else
-        oclError('For multi-stage problems, set the bounds to the stages directlly.')
-      end
-    end
-
-  end
-end
diff --git a/Core/OclStage.m b/Core/OclStage.m
deleted file mode 100644
index 72697d49..00000000
--- a/Core/OclStage.m
+++ /dev/null
@@ -1,282 +0,0 @@
-classdef OclStage < handle
-
-  properties
-    T
-    H_norm
-    integrator
-    
-    pathcostfun
-    gridcostsfh
-    pointcostsarray
-    
-    gridconstraintsfh
-    
-    callbacksetupfh
-    callbackfh
-    
-    integratormap
-    
-    
-    stateBounds
-
-    stateBounds0
-    stateBoundsF
-    controlBounds
-    parameterBounds
-    
-    nx
-    nz
-    nu
-    np
-    
-    states
-    algvars
-    controls
-    parameters
-  end
-  
-  properties (Access = private)
-
-  end
-  
-  methods
-    
-    function self = OclStage(T, varargin)
-      
-      emptyfh = @(varargin)[];
-      
-      p = ocl.utils.ArgumentParser;
-      
-      p.addKeyword('vars', emptyfh, @oclIsFunHandle);
-      p.addKeyword('dae', emptyfh, @oclIsFunHandle);
-      p.addKeyword('pathcosts', emptyfh, @oclIsFunHandle);
-      p.addKeyword('gridcosts', emptyfh, @oclIsFunHandle);
-      
-      p.addKeyword('gridconstraints', emptyfh, @oclIsFunHandle);
-      
-      p.addKeyword('callbacksetup', emptyfh, @oclIsFunHandle);
-      p.addKeyword('callback', emptyfh, @oclIsFunHandle);
-      
-      p.addKeyword('pointcosts', {}, @(el) iscell(el) && (isempty(el) || isa(el{1}, 'ocl.Pointcost')));
-
-      p.addParameter('N', 20, @isnumeric);
-      p.addParameter('d', 3, @isnumeric);
-      
-      r = p.parse(varargin{:});
-      
-      varsfhInput = r.vars;
-      daefhInput = r.dae;
-      
-      pathcostsfhInput = r.pathcosts;
-      gridcostsfhInput = r.gridcosts;  
-      pointcostsarrayInput = r.pointcosts;  
-      
-      gridconstraintsfhInput = r.gridconstraints;
-      
-      callbacksetupfh = r.callbacksetup;
-      callbackfh = r.callback;
-      
-      H_normInput = r.N;
-      dInput = r.d;
-
-      oclAssert( (isscalar(T) || isempty(T)) && isreal(T), ... 
-        ['Invalid value for parameter T.', oclDocMessage()] );
-      self.T = T;
-      
-      oclAssert( (isscalar(H_normInput) || isnumeric(H_normInput)) && isreal(H_normInput), ...
-        ['Invalid value for parameter N.', oclDocMessage()] );
-      if isscalar(H_normInput)
-        H_normInput = repmat(1/H_normInput, 1, H_normInput);
-      elseif abs(sum(H_normInput)-1) > 1e-6 
-        H_normInput = H_normInput/sum(H_normInput);
-        oclWarning(['Timesteps given in pararmeter N are not normalized! ', ...
-                    'N either be a scalar value or a normalized vector with the length ', ...
-                    'of the number of control grid. Check the documentation of N. ', ...
-                    'Make sure the timesteps sum up to 1, and contain the relative ', ...
-                    'length of the timesteps. OpenOCL normalizes the timesteps and proceeds.']);
-      end
-      
-      system = OclSystem(varsfhInput, daefhInput);
-      colocation = OclCollocation(system.states, system.algvars, system.controls, ...
-                                  system.parameters, @system.daefun, pathcostsfhInput, dInput, ...
-                                  system.stateBounds, system.algvarBounds);
-      
-      self.H_norm = H_normInput;
-      self.integrator = colocation;
-      
-      self.pathcostfun = @colocation.pathcostfun;
-      self.gridcostsfh = gridcostsfhInput;
-      self.pointcostsarray = pointcostsarrayInput;
-      
-      self.gridconstraintsfh = gridconstraintsfhInput;
-      
-      self.callbacksetupfh = callbacksetupfh;
-      self.callbackfh = callbackfh;
-      
-      self.nx = colocation.num_x;
-      self.nz = colocation.num_z;
-      self.nu = colocation.num_u;
-      self.np = colocation.num_p;
-      
-      self.states = system.states;
-      self.algvars = system.algvars;
-      self.controls = system.controls;
-      self.parameters = system.parameters;
-      
-      self.stateBounds = OclBounds(-inf * ones(self.nx, 1), inf * ones(self.nx, 1));
-      self.stateBounds0 = OclBounds(-inf * ones(self.nx, 1), inf * ones(self.nx, 1));
-      self.stateBoundsF = OclBounds(-inf * ones(self.nx, 1), inf * ones(self.nx, 1));
-      self.controlBounds = OclBounds(-inf * ones(self.nu, 1), inf * ones(self.nu, 1));
-      self.parameterBounds = OclBounds(-inf * ones(self.np, 1), inf * ones(self.np, 1));
-      
-      names = fieldnames(system.stateBounds);
-      for k=1:length(names)
-        id = names{k};
-        self.setStateBounds(id, system.stateBounds.(id).lower, system.stateBounds.(id).upper);
-      end
-      
-      names = fieldnames(system.controlBounds);
-      for k=1:length(names)
-        id = names{k};
-        self.setControlBounds(id, system.controlBounds.(id).lower, system.controlBounds.(id).upper);
-      end
-      
-      names = fieldnames(system.parameterBounds);
-      for k=1:length(names)
-        id = names{k};
-        self.setParameterBounds(id, system.parameterBounds.(id).lower, system.parameterBounds.(id).upper);
-      end
-      
-    end
-
-    function r = N(self)
-      r = length(self.H_norm);
-    end
-    
-    function setStateBounds(self,id,varargin)
-      
-      self.integrator.setStateBounds(id,varargin{:});
-      
-      x_lb = Variable.create(self.states, self.stateBounds.lower);
-      x_ub = Variable.create(self.states, self.stateBounds.upper);
-      
-      bounds = OclBounds(varargin{:});
-      
-      x_lb.get(id).set(bounds.lower);
-      x_ub.get(id).set(bounds.upper);
-      
-      self.stateBounds.lower = x_lb.value;
-      self.stateBounds.upper = x_ub.value;
-    end
-    
-    function setInitialStateBounds(self,id,varargin)
-      x0_lb = Variable.create(self.states, self.stateBounds0.lower);
-      x0_ub = Variable.create(self.states, self.stateBounds0.upper);
-      
-      bounds = OclBounds(varargin{:});
-      
-      x0_lb.get(id).set(bounds.lower);
-      x0_ub.get(id).set(bounds.upper);
-      
-      self.stateBounds0.lower = x0_lb.value;
-      self.stateBounds0.upper = x0_ub.value;
-    end
-    
-    function setEndStateBounds(self,id,varargin)
-      xF_lb = Variable.create(self.states, self.stateBoundsF.lower);
-      xF_ub = Variable.create(self.states, self.stateBoundsF.upper);
-      
-      bounds = OclBounds(varargin{:});
-      
-      xF_lb.get(id).set(bounds.lower);
-      xF_ub.get(id).set(bounds.upper);
-      
-      self.stateBoundsF.lower = xF_lb.value;
-      self.stateBoundsF.upper = xF_ub.value;
-    end
-    
-    function setAlgvarBounds(self,id,varargin)
-      self.integrator.setAlgvarBounds(id,varargin{:});
-    end
-    
-    function setControlBounds(self,id,varargin)
-      u_lb = Variable.create(self.controls, self.controlBounds.lower);
-      u_ub = Variable.create(self.controls, self.controlBounds.upper);
-      
-      bounds = OclBounds(varargin{:});
-      
-      u_lb.get(id).set(bounds.lower);
-      u_ub.get(id).set(bounds.upper);
-      
-      self.controlBounds.lower = u_lb.value;
-      self.controlBounds.upper = u_ub.value;
-    end
-    
-    function setParameterBounds(self,id,varargin)
-      p_lb = Variable.create(self.parameters, self.parameterBounds.lower);
-      p_ub = Variable.create(self.parameters, self.parameterBounds.upper);
-      
-      bounds = OclBounds(varargin{:});
-      
-      p_lb.get(id).set(bounds.lower);
-      p_ub.get(id).set(bounds.upper);
-      
-      self.parameterBounds.lower = p_lb.value;
-      self.parameterBounds.upper = p_ub.value;
-    end
-    
-    function r = gridcostfun(self,k,N,x,p)
-      gridCostHandler = OclCost();
-      
-      x = Variable.create(self.states,x);
-      p = Variable.create(self.parameters,p);
-      
-      self.gridcostsfh(gridCostHandler,k,N,x,p);
-      
-      r = gridCostHandler.value;
-    end
-    
-    function r = pointcostfun(self, k, x, p)
-      ch = OclCost();
-      
-      x = Variable.create(self.states,x);
-      p = Variable.create(self.parameters,p);
-      
-      fh = self.pointcostsarray{k};
-      fh(ch, x, p);
-      
-      r = ch.value;
-    end
-    
-    function [val,lb,ub] = gridconstraintfun(self,k,N,x,p)
-      gridConHandler = OclConstraint();
-      x = Variable.create(self.states,x);
-      p = Variable.create(self.parameters,p);
-      
-      self.gridconstraintsfh(gridConHandler,k,N,x,p);
-      
-      val = gridConHandler.values;
-      lb = gridConHandler.lowerBounds;
-      ub = gridConHandler.upperBounds;
-    end
-    
-    function callbacksetupfun(self)
-      self.callbacksetupfh();
-    end
-    
-    function u = callbackfun(self,x,z,u,t0,t1,p)
-      
-      x = Variable.create(self.states,x);
-      z = Variable.create(self.algvars,z);
-      u = Variable.create(self.states,u);
-      p = Variable.create(self.parameters,p);
-      
-      t0 = Variable.Matrix(t0);
-      t1 = Variable.Matrix(t1);
-      
-      u = self.callbackfh(x,z,u,t0,t1,p);
-    end
-    
-  end
-  
-end
diff --git a/Core/OclSystem.m b/Core/OclSystem.m
deleted file mode 100755
index 1fe74bc3..00000000
--- a/Core/OclSystem.m
+++ /dev/null
@@ -1,143 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-classdef OclSystem < handle
-
-  properties
-    varsfh
-    daefh
-    icfh
-    callbackfh
-    callbacksetupfh
-    
-    thisInitialConditions
-    
-    states
-    algvars
-    controls
-    parameters
-    stateBounds
-    algvarBounds
-    controlBounds
-    parameterBounds
-    
-    statesOrder
-  end
-
-  methods
-
-    function self = OclSystem(varargin)
-      % OclSystem()
-      % OclSystem(fhVarSetup,fhEquationSetup)
-      % OclSystem(fhVarSetup,fhEquationSetup,fhInitialCondition)
-
-      emptyfh = @(varargin)[];
-
-      p = ocl.utils.ArgumentParser;
-
-      p.addKeyword('vars', emptyfh, @oclIsFunHandle);
-      p.addKeyword('dae', emptyfh, @oclIsFunHandle);
-      p.addKeyword('ic', emptyfh, @oclIsFunHandle);
-      p.addKeyword('callbacksetup', emptyfh, @oclIsFunHandle);
-      p.addKeyword('callback', emptyfh, @oclIsFunHandle);
-      
-      r = p.parse(varargin{:});
-
-      varsfun = r.vars;
-      daefun = r.dae;
-      icfun = r.ic;
-      callbacksetupfh = r.callbacksetup;
-      callbackfh = r.callback;
-
-      self.varsfh = varsfun;
-      self.daefh = daefun;
-      self.icfh = icfun;
-
-      self.callbacksetupfh = callbacksetupfh;
-      self.callbackfh = callbackfh;
-      
-      svh = OclSysvarsHandler;
-      self.varsfh(svh);
-      
-      self.states = svh.states;
-      self.algvars = svh.algvars;
-      self.controls = svh.controls;
-      self.parameters = svh.parameters;
-      self.stateBounds = svh.stateBounds;
-      self.algvarBounds = svh.algvarBounds;
-      self.controlBounds = svh.controlBounds;
-      self.parameterBounds = svh.parameterBounds;
-      
-      self.statesOrder = svh.statesOrder;
-
-    end
-
-    function r = nx(self)
-      r = prod(self.states.size());
-    end
-
-    function r = nz(self)
-      r = prod(self.algvars.size());
-    end
-
-    function r = nu(self)
-      r = prod(self.controls.size());
-    end
-
-    function r = np(self)
-      r = prod(self.parameters.size());
-    end
-    
-    function simulationCallbackSetup(~)
-      % simulationCallbackSetup()
-    end
-
-    function simulationCallback(varargin)
-      % simulationCallback(states,algVars,controls,timeBegin,timesEnd,parameters)
-    end
-
-    function [ode,alg] = daefun(self,x,z,u,p)
-      % evaluate the system equations for the assigned variables
-
-      x = Variable.create(self.states,x);
-      z = Variable.create(self.algvars,z);
-      u = Variable.create(self.controls,u);
-      p = Variable.create(self.parameters,p);
-
-      daehandler = OclDaeHandler();
-      self.daefh(daehandler,x,z,u,p);
-
-      ode = daehandler.getOde(self.nx, self.statesOrder);
-      alg = daehandler.getAlg(self.nz);
-    end
-
-    function ic = icfun(self,x,p)
-      icHandler = OclConstraint();
-      x = Variable.create(self.states,x);
-      p = Variable.create(self.parameters,p);
-      self.icfh(icHandler,x,p)
-      ic = icHandler.values;
-      assert(all(icHandler.lowerBounds==0) && all(icHandler.upperBounds==0),...
-          'In initial condition are only equality constraints allowed.');
-    end
-
-    function callbacksetupfun(self)
-      self.callbacksetupfh();
-    end
-
-    function u = callbackfun(self,states,algVars,controls,timesBegin,timesEnd,parameters)
-      x = Variable.create(self.states,states);
-      z = Variable.create(self.algvars,algVars);
-      u = Variable.create(self.controls,controls);
-      p = Variable.create(self.parameters,parameters);
-
-      t0 = Variable.Matrix(timesBegin);
-      t1 = Variable.Matrix(timesEnd);
-
-      self.callbackfh(x,z,u,t0,t1,p);
-      u = Variable.getValueAsColumn(u);
-    end
-
-  end
-end
diff --git a/Core/Variables/SymVariable.m b/Core/Variables/SymVariable.m
deleted file mode 100755
index 2749ab07..00000000
--- a/Core/Variables/SymVariable.m
+++ /dev/null
@@ -1,55 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-classdef SymVariable < Variable
-  % SYMVARIABLE Variable arithmetic operations for Matlab symbolic 
-  % toolbox variables
-  
-  properties
-  end
-  
-  methods (Static)
-    
-    function var = create(t,value)
-      vv = sym('v',t.size());
-      vv = vv(:).';
-      v = OclValue(vv);
-      var = Variable(type,1:length(vv),v);
-      if nargin == 2
-        var.set(value);
-      end
-    end
-    
-    function var = Matrix(sizeIn)
-      var = SymVariable.create(OclMatrix(sizeIn));
-    end
-  end
-  
-  methods
-    function self = SymVariable(type,positions,val)
-      self = self@Variable(type,positions,val);
-    end
-    
-    function v = polyval(p,a)
-      if isa(p,'Variable') 
-        self = p;
-        p = p.value;
-      end  
-      if isa(a,'Variable')
-        self = a;
-        a = a.value;
-      end
-      % Use Horner's method for general case where X is an array.
-      nc = length(p);
-      siz_a = size(a);
-      y = zeros(siz_a);
-      if nc>0, y(:) = p(1); end
-      for i=2:nc
-        y = a .* y + p(i);
-      end
-      v = Variable.createMatrixLike(self,y);
-    end
-  end
-end
-
diff --git a/Core/oclPlot.m b/Core/oclPlot.m
deleted file mode 100644
index abbadaf8..00000000
--- a/Core/oclPlot.m
+++ /dev/null
@@ -1,10 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-function oclPlot(x,y,varargin)
-  x = Variable.getValue(x);
-  y = Variable.getValue(y);
-  
-  plot(x,y,'LineWidth', 3, varargin{:})
-  
\ No newline at end of file
diff --git a/Core/oclStairs.m b/Core/oclStairs.m
deleted file mode 100644
index e0f8ad19..00000000
--- a/Core/oclStairs.m
+++ /dev/null
@@ -1,10 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-function oclStairs(x,y,varargin)
-  x = Variable.getValue(x);
-  y = Variable.getValue(y);
-  
-  stairs(x,y,'LineWidth', 3, varargin{:})
-  
\ No newline at end of file
diff --git a/Core/utils/oclArgError.m b/Core/utils/oclArgError.m
deleted file mode 100644
index 4979c3f2..00000000
--- a/Core/utils/oclArgError.m
+++ /dev/null
@@ -1,4 +0,0 @@
-function oclArgError(arg)
-oclError(['Wrong value for argument ', arg, ' given. Please check the docs at ', oclDocMessage()]);
-end
-
diff --git a/Core/utils/oclAssert.m b/Core/utils/oclAssert.m
deleted file mode 100644
index dc8e868c..00000000
--- a/Core/utils/oclAssert.m
+++ /dev/null
@@ -1,3 +0,0 @@
-function oclAssert(condition, varargin)
-  assert(condition,varargin{:});
-end
\ No newline at end of file
diff --git a/Core/utils/oclFieldnamesContain.m b/Core/utils/oclFieldnamesContain.m
deleted file mode 100644
index a6f7ffd1..00000000
--- a/Core/utils/oclFieldnamesContain.m
+++ /dev/null
@@ -1,3 +0,0 @@
-function r = oclFieldnamesContain(names, id)
-  r = any(strcmp(names,id));
-end
\ No newline at end of file
diff --git a/Core/utils/oclIsFunHandle.m b/Core/utils/oclIsFunHandle.m
deleted file mode 100644
index 0b70481b..00000000
--- a/Core/utils/oclIsFunHandle.m
+++ /dev/null
@@ -1,6 +0,0 @@
-% Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
-% Redistribution is permitted under the 3-Clause BSD License terms. Please
-% ensure the above copyright notice is visible in any derived work.
-%
-function r = oclIsFunHandle(v)
-r = isa(v,'function_handle');
\ No newline at end of file
diff --git a/acados_setup.sh b/acados_setup.sh
new file mode 100644
index 00000000..1df74736
--- /dev/null
+++ b/acados_setup.sh
@@ -0,0 +1,33 @@
+#! /usr/bin/bash
+#
+# Usage source acados_setup.sh path/to/acados/lib
+#
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+#
+
+if [[ "${BASH_SOURCE[0]}" == "${0}" ]]; then
+	echo "ERROR: Script is a subshell"
+	echo "To affect your current shell enviroment source this script with:"
+	echo "source acados_setup.sh"
+	exit
+fi
+
+
+  # folder location of this file (main directory of OpenOCL)
+  OCL_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+
+  export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$OCL_DIR/Workspace/export/
diff --git a/info.xml b/info.xml
index 85aee013..a67c60aa 100644
--- a/info.xml
+++ b/info.xml
@@ -1,6 +1,6 @@
-<productinfo xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" 
+<productinfo xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
     xsi:noNamespaceSchemaLocation="optional">
-<!-- 
+<!--
 OpenOCL
 
 Copyright 2019 Jonas Koenemann, Moritz Diehl, University of Freiburg
@@ -16,4 +16,4 @@ ensure the above copyright notice is visible in any derived work.
     <icon></icon>
     <help_location>doc</help_location>
     <help_contents_icon></help_contents_icon>
-</productinfo>
\ No newline at end of file
+</productinfo> 
diff --git a/ocl.m b/ocl.m
index 54b39a89..d38b04f9 100644
--- a/ocl.m
+++ b/ocl.m
@@ -44,4 +44,4 @@
 % <https://openocl.org/bsd-3-clause/>
 
 function ocl
-  ocl.utils.StartupOCL
+  ocl.utils.startup