% udělej měření kdy se jen kývají obě kyvadla.
run ../../parametry.m
load dvojite.mat
dt=0.001;

%kompletace dat
simout=log_phi;
simout.signals(3).values=simout.signals(3).values+simout.signals(2).values;
simout.signals(4)=log_dphi.signals(1);
simout.signals(5).values=-log_phi.signals(2).values;
simout.signals(5)=log_dphi.signals(2);
simout.signals(6)=log_dphi.signals(3);
simout.signals(6).values=smooth([0; diff(simout.signals(3).values)/dt],20);
simout.signals(5).values=smooth([0; diff(simout.signals(2).values)/dt],20);

simoutB=simout;
for i=1:6
    simoutB.signals(i).values=simoutB.signals(i).values(simoutB.time>10);
end
simoutB.time=simoutB.time(simoutB.time>10)-simoutB.time(1);
%plot(simoutB.time, simoutB.signals(2).values,simoutB.time, simoutB.signals(3).values,simoutB.time, simoutB.signals(5).values,simoutB.time, simoutB.signals(6).values)
% tady si dej plot(simout) a najdi bod, v němž je derivace 0, to napiš do
% t0
%%

%pocatecni podminky
t0=9.49;
t0=16;
tf2=5;

%t0=2;tf2=0.001;

% priprava dat pro vlozeni do simulinku
data=[];
for i=1:6
    data=[data simout.signals(i).values(simout.time>t0&simout.time<=t0+tf2,:)];
end
time=[simout.time(simout.time>t0&simout.time<=t0+tf2,:)-t0];

% th1i=deg2rad(data(1,2))+pi;
% th2i=deg2rad(data(1,3))+pi;
% dth1i=deg2rad(data(1,5));
% dth2i=deg2rad(data(1,6));

experiment = timeseries(data, time); %prvni kyvadlo

N=1;
NN=floor(N/2);
kk=0.1;
k=0;
if N~=1
    k=linspace(-kk,kk,N);
end
log_goodness=[];
for ii=1:N
    close_system('doublePendulumSim_model_optim2015b.slx',0)
    run ../../parametry.m
th1i=deg2rad(simout.signals(2).values(simout.time==t0))+pi;
th2i=deg2rad(simout.signals(3).values(simout.time==t0))+pi;
dth1i=deg2rad(simout.signals(5).values(simout.time==t0));
dth2i=deg2rad(simout.signals(6).values(simout.time==t0));

%hrani s parametry. u toho parametru co chces ladit, pridej +k(ii).

b1s=0.000208;
b1c=0.00030;
b2c=0.000156;
b2s=0.000276;

k1=1.154+k(ii);
k2=1.046;
k1=1;k2=1;
I1=I1*k1;
m1=m1*sqrt(k1)*k2;
l1=l1*sqrt(k1)/k2;

k3=1;k4=1;
k3=0.976;
k4=0.996;
I2=I2*k3;
m2=m2*sqrt(k3)*k4;
l2=l2*sqrt(k3)/k4;

L1=L1;
run ../../vypocty.m

%run doublePendulumSim_model_verify.slx
sim doublePendulumSim_model_optim_2015b.slx
log_goodness(ii)=goodness.Data(end);
clc
ii
end
log_goodness
k

%%
simout2.Data(:,1)=simout2.Data(:,1)-pi;
simout2.Data(:,2)=simout2.Data(:,2)-pi;
plot(experiment/180*pi,'r')
hold on
plot(simout2,'b')
hold off
legend('mereni','simulace')