function [a,b,omega1,omega2,brojac] = funkcija(a_staro,b_staro,t,A,B,brojac_stari)
% ova funkcija treba na osnovu ulaza A i B da izracuna ugaonu brzinu i
% ugaoni pomak motora

% a, b, a_staro, b_staro vektori nx1
% brojac, brojac_stari vektori 2x1
% A, B, t, omega1, omega2 skalari

% ovdje pisete vas kod

P=2;
Ts=1e-4;
fc=10000;
Td=1000*Ts;
if brojac_stari(2)~=0
    S2=brojac_stari(2);
else
    S2=Inf;
end

if t==0 % moramo definirati neke pocetne vrijednosti izlaza
    a=A*ones(1000,1);
    b=B*ones(1000,1);
    omega1=0;
    omega2=0;
    brojac=zeros(2,1);
else
    % ovdje pisete ostatak koda
    % mora uvijek biti definirana vrijednost
    % na izlazu ove funkcije

    %azuriramo vektore a_staro i b_staro
    %tako sto izbacujemo pocetni clan, i na kraj
    %dodajemo novi, vrijednost A ili B
    a=[a_staro(2:end);A];
    b=[b_staro(2:end);B];

    %vrijednost S1 predstavlja ukupan broj
    %promjena min->max vrijednosti amplitude
    S1=0;
    for i=1:(size(a,1)-1)
        if(a(i)<a(i+1))
            S1=S1+1;
        end
    end

    %vektor c sadrzi odgovarajuce vrijednosti
    %za odgovarajucu kombinaciju vrijednosti
    %vektora a i b u trenutku t
    c=zeros(size(a));
    for i=1:size(c,1)
        if(a(i)==5 && b(i)==5)
            c(i)=3;
        elseif(a(i)==5 && b(i)==0)
            c(i)=2;
        elseif(a(i)==0 && b(i)==5)
            c(i)=1;
        elseif(a(i)==0 && b(i)==0)
            c(i)=0;
        end
    end

    %za odredjivanje smjera kretanja efikasnije je
    %posmatrati samo vektor koji sadrzi redoslijed
    %ponavljanja elemenata vektora c samo po jedanput,
    %zato izbacujemo duplicirane elemente
    br1=1; br2=2;
    d=zeros(4,1);
    d(1)=c(1);
    while br1<4
        if(c(br2)~=c(br2-1))
            br1=br1+1;
            d(br1)=c(br2);
        end
        if(br2==1000)
            break;
        else
            br2=br2+1;
        end
    end

    %pozitivan smjer: 3->1->0->2, ali i 2->3->1->0, i 0->2->3->1, i 1->0->2->3
    %negativan smjer: 3->2->0->1, ali i 1->3->2->0, i 0->1->3->2, i 2->0->1->3
    smjer=0;
    if (isequal(d,[3;1;0;2]) || isequal(d,[2;3;1;0]) || isequal(d,[0;2;3;1]) || isequal(d,[1;0;2;3]))
        smjer=1;
    elseif (isequal(d,[3;2;0;1]) || isequal(d,[1;3;2;0]) || isequal(d,[0;1;3;2]) || isequal(d,[2;0;1;3]))
        smjer=-1;
    end

    %racunamo omega1 i omega2 po odgovarajucim formulama
    omega1=smjer*(2*pi*S1)/(P*Td);
    omega2=smjer*(2*pi*fc)/(P*S2);

    %racunanje brojaca
    brojac=brojac_stari;
    if (A>a(end))
        brojac=[0;brojac_stari(1)];
    else
        brojac=[brojac(1)+1;brojac(2)];
    end
end