% Algorytm mrowkowy poszukiwania minimalnej sciezki
% 

clear
par = 0.01; %wsplczynnik parowania
fp = 5;        % stala ilosc feromonu w kazdym polu;
nazwa_pliku = 'labirynt4.bmp'
liczba_przejsc = 1000; 
liczba_krokow_max = 100;
% liczba_mowek = ...     % liczba mrowek idacych rownolegle
% fp = ...        % stala ilosc feromonu w kazdym polu;
% n = ...         % wspolczynnik nacisku selekcyjnego (im wiekszy, tym wieksze prawd. wyboru kierunku z wieksza iloscia feromonu)
% par = ...   % wspolczynik parowania feromonu (zmniejszenie feromonu na kazde przejscie)

sledzenie = 1;    % tryb sledzenia ilosci feromonu i dlugosci sciezki
obieg=1;
t = imread(nazwa_pliku);

imagesc(t);
mapa_kol = [[1:256];[1:256];[1:256]]'/256;
colormap(mapa_kol);
title(sprintf('labirynt %s nacisnij klawisz ...',nazwa_pliku));

pause

[lwierszy lkolumn] = size(t);

% zakladamy, ze start odbywa sie ze srodka pierwszej kolumny, meta jest na srodku ostatniej kolumny 
start = [ceil(lwierszy/2),1]
meta = [ceil(lwierszy/2),lkolumn]

f = zeros(lwierszy,lkolumn);         % tablica feromonu 

for i=1:liczba_przejsc
    krok = 1;
    pole = start;
    pole_pop = start;
    sciezka = [];
    while (krok < liczba_krokow_max)&&((pole(1) ~= meta(1))||(pole(2) ~= meta(2)))
        % wyznaczenie dostepnych pol, w ktore mozna wejsc:
        % zakladamy, ze mrowka nie moze wyjsc poza plansze, nie moze 
        % wejsc na sciane oraz nie cofa sie na pole z ktorego przyszla
        % (chyba ze nie ma innej mozliwosci)
        pola_dost = [];
        ilosci_fer = [];
        if (pole(2) > 1) && (t(pole(1),pole(2)-1)==1) && (pole(2)-1 ~= pole_pop(2))
            pola_dost = [pola_dost ; pole-[0 1]];
            ilosci_fer = [ilosci_fer f(pole(1),pole(2)-1)];
        end
        if (pole(2) < lkolumn) && (t(pole(1),pole(2)+1)==1) && (pole(2)+1 ~= pole_pop(2))
            pola_dost = [pola_dost ; pole+[0 1]];
            ilosci_fer = [ilosci_fer f(pole(1),pole(2)+1)];
        end
        if (pole(1) > 1) && (t(pole(1)-1,pole(2))==1) && (pole(1)-1 ~= pole_pop(1))
            pola_dost = [pola_dost ; pole-[1 0]];
            ilosci_fer = [ilosci_fer f(pole(1)-1,pole(2))];
        end
        if (pole(1) < lwierszy) && (t(pole(1)+1,pole(2))==1) && (pole(1)+1 ~= pole_pop(1))
            pola_dost = [pola_dost ; pole+[1 0]];
            ilosci_fer = [ilosci_fer f(pole(1)+1,pole(2))];
        end
        liczba_dost = length(ilosci_fer);
pola_dost
        
        if liczba_dost == 0
            p = pole; 
            pole = pole_pop;                          % cofniecie sie do pola poprzedniego, jesli brak innych dostepnych pol
            pole_pop = p;
        else
            pole_pop = pole;     
            % pole poprzednie (by do niego ponownie nie wejsc)
            % ..............  TUTAJ NALEZY WYBRAC NOWE POLE ...........
            % pole =
            
            % zeby nie bylo bledu - wybor pole pierwszego z brzegu
            % pole = pola_dost(1,:);
            

                
                 randomowa_liczba=rand()*100;
                feromon_calk=0;
                feromon_calk=sum(ilosci_fer);
                
                if feromon_calk == 0
 liczba=round(length(ilosci_fer)*rand());
 if liczba == 0
     liczba=1;
 end
                pole= pola_dost(liczba,:);
                else

                    p1(1) = 1;
                    p2(1) = ((ilosci_fer(1)+1) * 100)/(feromon_calk+length(ilosci_fer))
                    for a=1:length(ilosci_fer)
                        if a == 1
                         p1(1) = 1;
                         p2(1) = ((ilosci_fer(1)+1) * 100)/(feromon_calk+length(ilosci_fer))
                        else
                            
                        p1(a)=p2(a-1)
                        p2(a)=p1(a)+((ilosci_fer(a)+1)*100)/(feromon_calk+length(ilosci_fer))
                        end
                    end

                    for a=1:length(ilosci_fer)
                       if randomowa_liczba >= p1(a) && randomowa_liczba <= p2(a)
                           pole = pola_dost(a,:);
                       end
                    end
                end
            
        end
            
        sciezka(krok,:) = pole;                     % wpisanie pola do sciezki       
        krok = krok + 1;
    end  % while po krokach mrowki
    
    if (pole(1) == meta(1))&&(pole(2) == meta(2))  % jesli koncowym polem jest meta
        % usuwanie petli (moze byc lub nie):
        while i <= length(sciezka(:,1))
            pole = sciezka(i,:);
            for j=length(sciezka(:,1)):-1:i+1
                if (sciezka(j,1)==pole(1))&&(sciezka(j,2)==pole(2))
                    sciezka(i+1:j,:) = [];
                    break;
                end
            end
            i = i+1;
        end

        % ............... TUTAJ NALEZY ZMODYFIKOWAC TABLICE FEROMONU
        % f =
        for a = 1:length(sciezka)  
            f(sciezka(a, 1), sciezka(a,2)) = f(sciezka(a, 1), sciezka(a,2)) +  10/length(sciezka);
        end
        f = f*(1-0.05);
    end
    
        
    if sledzenie
        imagesc(1-f/max(max(f))*0.9);
        mapa_kol = [[1:256];[1:256];[1:256]]'/256;
        sciezka_max;
        title(sprintf('pozostaly feromon, dlugosc sciezki = %d',dlugosc_sciezki))
        pause(0.1);
    end
    
end

%fig = figure;
imagesc(1-f/max(max(f))*0.9);
mapa_kol = [[1:256];[1:256];[1:256]]'/256;
sciezka_max;
title(sprintf('pozostaly feromon, dlugosc sciezki = %d',dlugosc_sciezki))