clc;                                    % clear command window/console
clear;                                  % clear workspace                                      
close all;                              % close all windows (plots)

import casadi.*

% parameters
h = 10;                                 % throwing height (in m)
vmax = 20;                              % maximal throwing speed (in m/s)
g = 9.81;                               % gravity acceleration (in m / s^2)

% new optimization problem 
opti = Opti();

% Declare decision variables
vbar = opti.variable(2);                % throwing velocity
T = opti.variable();                    % time until ball hits ground

% intermediate expressions
pT_x = T * vbar(1);						% x position at time T
pT_z = h + T * vbar(2) - .5 * g * T^2;  % z position at time T

% objective
opti.minimize(-pT_x)                    % minimizing negative x position is same as maximizing x position

% constraints
opti.subject_to( pT_z == 0  );                  % ball is on ground at end
opti.subject_to( vbar' * vbar <= vmax^2  );		% maximal throwing speed
opti.subject_to( T >= 0 );						% time has to be positive

opti.solver('ipopt');                           % use ipopt as solve
sol = opti.solve();                             % actually solve the problem

% read optimal values
vopt = sol.value(vbar);
Topt = sol.value(T);
pT_x_opt = sol.value(pT_x);

% optimal throwing angle in degree
alpha_opt = atan( vopt(2) / vopt(1) )  * 180 / pi;		

disp(' ');
disp(['optimal throwing angle is ', num2str(alpha_opt), ' deg.'] );
disp(['x distance reached is ',  num2str(pT_x_opt), ' m.']);
disp(['Ball hits ground after ', num2str(Topt), ' s.']);


