% BSD 2-Clause License

% Copyright (c) 2022, Armin Nurkanović, Jonathan Frey, Anton Pozharskiy, Moritz Diehl

% 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.

% This file is part of NOSNOC.

%
%

%% Switch positions
 
clear all;
close all;
clc;
import casadi.*

filename = 'discs_switch_position.gif';
%% constants
r1 = 0.3;
r2 = 0.2;
q_target1 = [1;0];
q_target2 = [-1; 0];
%% discretizatioon
T = 3;
N_stg = 30; % control intervals
N_FE = 3;  % integration steps per control interval

%%
problem_options = NosnocProblemOptions();
solver_options = NosnocSolverOptions();
problem_options.irk_scheme = IRKSchemes.RADAU_IIA;
problem_options.n_s = 1;  
problem_options.time_freezing = 1;
problem_options.pss_lift_step_functions = 0;
problem_options.N_stages = N_stg;
problem_options.N_finite_elements = N_FE;
problem_options.T = T;
problem_options.dcs_mode = DcsMode.Step;
solver_options.mpcc_mode = MpccMode.elastic_ineq;
solver_options.objective_scaling_direct = 0;
solver_options.comp_tol = 1e-6;
solver_options.N_homotopy = 7;
solver_options.opts_casadi_nlp.ipopt.max_iter = 2e3;
solver_options.print_level = 5;
problem_options.g_path_at_fe = 1;
problem_options.g_path_at_stg = 1;

%% IF HLS solvers for Ipopt installed (check https://www.hsl.rl.ac.uk/catalogue/ and casadi.org for instructions) use the settings below for better perfmonace:
%solver_options.opts_casadi_nlp.ipopt.linear_solver = 'ma57';

model = get_disc_model(r1, r2, q_target1, q_target2);

%% Call nosnoc solver
mpcc = NosnocMPCC(problem_options, model);
solver = NosnocSolver(mpcc, solver_options);
[results,stats] = solver.solve();
%% read and plot results
plot_disc(results, problem_options, r1, r2, q_target1, q_target2, filename);
