% resit kolizi s prekazkama pomoci usecek, ne bod/polygon -done
%zkusit fmincon/fminsearch -done(fminsearch)
%analyticky najit kolizi mezi sousednima konfiguracema za letu - done
%zkusit vic a min bodu redukce -done
%body/dylka/cas tabulka + obrazky
%zkusit jinou strategii vyhazovani bodu nez nahodny vyber - done
%zkusit krajni reseni pro 3 body napr. - done
% zkusit slalom / sirokou prekazku - done
% vizualizace napr. V-REP -k team podivat se na to, kdyt to nepujde, pouzit kontakt
% na Daniela Brandtnera  - done
% psat poznamky a postrehy
% pridat senzory ve V-REPu - vytahat grafy - done
% poznamka: prestreleni pri modelovani ve V-Rep, viz. graf vzdalenosti -
% vysvetlit proc - tracking error
% zkusit proximity senzory misto vision - done
% napsat postrehy k ostatnim clankum - done
 
load('PathSample10obst2.mat');
filenameObst ='Simple2obst.txt';
% load('PathSample01obst4.mat');
% filenameObst ='Simple4Obst.txt';
filenameFin ='AoI4.txt';
delimiterIn = ' ';
headerlinesIn = 0;
rawdataObst = importdata(filenameObst,delimiterIn,headerlinesIn);
object_coord = rawdataObst(1:500,2);
rawdataFin = importdata(filenameFin,delimiterIn,headerlinesIn);
fin_coord = rawdataFin(1:500,2);

global res;
global MAVs;
global BF;
global obst;
global rob;
global Pstart;
global Pend;
Pstart=P(:,1);
Pend=P(:,end);
res=10;
MAVs=3;
BF=5;
NumPoints=3;
%shuffle=1:1:BF;



rob.ballradius = 0.5;
rob.minradius = 0.5;
rob.maxradius = 4;

rob.ballradius = rob.ballradius^2;
rob.minradius = rob.minradius^2;
rob.maxradius = rob.maxradius^2;


% Parameters
param.res = 0.5;
param.thresh = 0.2;  %distance to goal point
param.thresh = param.thresh^2;
param.maxiters = 15000;
param.opt = 1;
param.smoothiters=200;

figure(1);
course_outx=[0,-10,-10,10,10,0];
course_outy=[-6,-6,12,12,-6,-6];
% course_outx=[0,-2,-2,6,6,0];
% course_outy=[-9,-9,1,1,-9,-9];

hold on
plot(course_outx,course_outy,'k','LineWidth',3);
drawnow;
axis([-7,7,-10,13]);
axis equal
set(gca,'XTick',-13:1:13)
set(gca,'YTick',-10:1:13)

% axis([-3,7,-10,2]);
% axis equal
% set(gca,'XTick',-3:1:7)
% set(gca,'YTick',-10:1:2)

idx=perms(1:MAVs);
idx=idx(:);
obst.ball = {};
fin.ball = {};
for i=1:8:500
    if (object_coord(i,1)<999)

        onew.p = [object_coord(i,1) object_coord(i+2,1) object_coord(i+4,1) object_coord(i+6,1) object_coord(i,1);
            object_coord(i+1,1) object_coord(i+3,1) object_coord(i+5,1) object_coord(i+7,1) object_coord(i+1,1)];
        onew.handle = [];
        obst.ball{end+1} = onew;
        hold on;        
        draw(onew.p(1,:),onew.p(2,:),'b');
    end
    
    if (fin_coord(i,1)<999)

        fnew.p = [fin_coord(i,1) fin_coord(i+2,1) fin_coord(i+4,1) fin_coord(i+6,1) fin_coord(i,1);
            fin_coord(i+1,1) fin_coord(i+3,1) fin_coord(i+5,1) fin_coord(i+7,1) fin_coord(i+1,1)];
        fnew.handle = [];
        fin.ball{end+1} = fnew;
        hold on;
        draw(fnew.p(1,:),fnew.p(2,:),[1,0.5,0]);
    end
end

for j=1:MAVs
for i=2:length(P)
    plot([P(2*j-1,i);P(2*j-1,i-1)],[P(2*j,i);P(2*j,i-1)],'r','LineWidth',2);
end
end    

[n,m] = size(P);
clearvars n;
l = zeros(m,1);
for k=2:m
    l(k)=norm(P(:,k)-P(:,k-1)) + l(k-1); % find all of the straight-line distances
end
l_init = l(m);
iter = 1;
P1=P;
%--------------------------------------------------------------------------
while iter <= param.smoothiters
    [n,m] = size(P1);
    clearvars n;
    %---------------
    i=randi([1,m]);
    j=randi([i,m]);
    %---------------

    % %---------------
    % k=ceil(linspace(1,m,NumPoints+1));
    % pom=mod(iter,NumPoints)+1;
    % i=k(pom);
    % j=k(pom+1);
    % %---------------
    if (m-(j-i)<NumPoints)
        iter=iter+1;
        continue    
    end


        % random i-th and j-th point on path
        if (j <= i)
            iter = iter + 1;
            continue;
        end

            gamma1=P1(:,i);
            gamma2=P1(:,j);
        col = InCollision_Edge(rob,obst,gamma1,gamma2); %check for obstacle collision
        if col == 1
            iter = iter + 1;
            continue;
        end
        
        col = InCollision_conf(rob,gamma1,gamma2);  %check for crossed paths collision
            if col==1
                change=0;
                pom=reshape(gamma2,2,MAVs);
                per1=pom(:,idx(:));
                per=reshape(per1(:),2*MAVs,factorial(MAVs));
                for k=1:factorial(MAVs)
                    col=InCollision_conf(rob,gamma1,per(:,k));
                    if col==0
                        gamma2=per(:,k);
                        change=1;
                    end
                end
                if change==0
                    iter=iter+1;
                    continue;
                end
            end 
        
        if i~=1
        newP = [P1(:,1:i-1) gamma1 gamma2 P1(:,j+1:m)];
        else
        newP = [gamma1 gamma2 P1(:,j+1:m)];
        end
        clearvars P;
        P1 = newP;
        [n,m] = size(P1);
        clearvars n;
    %     l = zeros(m,1);
    %     for k=2:m
    %         l(k)=norm(P(:,k)-P(:,k-1)) + l(k-1);
    %     end
        iter = iter + 1
        velikost=m
end



%P=[Pstart P(:,3:end)];

for j=1:MAVs
for i=2:size(P1,2)
    plot([P1(2*j-1,i);P1(2*j-1,i-1)],[P1(2*j,i);P1(2*j,i-1)],'g','LineWidth',2);
end
end
drawnow;




tic
[P2,Fval]=fminsearch(@CostFun3,P1)
ElapsedTime=toc;
display(toc);

P2(:,1)=Pstart;
P2(:,end)=Pend;

for i=2:size(P2,2)
for j=1:MAVs
    plot([P2(2*j-1,i);P2(2*j-1,i-1)],[P2(2*j,i);P2(2*j,i-1)],'c','LineWidth',2);
    pause(0.1)
end
end

