%clc;
clear all
run ../../parametry.m

%% návrh NQR kontroleru
Q =diag([5 50 50 700 700 700])*10;
%Q =diag([100 50 50 10 10 10]);
%Q =diag([700, 0, 0, 0, 0, 0])*30 % fungoval dobre pro stabilizaci
%Q=diag([110 110 110 0 0 0])
R =1;
Nh =40;
dt= 0.001 ;

%po kolika stupnich se bude linearizovat
dist=10;
%kolik na kazdou stranu
N=5;
%velikost matice kontroleru
N2=(N-1)*2+1;
linPt=zeros(6,N2,N2)

%linearizace a navrh LQR
for i=1:N2
    for j=1:N2
    linPt(2,i,j)=deg2rad((i-N)*dist);
    linPt(3,i,j)=deg2rad((j-N)*dist);

[a,b,c,d]=linmod('lagrangeonly',linPt(:,i,j),0);
sys=ss(a,b,c,d);

k(i,:,j)=lqr(sys,Q,R);
    fprintf('%i from %i\r\n',(i-1)*N2+j, N2^2);
    end
end

%% navrh LQR kontroleru
[a,b,c,d]=linmod('lagrangeonly',linPt(:,i,j),0);
sys=ss(a,b,c,d);
k_LQR=lqr(sys,Q,R);

%%

k1=0;

save('LQR_structure');
run LQR_multiple_visualisation.m