function simple_car_CasADI
%% Optimal Control (EE-715)
% Course at EPFL
% Example - Direct single shooting
%
%
% Autor:    timm.faulwasser@kit.edu
% Date:     11-06-17
% Status:   final

clc
close all
tic

opti = casadi.Opti()
N = 100; % number of shooting intervals
tf = 10.5; % final time

% ode
ode = @(x,u) [x(2); u; u^2];

% state: position
x_1 = opti.variable(N+1,1);

% state: speed
x_2 = opti.variable(N+1,1);

% state: cost
x_3 = opti.variable(N+1,1);

% control: 
u = opti.variable(N,1);


% construct list of all constraints
g = {};

for k=1:N
   xk      = [x_1(k);   x_2(k); x_3(k)  ];
   xk_plus = [x_1(k+1); x_2(k+1); x_3(k+1)];
   
   % shooting constraint
   xf = rk4(ode,tf/N,xk,u(k));
   g = {g{:}, xk_plus==xf};
end

% initialize optimization with position 1.
opti.set_initial(x_1, 1);

% input box constraint
opti.subject_to(-1 <= u <= 1);

g = {g{:}, x_1(1)==1, x_2(1)==0, x_3(1) == 0, x_1(end)==0, x_2(end) == 0};
display(['Time for poblem construction:' , num2str(toc)])

tic
opti.subject_to(g);
opti.minimize(x_3(end));
opti.solver('ipopt'); % set numerical backend
sol = opti.solve(); 

%optisolve(x_3(end),g);
display(['Time for poblem solution:' , num2str(toc)])



figure
hold on
plot(sol.value(x_1));
plot(sol.value(x_2));
stairs(sol.value(u),'b');
legend('x_1', 'x_2', 'u');
grid on

end

%% subfunction for integration
function xf = rk4(ode,h,x,u)
  k1 = ode(x,       u);
  k2 = ode(x+h/2*k1,u);
  k3 = ode(x+h/2*k2,u);
  k4 = ode(x+h*k3,  u);
  xf = x + h/6 * (k1 + 2*k2 + 2*k3 + k4); 
end



