%clc;
run ../../parametry.m
dt=0.005;
tf= 2.5;

%% pocatecni podminky
th1i  = deg2rad(3); 
th2i  = deg2rad(-3);
th0i  = 0.0;

%% definice statespace
%systemove matice kolem [0 0 0 0 0 0]

d0=[d1 d2 d3; d2 d4 d5; d3 d5 d6];
dg=[0 0 0;0 -f1 0; 0 0 -f2];

a=[zeros(3),eye(3);-inv(d0)*dg,zeros(3)];
b=[zeros(3,1);inv(d0)*[1;0;0]];
c=eye(3,6);
d=[0;0;0];
sys=ss(a,b,c,d);

sysd=c2d(sys,dt);

%% Discretize and get Augmented System
a=[zeros(3),eye(3);-inv(d0)*dg,zeros(3)];
b=[zeros(3,1);inv(d0)*[1;0;0]];
c=eye(6);
sys=ss(a,b,c,0);
sysd=c2d(sys,0.002,'zoh');
[ad,bd,cd,dd]=ssdata(sysd);
ad_comp=[ad, zeros(6);cd * ad, eye(6)];
bd_comp=[bd;cd * bd];
cd_comp=[zeros(6),eye(6)];

%% Design MPC Using Laguerre Functions
a=0.4;
beta=sqrt(1-a^2);
Np=1000;
Nc=200;
Q =diag([100000 11 11 0 0 0 100000 11 11 0 0 0]);
% Create Laguerre Matrix
[A1,L0T]=lagd(a,Nc);
clear L
L(:,1)=L0T;
for kk=2:Np;
L(:,kk)=A1 * L(:,kk-1);
end

%% Define Psi and Omega
psi=0;
omega=0;
for i=1:Np
f=0;
lag=[];
for j=0:i-1
f=f+ad_comp^(i-1-j) * bd_comp * L(:,j+1)';
end
psi=psi+f' * Q * ad_comp^i;
omega=omega+f' * Q * f;
end

k=L0T' * pinv(omega) * psi;
k=k(1:6)


%% simulace

open('doublePendulumSim_model_Laguerre.slx');
sim('doublePendulumSim_model_Laguerre.slx');
%simout.Data(end)