clc
clear koordinates M
R=400;
min_posukis=(p_min/R)*180/pi; 
%trajektorijos formavimas ivertinant tiksluma
M=size(taskai,1);                          %rastu tasku kiekis

%aspskaiciuojami kampai tarp tiesiu
m=2;
i=1;
koordinates(1,:)=taskai(1,1:3);
while i<=M 
    if i<M-1
        koord1=[koordinates(m-1,1:2); taskai(i+1,1:2)];
        kampas1=orientacija(koord1);
        kampas2=orientacija(taskai(i+1:i+2,1:2));
        skirtumas(m)=abs(kampas1-kampas2);
        %kol skirtumas mazas, ieskomos kitos koordinates
        while skirtumas(m)<min_posukis && i<=M-2
            i=i+1;
            if i==M-1
                break
            end
            koord1=[koordinates(m-1,1:2); taskai(i+1,1:2)];
            kampas1=orientacija(koord1);
            kampas2=orientacija(taskai(i+1:i+2,1:2));
            skirtumas(m)=abs(kampas1-kampas2);
        end
        
        if skirtumas(m)>=min_posukis
            koordinates(m,:)=taskai(i+1,1:3);
            i=i+1;
        else
            if i==M-1
                koordinates(m,:)=taskai(i+1,1:3);
            else
                koordinates(m,:)=taskai(i+2,1:3);
            end
            i=i+2;
        end
        m=m+1;
    else
        koordinates(m,:)=taskai(M,1:3);
        i=i+1;
    end
end