clc;
clear all;
format long e
%--------------------------PRADINIAI DUOMENYS------------------------------

c=3;                            %kontaktu skaicius
% e=5*10^-2;                    %nuokrypis nuo funkcijos
alfa=30;   %laipsniai           %pirmojo kontakto kampas su x asimi
% d_s=0.6;     %m            %zingsnis
r=1;                            %ciklines trajektorijos kartojimu kiekis
R=500*10^(0);   %m           %roboto spindulys - atstumas nuo jo centro iki kontakto
% e=3;

j=0;                            %kontakto nr.; 0 - nera aktyvus nei vienas
Atsarginis_kontaktas=0;         %gali ir nereikti
l=1;                            %nagrinejamos trajektorijos lygties numeris
i=1;                            %surasto tasko numeris

%testavimo judejimo taskai
%K=[-2 -1; -1 -2; 1 -2; 2 -1; 2 1; 1 2; -1 2; -2 1; -2 -1];
%K=[1 -2; 0 3];
%K=[ -1 -2; -0.5 -2.4; 1 -2; 2 -1; 1 0.5];
%K=[1 1; 2 2; 3 1; 1 1];
%K=[1 1; 3 1; 2 2; 1 1];
%K=[1 1; 3 1; 2 0; 1 1];
%K=[1 1; 2 0; 3 1; 1 1];
%K=[1 1; 2 2; 2 1; 1 3; 1 1];
%K=[1 1; 1 2; 2 2; 2 1; 1 1];
%K=[1 1; 2 1; 2 2; 1 2; 1 1];
%K=[0 0; 1 3; 2 2; 1 1];
%K=[5 2; 3 -1; 1 3; 0 0];
%K=[2 2; 3 4; 3 5; 1 2];
%K=[1 1; 2 2; 4 5];
%K=[4 5; 2 2; 1 1];
%K=[3 2; 3 4; 1 1];
%K=[1 2; 3 4; 3 2];
%K=[0 0; 0.5 1; 1 1; 0 0];
%K=[-1 0; 0 1; 1 0; 0 -1; -1 0];
%K=[0 0; 0.00001 -40; 0 0];
%K=[1 1; 2 2; 3 1; 4 2]; %kalnai
% K=[-0.5 -0.5; 0.8 0; -0.5 0.5; 0.8 1];
%K=[0.2 -0.5; 0 -0.3; 0 0; 0.3 0.1; 0.7 -0.6; -0.2 -0.7; -0.2 0.2; 0.2 0.4];
%K=[0 0; 2 1; 0 2; 2 3];
%K=[0 0; 0.5 -8; 1 0]; %tikrinimui nuo spindulio
K=[0 2; 0.9 1; 0 0; 1 -0.5; 2 0; 1.1 1; 2 2]; %vaza
K=[-2.2 0; -1.5 1; -0.6 0.4; -0.4 0.6; -1 1.5; 0 2.2; 1 1.5; 0.4 0.6; 0.6 0.4; 1.5 1; 2.2 0; 1.5 -1; 0.6 -0.4; 0.4 -0.6; 1 -1.5; 0 -2.2; -1 -1.5; -0.4 -0.6; -0.6 -0.4; -1.5 -1; -2.2 0];
%K=[0 0; 0.5 0; 0.5 0.5; 0 0.5; 0 0]; %apskritimas
% K=[0 -0.5; 0.5 0; 0 0.5; -0.5 0; 0 -0.5];
%K=[-1.5 2.25; -1 1; 0 0; 1 1; 1.5 2.25];%x^2
%K=[1 1; 1 2; 2 1; 2 2; 1 1]; %astunke
%K=[0 0; 0.5 0.5; 0 1; -1 0; 0 -0.9; 1.5 0; 1 1.2; 0 1.7]; %spirale,
%REZULTATAS: aktyvaus kontakto numeris, einamas atstumas ir roboto koordinates
% kelias(1,:)=[0 0 K(i,:)];
taskai=[K(i,:) 0 1];
%-------------------------------SKAICIAVIMAI------------------------------

%tasku kiekis
N=length(K);
%nustatomos judejimo bendroji trajektorijos kryptis tarp tasku
[K_skirtumas TR_kr]=Skirtumai(K,1);

%Randamos tiesiu lygtys tarp duotu tasku
[Sp, T, Atstumas]=Splainai(K, ciklinis);      %skaiciuojami splainai, parametru reiksmiu intervalai tarp tasku

%nustatomas max zingsnis
p_min=0.01;  %1nm        %minimalus poslinkis
% e=min_e(Sp, T, p_min);
e=0.1;
d_s=max_zingsnis2(Sp, T, e);
% d_s=0.1
% d_s=p_min;

%minimalus roboto posukis laipsniais
min_posukis=(p_min/R)*180/pi; 

i=1;    %surasto tasko numeeris
j=0;    %aktyvaus kontakto numeris

kartoti=1;  %kartojimu kiekis
while kartoti<=r
    l=1;       %trajektorijos lygties numeris
    param0=0;
    if i ==200
        break;
    end
    while l<N
        atstumas_iki=atstumas(taskai(i,1),K(l+1,1),taskai(i,2),K(l+1,2));
        if atstumas_iki > d_s
             % kreives lygtis
             lygtis(taskai(i, :), d_s);
             
             %surandami splaino ir kreives susikirtimo taskai
             lygtys=Kreive;
             lygtys(2,:)=Sp(l,:);
             param=[param0 T(l+1)];
             [Taskas param0]=Susikirtimo_taskai_param(lygtys,param);
             if isempty(Taskas)==0
                 taskai(i+1,:)=[Taskas(1,:) param0 kartoti];
                 i=i+1;
                 i
                 if i==200
                     break;
                 end
             else
                 l=l+1;
                 kartoti=kartoti+1;
             end
        else
            if l+1~=N 
                l=l+1;
            else
                if atstumas_iki >= p_min
                    taskai(i+1,:)=[K(N,:) T(N) kartoti];
                    kartoti=kartoti+1;
                    break;
                else
                    kartoti=kartoti+1;
                    break;
                end
            end
        end
    end  
end

%trajektorijos formavimas ivertinant tiksluma
M=size(taskai,1);                          %rastu tasku kiekis

%aspskaiciuojami kampai tarp tiesiu
m=2;
i=1;
koordinates(1,:)=taskai(1,1:2);
while i<=M 
    if i<M-1
        koord1=[koordinates(m-1,:); 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,:); 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:2);
            i=i+1;
        else
            if i==M-1
                koordinates(m,:)=taskai(i+1,1:2);
            else
                koordinates(m,:)=taskai(i+2,1:2);
            end
            i=i+2;
        end
        m=m+1;
    else
        koordinates(m,:)=taskai(M,1:2);
        i=i+1;
    end
end

%Roboto valdymo skaiciavimas
tasku_sk=size(koordinates,1);

%judesio trajektorijos tiesiu kampai su x asimi
kryptys=zeros(tasku_sk-1,1);
for i=1:tasku_sk-1
    kryptys(i)=orientacija(koordinates(i:i+1,:));
end

l=1;
ll=1;
for i=2:tasku_sk
    %Roboto kontaktu kampai su x asimi
    gama=zeros(1, c);
    for j=1:c
        gama(j)=alfa + 360/c*(j-1);
        gama(j)=mod(gama(j),360);
    end
    
    %Apskaiciuojami posukio kampai
    posukiai=zeros(c, 2);
    for j=1:c
        posukiai(j, :)=[mod(360+gama(j)-kryptys(i-1), 360) mod(360-gama(j)+kryptys(i-1), 360)];
    end
    
    %randamas maziausias posukio kampas
    [K_min, ind]=min(posukiai);
    [minimumas, kryptis]=min(K_min);
    kontaktas=ind(kryptis);
    
    %apskaiciojamas nueinamas arba posukio kelias
    posukis=R*pi*minimumas/180;
    s=atstumas(koordinates(i-1,1), koordinates(i,1), koordinates(i-1,2), koordinates(i,2));
%     if s>=d_s_min
%         if minimumas~=0
            kelias(l, :)=[0  minimumas posukis kryptis koordinates(i-1,1:2)];
%             l=l+1;
%         end
        kelias(l+1, :)=[kontaktas  0 s 0 koordinates(i, 1:2)];
        %perskaiciuojami pradiniai roboto kampai
        if kryptis==1
            alfa=alfa - minimumas;
        else
            alfa=alfa + minimumas;
        end

    nuokr(ll,:)=[gama(kontaktas) kelias(l+1, 5:6)];
    S(ll,1)=kelias(l+1,3);
    ll=ll+1;
    l=l+2;
end;

%Kreivumo spindulio apskaiciavimas
% R=kreivumo_spindulys(nuokr(:,2:3), Sp, T);
%----------------------
% x1=Sp(1,1);
% x2=Sp(1,2);
% atsilenkimas=testas(Sp, T, nuokr(:,2:3));
% max_atsilenkimas=max(atsilenkimas);
% min_atslinekimas=min(atsilenkimas);
% vid_astsilenkimas=mean(atsilenkimas);
% std_atsilenkimas=std(atsilenkimas);

