function [ p_sorted, r_sorted] = sortEffectivePoint( p, r )
% This function receive points and their corresponding collision-free
% radii and return some of them to make more efficient path planning 
% Input:
%             p  - center of the circle
%             r  - radius of the circle
% Output:
%          p_sorted - sorted points of the circle
%          r_sorted - sorted radius of the circle
%
% Copyright (c) 2017, Rujie Wang
% All rights reserved.
%
% IN NO CIRCUMSTANCES SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
% RESPONSIBLE FOR ANY PROBLEM, DAMAGE OR LOSS CAUSED.
%
% Redistribution of this code, with or without modification, are permitted 
% provided that the above copyright and disclaimer are retained.

    j = 0.7; % coefficent of the radius of the ith circle    
    legnth_r1 = length(r); % the number of points to handle
    
    
    p_sorted = [p(1,:)];
    r_sorted = [r(1)];
    
    for i = 1 : 1 : length(r)
        A=find(r_sorted>0);
        r_pos = r_sorted(A(end));
        B=find(p_sorted(:,1)>0);
        p_pos = [p_sorted(B(end),1), p_sorted(B(end),2)];
        
        if ( norm((p(i,:) - p_pos),2) <= j* max(r_pos, r(i)) ) % if distance between adjacent two points
            p_sorted = [p_sorted ; [0 0]]; % is less than j times the bigger radius of two points
            r_sorted = [r_sorted  0]; % The ith point is left out
        else 
            p_sorted = [p_sorted; [p(i,1), p(i,2)]];
            r_sorted = [r_sorted r(i)];   
        end

    end
%% pull out all nonzero elements
    A=find(r_sorted>0);
    r_sorted = r_sorted(A);
    B=[find(p_sorted(:,1)>0)];
    p_sorted = p_sorted(B,:);
    
    length_r2 = length(r_sorted); % the number of points left
    
    fprintf('--- %d%% points are sorted out!\n',floor((length_r2/legnth_r1)*100));
    
end

