% Import Obstacle Locations
% RRT je matice a ne spojovej seznam
% clear all;
filenameObst ='Simple2obst.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;
res=20;
MAVs=3;
BF=5;
shuffle=1:BF;
checkem=NaN(1,MAVs);
% Draw Course
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];

fieldwidth=abs(min(course_outx))+abs(max(course_outx));
fieldleft=abs(min(course_outx));
fieldlength=abs(min(course_outy))+abs(max(course_outy));
fieldbot=abs(min(course_outy));

hold on
%plot(course_outx,course_outy,'k','LineWidth',3);
drawnow;
% axis([-3,7,-10,2]);
% axis equal
% set(gca,'XTick',-3:1:7)
% set(gca,'YTick',-10:1:2)
axis([-11,11,-7,13]);
axis equal
set(gca,'XTick',-11:1:11)
set(gca,'YTick',-7:1:13)
% grid ON
TREE=[];
obst.ball = {};
fin.ball = {};

%reading space and goal properties
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
%------------------------------------------
numfin = length(fin.ball);
X=[];
Y=[];
for j=1:numfin
        X=[X fin.ball{j}.p(1,:)];   %#ok
        Y=[Y fin.ball{j}.p(2,:)];   %#ok
end
minX=min(X);
minY=min(Y);
X=X-minX;
Y=Y-minY;
Xdim=round(res*(max(X)-min(X)));
Ydim=round(res*(max(Y)-min(Y)));
fin.mask=poly2mask(res*X,res*Y,Ydim,Xdim);
fin.minX=minX;
fin.minY=minY;
fin.Xdim=Xdim;
fin.Ydim=Ydim;
%------------------------------------------

% Set Start and Goal locations
start_posts=[1 2 3 4 5;11 11 11 11 11];
% start_posts=[0 2 4;0 0 0];
p_start = start_posts(1:2*MAVs)';
%p_goal = [-3.5;-2.5];
p_goal = center(fin);

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

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

rob.p = start_posts(:,1:MAVs);

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

circle(rob.p(1,:),rob.p(2,:),rob.ballradius,'g');
%%plot(rob.p(1,:),rob.p(2,:),'+');
%circle(p_goal(1,:),p_goal(2,:),rob.ballradius,'r');
%%plot(p_goal(1),p_goal(2),'+');
% Plan the path
P = [];
P2= [];
rrt = [];
rrt = AddNode2(rrt,p_start,0);
iter = 1;
offset=linspace(-pi/4,pi/4,BF-1); %turning ability of MAV
%nogo=zeros(BF,MAVs);
%nogo(:)=-1;
dist=zeros(MAVs+1,1);
mindist=zeros(MAVs,1);
mindist(:)=9999;
MAVcolor={'m' 'c' 'g' 'y' 'k'};
Z = zeros(BF,MAVs);
problemMa=zeros(MAVs,1);
newp=zeros(2*MAVs,1);
gut=zeros(MAVs,1);
cur=zeros(2,MAVs);
phase1=NaN;
while iter <= param.maxiters

    %%random point---------------------------
        p = rand(2,1);
    p(1) = p(1)*fieldwidth-fieldleft;
    p(2) = p(2)*fieldlength-fieldbot;
    
    chanceOfGoal=rand;    %with 5% probability random point is goal location
    if (chanceOfGoal<=0.05)
        p=p_goal;
    end
    
    rob.p = p;
    if (iter==param.maxiters)
        p=p_goal;
        rob.p=p_goal; 
    end
    col = InCollision_Node(rob,obst);
    if col == 1 % skip to next iteration
        continue
    end
    
%     if (norm2(p-p_goal)<param.thresh)
%         iter = iter + 1;
%         lolFAIL=1;
%     end
    
    
    
    %%plot(p(1),p(2),'+');
    %%--------------------
    
    %%looking for closest node---------------
    for i=1:size(rrt,2)
        dist(1) = norm2(rrt(3:4,i) - p); %distance from 1st MAV in each node
        if (i==1) || (dist(1) < mindist(1))
            mindist(1) = dist(1);
            imin = i; %rrt{imin}.p = nearest node
            l = rrt(3:end,i); %l= nearest node
            
        end
    end
    %%------------------
    %case closest node is root configuration
    if (imin==1)
        offset2=linspace(-pi,5*pi/6,12);
        for i=1:12
            newp(1:2:end)=p_start(1:2:end) + 0.1*cos(offset2(i));
            newp(2:2:end)=p_start(2:2:end) + 0.1*sin(offset2(i));
            dist(1) = norm2(newp([1;2]) - p);
            if (i==1) || (dist(1) < mindist(1))
                mindist(1) = dist(1);
                nextp=newp;
            end
        end
        rrt = AddNode2(rrt,nextp(:),1);
        for i=1:MAVs
        %plot([nextp(2*i-1);p_start(2*i-1)],[nextp(2*i);p_start(2*i)],'m','LineWidth',1);
        end
        imin=size(rrt,2);
        %iter=iter+1;
        %continue
    end
    %%-----------------
    
    %%case closest node is any other node---
    ipr = rrt(2,imin);
    pass=0;
    if(ipr~=0)
        prev=reshape(rrt(3:end,ipr),2,MAVs);    %previous node
        cur=reshape(l,2,MAVs);          %current node

        
        angle=atan2(cur(2,:)-prev(2,:),cur(1,:)-prev(1,:));
        mindist(:)=9999;
        gut(:)=0;
        newp=cur;
        nextp=newp;
        for i=1:MAVs
            dist(i)=norm2(newp(:,i)-p);
            if dist(i)<dist(1)
                pomMAV=newp(:,1);
                newp(:,1)=newp(:,i);
                newp(:,i)=pomMAV;
            end                
        end
        %%--Leader MAV chooses direction--------
        for i=1:BF
                if i==BF
                newp(:,1)=cur(:,1)+[0.1*cos(angle(1)) ; 0.1*sin(angle(1))];    
                %elseif i==BF-1
                %newp(:,1)=cur(:,1);
                else
            newp(:,1)=cur(:,1)+[0.2*cos(angle(1)+offset(i));0.2*sin(angle(1)+offset(i))];
                end
            dist(1) = norm2(newp(:,1) - p);
            rob.p = newp;
            col = InCollision_Node(rob,obst);
            if (col==0) && (dist(1) < mindist(1)) %&& (~ismember(i,nogo(:,1)))  %problemMa(1)*nogo(:,1)
                mindist(1) = dist(1);
                nextp(:,1)=newp(:,1);
                gut(1)=1;
            end
        end
        %%--------
        
        %%--Following MAVs choose direction-----
        for j=2:MAVs
            for i=1:BF
                if i==BF
                newp(:,j)=cur(:,j)+[0.1*cos(angle(j)) ; 0.1*sin(angle(j))];
                %elseif i==BF-1
                %newp(:,j)=cur(:,j);
                else
            newp(:,j)=cur(:,j)+[0.2*cos(angle(j)+offset(i)) ; 0.2*sin(angle(j)+offset(i))];
                end
                

                for k=j-1:-1:1
                    checkem(k) = norm2(newp(:,j) - newp(:,k));
                end
                check=min(checkem);
            
            %check = norm2(newp(:,j) - newp(:,j-1));
            dist(j)=norm2(newp(:,j)-p);
            rob.p = newp;
            col = InCollision_Node(rob,obst);
            if (col==0) && (check < rob.maxradius) && (check > rob.minradius) && (dist(j) <= mindist(j)) %&& (~ismember(i,nogo(:,j))) %problemMa(j)*nogo(:,j)
                mindist(j) = dist(j);
                nextp(:,j)=newp(:,j);
                gut(j)=1;
            end
            
            end
            checkem(:)=NaN;
        end
        %%-----------
        if (nextp(1,1)>max(course_outx)) || (nextp(1,1)<min(course_outx))|| (nextp(2,1)>max(course_outy)) || (nextp(2,1)<min(course_outy))
            gut(:)=0;
        end
        
         %problemMa=~gut
        if all( gut==ones(MAVs,1))          
            
            for j=1:MAVs
                dist(j)=norm2(cur(:,j) - p_goal);
            end
            
            if (min(dist(1:MAVs)) < 0.5 || iter==param.maxiters) %if close enough to goal, endeschluss
                rrt = AddNode2(rrt,nextp(:),imin);
                %GREATSUCCESS=1
                iterations = iter;
                %rrt = AddNode(rrt,p_goal,length(rrt),chosen);
                i = size(rrt,2);

                P(:,1) = rrt(3:end,i);

                
                phase1=rrt(2,i);
                while i>0
                    i = rrt(2,i);
                    if i == 0
                        iterations = iter - 1;
                        continue
                    end
                    

                    pom(:,1) = rrt(3:end,i);

                    P = [pom P]
                end
                iter=param.maxiters;
            end
            
            rrt = AddNode2(rrt,nextp(:),imin);
            %plot([nextp(1);cur(1)],[nextp(2);cur(2)],'m','LineWidth',1);
            for j=1:MAVs
                %plot([nextp(1,j);cur(1,j)],[nextp(2,j);cur(2,j)],MAVcolor{j},'LineWidth',1);                                
            end
            kreslim=imin;
            ipr=size(rrt,2);
            %prev=cur;
            %cur=nextp;
            imin=size(rrt,2);
            %nogo(:)=-1;
        end        
        
        %%------------------
    end
iter = iter + 1;
if mod(iter,100)==0
    drawnow;
end
end

if isnan(phase1)
    phase1=imin;
end

% %plot the unsmoothed path
for j=1:MAVs
for i=2:size(P,2)
    %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


iter2=0;
globalbest.cover=0;
globalbest.i=NaN;
while iter2 <= 30
     
    imin=size(rrt,2);
    iprev=rrt(2,imin);
    cur=reshape(rrt(3:end,imin),2,MAVs);
    prev=reshape(rrt(3:end,iprev),2,MAVs);
    shallbeadded=0;
    gut(:)=0;
    mincover=0;
    angle=atan2(cur(2,:)-prev(2,:),cur(1,:)-prev(1,:));        
        %%--All MAVs choose direction-----
        for j=1:MAVs
            shuffle=shuffle(randperm(length(shuffle))); %random picking new direction
            for i=shuffle  %for i=1:BF i=BF:-1:1
                if i==BF
                newp(:,j)=cur(:,j)+[0.05*cos(angle(j)) ; 0.05*sin(angle(j))];
                else
            newp(:,j)=cur(:,j)+[0.1*cos(angle(j)+offset(i)) ; 0.1*sin(angle(j)+offset(i))];
                end
                
%             if j>1
%             check = norm2(newp(:,j) - newp(:,j-1));
%             else
%                 check=norm2(newp(:,j) - newp(:,j+1));
%             end            

            if j>1
                for k=j-1:-1:1
            checkem(k) = norm2(newp(:,j) - newp(:,k));
                end
            else
                checkem(1)=norm2(newp(:,j) - newp(:,j+1));
            end  
            check=min(checkem);
            rob.p = newp;
            cover=coverage(rob,fin);
            
%                 if (check < rob.maxradius) && (check > rob.minradius) && (i==BF) %(i==shuffle(1)) %(i==1)
%                     nextp(:,j)=newp(:,j);
%                 end

            %col = InCollision_Node(rob,obst);
            col = InCollision_Node(rob,obst);
            if (col==0) && (check < rob.maxradius) && (check > rob.minradius) && (cover >= mincover)
                mincover = cover;
                nextp(:,j)=newp(:,j);
                gut(j)=1;
                if(cover>globalbest.cover)
                    globalbest.cover=cover;
                    globalbest.i=imin+1;
                    %globalbest.p=rob.p;
                    shallbeadded=1;
                end
            end
            end
            checkem(:)=NaN;
            mincover=0;
        end
    
        if all( gut==ones(MAVs,1))
            if (shallbeadded==1)
                globalbest.p=nextp;
            end
            rrt = AddNode2(rrt,nextp(:),imin);
            figure(1);
            for j=1:MAVs
                %plot([nextp(1,j);cur(1,j)],[nextp(2,j);cur(2,j)],MAVcolor{j},'LineWidth',1);                                
            end
        else
            nextp(:,:)=newp(:,:);
            rrt = AddNode2(rrt,nextp(:),imin);
            figure(1);
            for j=1:MAVs
                %plot([nextp(1,j);cur(1,j)],[nextp(2,j);cur(2,j)],MAVcolor{j},'LineWidth',1);                                
            end

        end
        shallbeadded=0;
        iter2=iter2+1
end

if(~isnan(globalbest.i))
    i=globalbest.i;
    rob.p=globalbest.p;
    cover=coverage(rob,fin);
end

while i>phase1
    
    for j=1:MAVs
    pom = rrt(3:end,i);
    end
	P2 = [pom P2];
    i = rrt(2,i);
end
P=[P P2];

figure(1);
for k=1:MAVs
for l=size(P,2)-size(P2,2):size(P,2)
    %plot([P(2*k-1,l);P(2*k-1,l-1)],[P(2*k,l);P(2*k,l-1)],'r','LineWidth',2);
end
end
%plot(rob.p(1,:),rob.p(2,:),'x','LineWidth',2);