%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% (CHEM-E7225) ADVANCED PROCESS CONTROL D
%   #00 - Introduction to CasADi (Part 2)
%
%   CasADi is a tool to quickly implement and efficiently solve dynamic 
%   optimisation (optimal control) problems. You can also think of it as 
%   a general toolkit for gradient-based optimisation
%
%   Casadi is available in C++, Python, and Matlab/Octave
%   (There used to be an interface to Haskell, too)
%
%   The numerical back-end for optimisation is IPOPT, plus others
%   The numerical back-end for simulation is SUNDIALS
%
%   CasADI is open-source, LGPL-licensed
%   Download from https://web.casadi.org  (current version: 3.6.4)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all; clear; clc

addpath('~/Documents/MATLAB/casadi')    % --> addpath('~/change_me_with_path_to/casadi-v3.6.4')
import casadi.*

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FUNCTIONS OF SCALAR EXPRESSIONS
%  A function is created by passing a structure (python list) of input 
%  expressions and a structure of output expressions 

% Create a scalar function f: R x R -> R with f(x,y) = x * sin(x+y) and 
% evaluate it for some pair of values like (x,y) = (1,1)

x = MX.sym('x');                % Symbolic argument, x
y = MX.sym('y');                % Symbolic argument, y
z = x * sin(x+y);               % Scalar expression, z

f = Function('f',{x,y},{z});    % A "Function" object, f : (x,y) |-> y
display(f)                      % A DM type is like SX in which the non-zero

f_11 = f(1,1);                  % The result of f(x,y), with (x,y) = (1,1)
display(f_11)                   % elements are not symbolic, but numerical

whos f f_11                     

% Alternative, more explicit, formulation...

f = Function('f', {x,y}, {z}, {'x','y'}, {'z'});    % A "Function" object, f : (x,y) |-> y
f_res = f('x',1, 'y',1);                            % The result of f(x,y), as a struct object
f_11  = f(1,1);                                     % The result of f(x,y), as a DM object

display(f)
display(f_res.z)
whos f f_res f_11

% Create a vector valued function g: R x R -> R^2  
%           
% g(x,y) =  | x * sin(x + y) |
%           | y * cos(x + y) |
%
% Evaluate g for some pair of values like (x,y) = (1,1)

% Scalar form, each output as a separated scalar:
g = Function('g', {x,y}, {z, y * cos(x+y)}, {'x','y'}, {'g1','g2'});
[g_1, g_2] = g(1,1);

display(g)
display(g_1), display(g_2)
whos g g_1 g_2

% Vector form, single output as a vector (more common):
g = Function('g',{x,y},{[z; y * cos(x+y)]},{'x','y'},{'g(x,y)'});
g_12 = g(1,1);

display(g)
display(g_12)
whos g g_12


%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FUNCTIONS WITH MATRIX ARGUMENTS

% Create a scalar function f: R^2 -> R with f(x) = 100(x2 - x1)^2 + (1 - x1)^2
% and create functions for its Gradient (gf: R^2 -> R^2) and Hessian (Hf: R^2 -> R^{2x2})

x = MX.sym('x',2);

y = 100*(x(2) - x(1)^2)^2 + (1 - x(1))^2;

f  = Function('f',{x},{y});
gf = Function('gf',{x},{gradient(y,x)});
Hf = Function('Hf',{x},{hessian(y,x)});

display(f)
display(gf)
display(Hf)
whos f gf Hf

% Evaluate f, gf and Hf for a some value like x = [0 0]'

x0 = [0 0]';

f_x0  = f(x0); 
gf_x0 = gf(x0);
Hf_x0 = Hf(x0);

display(f_x0)
display(gf_x0)
display(Hf_x0)
whos f_x0 gf_x0 Hf_x0


%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HANDS-ON EXERCISE #01 (Differential equations)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Write a function that evaluates the dynamics xplus = f(t,x,u)
% given by the vector-valued function
%
%   f(t,x,u) =  | x1_plus | = | (1-x2^2)*x1 - x2 + u |
%               | x2_plus | = | x1                   |
%               | x3_plus | = | x1^2 + x2^2 + u^2    |
%
% then find the Jacobians (df/dx) and (df/du), and
% evaluate these functions at t=0, x=[0.5,-1,2], u=[2].

t = MX.sym('t',1);      % Time 
u = MX.sym('u',1);      % A control variable
x = MX.sym('x',3);      % Three state variables 

% The dynamics (as symbolic expressions)
xplus = ???;

% The dynamics (as a function)
f = ???;

% The Jacobian matrices (as symbolic expressions)
dfdx = ???;
dfdu = ???;

% The Jacobians matrices (as functions)
Jx = ???;
Ju = ???;

% Evaluate the function and Jacobians
t_ = 0;  x_ = [0.5, -1, 2]; u_ = [2];

fplus  = ???;
Jxplus = ???;
Juplus = ???;

display( full(fplus ) )
display( full(Jxplus) )
display( full(Juplus) )


%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HANDS-ON EXERCISE #02 (Differential equations)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Write the following expresions using the MX type,
%
%   x(1) = x0
%   x(2) = A*x(1) + B*u(1)
%   x(3) = A*x(2) + B*u(2)
%       ...
%   x(N) = A*x(N-1) + B*u(N-1)
%
% given a 'horizon' N, matrices (A,B), and a symbol x0. (Play with the sizes!)
% Then compute and inspect the Jacobian Jx = (dx / dx0)

N = 10;
nx = 2;  nu = 1; 

A  = randn(nx,nx); A = A + eye(nx).*(min(eig(A)));
B  = randn(nx,nu);
u  = MX.sym('u',nu,N-1);
x0 = MX.sym('x0',nx);

% Main loop
x = [x0];
for k = 1:N-1
    x = [x, A*x(:,k) + B*u(:,k)];
end

% Creates a function and evaluates the entire horizon
F = ???;

X_horizon = ???;

figure(1), clf
 stairs(full(X_horizon)')
 legend('x1', 'x2')

% -- --