clear all;
clear all;
clc;
import casadi.*
close all

friction = true;

problem_options = NosnocProblemOptions();
solver_options = NosnocSolverOptions();
%%
solver_options.print_level = 3;
solver_options.N_homotopy = 15;
solver_options.mpcc_mode = MpccMode.Scholtes_ineq;
solver_options.comp_tol = 1e-6;

%%
problem_options.irk_scheme = IRKSchemes.RADAU_IIA;
problem_options.dcs_mode = DcsMode.Step;
problem_options.cross_comp_mode = 3;
problem_options.impose_terminal_phyisical_time = 1;
problem_options.local_speed_of_time_variable = 1;
problem_options.stagewise_clock_constraint = 0;
problem_options.time_freezing = 1;
problem_options.pss_lift_step_functions = 0;

%% get model

model = get_particle_model(friction);

%% Simulation settings
N_FE = 5;
T_sim = 2.5;
N_sim = 20;
problem_options.T_sim = T_sim;
problem_options.N_sim = N_sim;
problem_options.N_finite_elements = N_FE;
solver_options.use_previous_solution_as_initial_guess = 0;
%% Call nosnoc Integrator
integrator = NosnocIntegrator(model, problem_options, solver_options, [], []);
[results,stats] = integrator.solve();
%% read and plot results
plot_particle(results, friction);
