% udělej měření kdy se jen kývají obě kyvadla.
run ../../parametry.m
load opakovatelnost.mat

dt=0.001;
%sem zadej cislo nejtypictejsiho predstavitele
simout=log_stav.mereni(3);
simin=timeseries(simout.signals.values,simout.time);
hold off
%plot(simin)

%% 
%pocatecni podminky
t0=0;
tf2=1.45;
tf=tf2;
tc0=0.002;
qa0=0.00036;


% N doporucuji 11 kdyz provadim ladeni nebo 1 kdyz chci jen ukazat nejlepsi
% vysledek.
% promenna co meni svoji hodnotu je'k(ii)'. Pro ten parametr, ktery chci 
% ladit, doporucuju vynasobit faktorem '(1+k(ii))'.

N=1;
NN=floor(N/2);
kk=0.01;
k=0;
Ep=0;
dt_su=0.001;
if N~=1
    k=linspace(-kk,kk,N);
end
log_goodness=[];
for ii=1:N
    close_system('doublePendulumSim_model_optim_parametru.slx',0)
    run ../../parametry.m
    
    
th1i=-pi;
th2i=-pi+0.000001;
dth1i=0;
dth2i=0;


%hrani s parametry. u toho parametru co chces ladit, pridej +k(ii).
b1c=b1c*(0.6);
b2c=b2c*(1);
b2s=b2s*(1);
b1s=b1s*(1);

K1=-0.0084;

k1=1.070+k(ii);
k2=0.86;
% k1=1;k2=1;
I1=I1*k1;
m1=m1*sqrt(k1)*k2;
l1=l1*sqrt(k1)/k2;

k3=0.972;
k4=1;
% k3=1;k4=1;
I2=I2*k3;
m2=m2*sqrt(k3)*k4;
l2=l2*sqrt(k3)/k4;

L1=L1*(0.94);
P=250*(1);
I=250*(0.968);
run ../../vypocty.m

%run doublePendulumSim_model_verify.slx
sim doublePendulumSim_model_optim_parametru.slx
log_goodness(ii)=goodness.Data(end);
clc
ii
end
log_goodness
k
save('parametry_naladene','m0','m1','m2','l1','l2','L1','L2','I1','I2','b0','b1c','b2c','b1s','b2s','P','I','g','K1','maxpow','th0i','th1i','th2i','dth0i','dth1i','dth2i')