% Navrhne trajektorii pro kyvadlo. Je možno náhodně vybrat čtveřici
% parametrů a pak je ladit, jeden po druhém. Výsledek lze uložit pro další
% zpracování.

% spoustet po sekcich!!!
%napr. 1 2 3 4 5 5 4 5 5 6 6 5 7 5 5 5 5 5 9 ...

%% 1 inicializace
clc; clear all; 

run ../../parametry.m
model_name='doublePendulumSim_model_optimalizaceB.slx';
tc0=0.002;
qa0=0.00036;

%% 2 pocatecni podminky a parametry
th1i  = deg2rad(180); 
th2i  = deg2rad(180+0.00001); 

dt_su=0.001;
tf=1.5;
t=[0:dt_su:tf];
log_p=[];
p_diff=zeros(1,4);

Ep=0.3;


%% 3 nahodne parametry
close_system(model_name,0);
opak=50;
p2=rand(opak,4)*0.2-0.1;
p_diff=[0 0 0 0];
for k=1:size(p2,1)
    a=-p2(k,1)-p2(k,3)+(-p2(k,2)-p2(k,4))*cos(pi*t/tf);
    for i=2:5
        a=a+p2(k,i-1)*cos(i*pi*t/tf);
    end
    a2=diff(a)/dt_su;
    a3=diff(a2)/dt_su;
    acc=[t(1:end-2);a3]';
    sim(model_name);
    lagr(k)=min(lagrange.Data(lagrange.Time>tf-0.4&lagrange.Time<tf));
    clc
    fprintf('%d z %d',k,opak);
end
[lagr,idx]=sort(lagr);
p=p2(idx(1),:);
lagr(1)
lagr_prev=lagr(1);
log_p=p;


%% 4 update hodnotici funkce
Ep=5; %na konci 1.5, na yacatku 0.8

close_system(model_name,0);
sim(model_name);
lagr_prev=min(lagrange.Data)
lagr=lagr_prev;


%% 5 nahodne parametry kolem viteze

close_system(model_name,0);
opak=200;
lagr_first=lagr_prev;
lagr_target=-(f1+f2)*Ep;
momentum=8;
for i=1:opak
    i
    lagr
    lagr_first
    lagr_prev
    lagr_target
    p3=p+p_diff/momentum*0.4*(rand/1.5+0.33)+rand(1,4)*0.0005-0.00025; %momentum 0.4, krok 0.002

    a=-p3(1)-p3(3)+(-p3(2)-p3(4))*cos(pi*t/tf);
    for j=2:5
        a=a+p3(j-1)*cos(j*pi*t/tf);
    end
    a2=diff(a)/dt_su;
    a3=diff(a2)/dt_su;
    acc=[t(1:end-2);a3]';
    sim(model_name);
    lagr=min(lagrange.Data);
    if lagr<lagr_prev
        p=p3;
        if size(log_p,1)>momentum
            p_diff=(p-log_p(end-momentum,:))/momentum;
        end
        lagr_prev=lagr;
        log_p=[log_p;p];
    end
    clc
end
    lagr_first
    lagr_prev
    lagr_target
%%  6 ladeni viteze
clearvars lagr
close_system(model_name,0);

%tuned parameter
tuning.idx=4;
tuning.res=0.0005;
tuning.range=tuning.res*5;
tuning.orig=p(tuning.idx);
opak=tuning.range/tuning.res*2+1;

for j=1:tuning.range/tuning.res*2+1
    p(tuning.idx)=tuning.orig-tuning.range+tuning.res*(j-1);

    a=-p(1)-p(3)+(-p(2)-p(4))*cos(pi*t/tf);
    for i=2:5
        a=a+p(i-1)*cos(i*pi*t/tf);
    end
    a2=diff(a)/dt_su;
    a3=diff(a2)/dt_su;
    acc=[t(1:end-2);a3]';

    sim(model_name);
    clc
    lagr(j)=min(lagrange.Data);
    fprintf('%d z %d',j,opak);
end
[tuning.best,tuning.bestidx]=min(lagr);
p(tuning.idx)=tuning.orig-tuning.range+tuning.res*(tuning.bestidx-1);
clearvars tuning
lagr
min(lagr)
%% 7 
p=p2(idx(7),:);
    
%% 8 vizualizace
a=-p(1)-p(3)+(-p(2)-p(4))*cos(pi*t/tf);
for i=2:5
    a=a+p(i-1)*cos(i*pi*t/tf);
end
a2=[diff(a)/dt_su 0];
a3=[diff(a2)/dt_su 0];
acc=[t(1:end) ; a3]';

dt=0.001;
run(model_name);
sim(model_name);
dt=0.001;
lagr=min(lagrange.Data);

%% 9 uložení

save('traj91','p','dt','t','tf','th1i','th2i','acc','stav')
