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

c=3;                            %kontaktu skaicius
r=1;                            %ciklines trajektorijos kartojimu kiekis
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


K=[0 0; 0.5 1; 0.7 0.6; 1.1 0.9; 1.5 0]; %kalnai
%REZULTATAS: aktyvaus kontakto numeris, einamas atstumas ir roboto koordinates
kelias(1,:)=[0 0 K(i,:)];
f_taskai(1,:)=[0 1];
%-------------------------------SKAICIAVIMAI------------------------------

%tasku kiekis
N=int8(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

%ilgis
visas_ilgis=ilgis(T, Sp,'');

%apskaiciuojamas roboto orientacijos kampas pagal splainu liestines krypti
liestiniu_kryptys=kreives_liestiniu_kryptys(Sp, T, TR_kr);
[kreivis_max, ind]=max(liestiniu_kryptys);
liestines_kryptis=liestiniu_kryptys(ind(2));

%Apskaiciuajmas judejimo tieses kampas su x asimi
tieses_kampas=Tieses_krypties_koeficientas(K, TR_kr);

%optimalaus kampo paieska
%alfa=optimalus_kampas(K_skirtumas, tieses_kampas);
 alfa=50;
%Roboto kontaktu kampai su x asimi bei kontaktu kryptys x ir y atzvilgiu
[Gama, R_kr]=Roboto_pozicija(c, alfa);

% e=epsilon_min(tieses_kampas, Gama, p_min);
p_min=1*10^-3;
% e=epsilon_min(liestiniu_kryptys, Gama, p_min);
e=0.05;
%  e=epsilon_min(liestiniu_kryptys(:,1:2), Gama, p_min, liestines_kryptis);

%Apskaiciuojamos ribines koordinates
[RK, Rib_kr]=Ribines_koordinates(K, Sp, TR_kr, e, T);

%ribiniu koordinaciu luziu tieses lygtys
for i=1:N
    [Luziu_lygtys(i,1), Luziu_lygtys(i,2)]=Tieses_lygtis(RK(i,1:2), RK(i,3:4));
end

%uzrasomos ribiniu koordinaciu lygtys ir ju parametro reiksmes
[Rib_lygtys1, Rib_T1]=Splainai(RK(:,1:2), ciklinis);   %apatines
[Rib_lygtys2, Rib_T2]=Splainai(RK(:,3:4), ciklinis);   %virsutines
% Rib_Lygtys=[Rib_lygtys1 Rib_lygtys1];

i=1;    %surasto tasko numeeris
j=0;    %aktyvaus kontakto numeris
kartoti=1;  %kartojimu kiekis
kontakto_keitimas=1;
sustoti=0;
nueitas_ilgis=0;
while kartoti<=r
    l=1;       %trajektorijos lygties numeris
    while l<N
        %-----
        t=f_taskai(i);
        xx(1,1)=eval(Sp(l,1));
        xx(1,2)=eval(Sp(l,2));
        t=f_taskai(i)+e/10;
        xx(2,1)=eval(Sp(l,1));
        xx(2,2)=eval(Sp(l,2));
        skirt=sign(Skirtumai(xx,1));
        clear xx
       
         %Apskaiciuojami kampai tarp judejimo tieses ir kontaktu asiu
            P=zeros(c, 2);
            for h=1:c
                P(h, :)=[mod(360+Gama(h)-tieses_kampas(l), 360) mod(360-Gama(h)+tieses_kampas(l), 360)];
            end
            
            %randamai maziausi du kampai ir kontaktu numeriai sudarantys judejimo tiese
            [Kampai_min, kontaktai]=min(P);
            if kontaktai(1)==kontaktai(2) && kontaktai(1)==j
                P(kontaktai,:)=P(kontaktai,:)+360;
                [Kampai_min, kontaktai]=min(P);
            end
%             clear  Aktyvus_kontaktas Atsarginis_kontaktas
%             for ii=1:c
%                 if R_kr(ii,1)==skirt(1) && R_kr(ii,2)==skirt(2) && kelias(i,1)~=ii
%                     Aktyvus_kontaktas=ii;
%                 end
%             end
%             if exist('Aktyvus_kontaktas', 'var')==0
                
                %surandami kontaktai kurie gali buti aktyvus, isskyrus j kontakta
                [Aktyvus_kontaktas Atsarginis_kontaktas]=Kontaktu_parinkimas(Kampai_min, kontaktai, i, j);
           if R_kr(Atsarginis_kontaktas,1)==skirt(1) &&  R_kr(Atsarginis_kontaktas,2)==skirt(2)
               laikinas=Aktyvus_kontaktas;
               Aktyvus_kontaktas=Atsarginis_kontaktas;
               Atsarginis_kontaktas=laikinas;
               clear laikinas
           end
% else
%                 for ii=1:2
%                     if exist('Aktyvus_kontaktas') && kelias(i,1)~=kontaktai(ii) && Aktyvus_kontaktas~=kontaktai(ii)
%                         Atsarginis_kontaktas=kontaktai(ii);
%                     end
%                 end
%             end
        
        %-----
        %Apskaiciuojami kampai tarp judejimo tieses ir kontaktu asiu
%         P=zeros(c, 2);
%         for h=1:c
%             P(h, :)=[mod(360+Gama(h)-tieses_kampas(l), 360) mod(360-Gama(h)+tieses_kampas(l), 360)];
%         end
%         
%         %randamai maziausi du kampai ir kontaktu numeriai sudarantys judejimo tiese
%         [Kampai_min, kontaktai]=min(P);
%         if kontaktai(1)==kontaktai(2) && kontaktai(1)==j
%             P(kontaktai,:)=P(kontaktai,:)+360;
%             [Kampai_min, kontaktai]=min(P);
%         end
        %surandami kontaktai kurie gali buti aktyvus, isskyrus j kontakta
%         [Aktyvus_kontaktas Atsarginis_kontaktas]=Kontaktu_parinkimas(Kampai_min, kontaktai, i, j);
        
        %bendroji tieses lygtis bet kuriam kontaktui
        Jud_lygtis=Kontakto_tieses_lygtis(Gama(Aktyvus_kontaktas),kelias(i,3:4));
        if Atsarginis_kontaktas~=0
            Jud_lygtis_atsargine=Kontakto_tieses_lygtis(Gama(Atsarginis_kontaktas),kelias(i,3:4));
        end
        
        %nuskaitomas nagrinejamas trajektorijos splainas
        Tx=Sp(l,1);
        Ty=Sp(l,2);
        
        kontaktas=Aktyvus_kontaktas;
        lygtis=Jud_lygtis;
        
        rastas_taskas=0;
        rastas=0;
        while rastas_taskas==0 
            %nuskaitomas nagrinejamas trajektorijos splainas
            Tx=Sp(l,1);
            Ty=Sp(l,2);
            %ieskomas susikirtimo taskas su ribinemis koordinatemis g* ir g**
            %             [Judejimo_taskas, Ak, Fk]=Ejimas(kontaktas,Tx, Ty, Rib_T1(l:l+1), Rib_T2(l:l+1), Gama(kontaktas), lygtis, e, kelias(i, 3:4), R_kr(kontaktas,:));
            [Judejimo_taskas, Ak, Fk]=Ejimas(kontaktas,Tx, Ty, T(l:l+1), Gama(kontaktas), lygtis, e, kelias(i, 3:4), R_kr(kontaktas,:));
            
            if isempty(Judejimo_taskas)==0
                break
            else
                lygtys(1,1)=Luziu_lygtys(l+1,1);
                lygtys(1,2)=Luziu_lygtys(l+1,2);
                lygtys(2,:)=Jud_lygtis;
                [kerta kirtimo_koord]=Ar_kerta_luzi(lygtys, RK(l+1,:), R_kr(kontaktas,:), kelias(i,3:4));
                if kerta==1
                    if l==N-1 && kartoti==r
                        Judejimo_taskas=kirtimo_koord;
                        Ak=Aktyvus_kontaktas;
                        l=l+1;
                        break;
                    end
                    l=l+1;
                else
                    while rastas==0 && l<N
                        %nuskaitomas nagrinejamas trajektorijos splainas
                        Tx=Sp(l,1);
                        Ty=Sp(l,2);
                        kontaktas=Atsarginis_kontaktas;
                        lygtis=Jud_lygtis_atsargine;
                        [Judejimo_taskas, Ak, Fk]=Ejimas(kontaktas,Tx, Ty, T(l:l+1), Gama(kontaktas), lygtis, e, kelias(i, 3:4), R_kr(kontaktas,:));
                        if isempty(Judejimo_taskas)==0
                            rastas_taskas=1;
                            break
                        else
                            lygtys(1,1)=Luziu_lygtys(l+1,1);
                            lygtys(1,2)=Luziu_lygtys(l+1,2);
                            lygtys(2,:)=lygtis;
                            kerta=Ar_kerta_luzi(lygtys, RK(l+1,:), R_kr(kontaktas,:), kelias(i,3:4));
                            if kerta==1
                                if l==N-1 && kartoti==r
                                    Judejimo_taskas=kirtimo_koord;
                                    Ak=Atsarginis_kontaktas;
                                    l=l+1;
                                    break;
                                end
                                l=l+1;
                            else
                                l=l-1;
                                if l==0
                                    break;
                                end
                            end
                        end
%                         if l==0
%                             break;
%                         end
                    end
                end
            end
%             if l==0
%                 break;
%             end
        end
%         if l==0
%             break;
%         end
        if j~=Aktyvus_kontaktas 
            i=i+1
            j=Ak;
            ats=atstumas(kelias(i-1,3),Judejimo_taskas(1,1), kelias(i-1,4),Judejimo_taskas(1,2));
            if ats>=p_min
            kelias(i,:)=[j ats Judejimo_taskas];
            if isempty(Fk)==0
               
                f_taskai(i,:)=[Fk l];
                Spindulys(i-1,:)=kreivumo_spindulys(Fk, Tx, Ty);
                
                %ilgis
                difx=diff(Tx);
                dify=diff(Ty);
                fun=(difx^2+dify^2)^(1/2);
                ilg=double(int(fun,f_taskai(i-1,1), f_taskai(i,1)));
                nueitas_ilgis=nueitas_ilgis + sign(f_taskai(i,1)-f_taskai(i-1,1))*ilg;
            end
            else
                sustoti=1;
                break;
            end
%             
%            
%             liestine(i-1,1)=Kreives_krypties_koeficientas(Fk, Tx, Ty);
        end
        clear Judejimo_taskas Ak Fk
        if l==N
           kartoti=kartoti+1;
        end
        if sustoti==1 ||i==60
            break;
        end
    end
    if sustoti==1 || i==60
        break;
    end
end
proc_kelio=(nueitas_ilgis/visas_ilgis)*100;



