clc; close all; clear all;
import casadi.*
%% settings
% collocation settings
problem_options = NosnocProblemOptions();
solver_options = NosnocSolverOptions();
problem_options.n_s = 2;
% problem_options.time_freezing_time_rescaling = 1;
problem_options.use_speed_of_time_variables =  1; 
problem_options.local_speed_of_time_variable = 1;  
problem_options.stagewise_clock_constraint = 1;
problem_options.time_freezing = 1;
problem_options.lift_complementarities = 0;
problem_options.N_stages = 40;
problem_options.N_finite_elements = 4;
problem_options.cross_comp_mode = 7;
solver_options.N_homotopy = 6;
solver_options.mpcc_mode = MpccMode.elastic_eq;
solver_options.sigma_0 = 1e-1;
solver_options.objective_scaling_direct = 0;
solver_options.comp_tol = 1e-6;
solver_options.print_level = 5;
solver_options.opts_casadi_nlp.ipopt.max_iter = 1e3;
solver_options.opts_casadi_nlp.ipopt.tol = 1e-5;
solver_options.opts_casadi_nlp.ipopt.acceptable_tol = 1e-5;
solver_options.opts_casadi_nlp.ipopt.acceptable_iter = 3;
%% Generate Model
% angulary velocity of reference
omega = -2*pi; % no impacts
omega = -3*pi; % impacts
[model, ref] = elastic_ball_in_box_model(omega, problem_options);
%% Solve OCP via nosnoc
mpcc = NosnocMPCC(problem_options, model);
solver = NosnocSolver(mpcc, solver_options);
x_guess = {};
for ii = 1:problem_options.N_stages
    x_guess{ii} = ref(:,ii+1);
end
%solver.set('x', x_guess');

[results,stats] = solver.solve();
%% Read and plot Result
unfold_struct(results,'base');
unfold_struct(model,'base');
plot_results_elastic_ball_in_box
