%Funkce pro optimální nalezení parametrů geometrické transformace rojem                  %
% částic PSO                                                                             %
%vystup - parametry transformace, gbest_fit - hodnota kriteriální funkce                 %
%vstup - obraz1, obraz2, [parametry modelu]                                              %
%příklad spuštění: [vystup,gbest_fit] = PSO_MSE(ref_obraz,transf_obraz,20,50,10,1,1,0.6) %
% ______________________________________________________________________ %

function [vystup,gbest_fit] = PSO_MSE(ref_obraz,transf_obraz,n_jedincu,maxit,xmax,c1,c2,W)
obr1=ref_obraz;
obr2=transf_obraz;
%okno=[100,200,90,330];
% výběr obdélníkového okna
imshow(ref_obraz)
title('Tažením myši vyberte obdélníkovou oblast pro registraci')
h=imrect()
pos = h.getPosition();
pos_rect = round(pos);
okno=[pos_rect(2),pos_rect(2)+pos_rect(4),pos_rect(1),pos_rect(1)+pos_rect(3)];

%% definice kriterialni fce
fcn = @(x,y) immse(x,y);  %MSE
%%
%n_jedincu=n_jedincu; % pocet castic
D=3; % dimenzionalita
%c1=2; % konstanta c1 0.5
%c2=4-c1; % konstanta c2 0.9
%W=0.5; % konstanta W
%maxit=20; % maximum iteraci
ite=1; % aktualni iterace
min_xy=[-10, -10]; % minima pro x a y
max_xy=[10, 10]; % maxima pro x a y
%xmax=10;
%castice_best=cell(1,n_jedincu); % zde její nejlepší pozice
pbest_pos=cell(1,n_jedincu); % zde její nejlepší pozice
pbest_fit=ones(1,n_jedincu)*1E50; % nejlepsi pozice castice
gbest_fit=1E50;
gbest_pos=zeros(1,D); %globalni nejlepsi pozice
pos = cell(1,n_jedincu); % pozice castice               
krit=zeros(1,n_jedincu); % hodnota kvality
rychlost = cell(1,n_jedincu); % rychlost castice
%% inicializace algoritmu????
LIST = zeros(1,length(D)); 

for m = 1:n_jedincu
    for n = 1:D
        LIST(n) = rand()*xmax;
    end
    pos{m} = LIST;
    rychlost{m} = pos{m};

end


%%
%kvalita_vsech=zeros(n_jedincu,maxit);
while ite<=maxit
    for i=1:n_jedincu% pro kazdou castici
    p=pos{i}; %pozice castice
    pom=imtranslate(obr2,[p(1) p(2)]);
    pom=imrotate(pom,p(3),'bilinear','crop');
    krit(i)=fcn(obr1(okno(1):okno(2),okno(3):okno(4)),pom(okno(1):okno(2),okno(3):okno(4)));
        if krit(i)<pbest_fit(i) % vyhodnotim zlepseni castice
             pbest_fit(i)=krit(i);
             pbest_pos(i)=pos(i);
        end
    end
     if min(pbest_fit)<gbest_fit %nejlepsi globalni pozice
         gbest_pos=pos{krit==min(pbest_fit)};
         gbest_fit=min(krit);
     end
     
     
  for ii=1:n_jedincu
      s1{ii}=W*rychlost{ii};
      s2{ii}=c1*rand()*(pbest_pos{ii}-pos{ii});
      s3{ii}=c2*rand()*(gbest_pos-pos{ii});
     
     rychlost{ii}= s1{ii}+s2{ii}+s3{ii}; % prepocet rychlosti
     %rychlost{ii} = W*rychlost{ii}+c1*rand()*(pbest_pos{ii}-pos{ii})/DT+c2*rand()*(gbest_pos-pos{ii})/DT
     pos{ii}=pos{ii}+rychlost{ii};% uprava pozice
  end
   %kvalita_vsech(1:n_jedincu,ite) = pbest_fit(:); % ulozeni vsech kvalit
   %[a b] = max(pbest_fit);
   
   ite=ite+1;
end

%disp(['nejlepsi pozice castice je ', num2str(gbest_pos(1)) ,' ',  num2str(gbest_pos(2)), ' ', num2str(gbest_pos(3))]);
vystup=[gbest_pos(1),gbest_pos(2),gbest_pos(3)];

end