function [ p, r ] = intervalCheck( p, r, img)
% This function finds adjacent points whose collision-free circle do not
% intersect and computes a supplement point for these two points
%
% Input:
%                 p - The center of the circle
%                 r - The radius of the circle
%                 img - Environment with obstables
% Output:
%                 p - The center of the circle
%                 r - The radius of the circle with all intersect
%
% 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.

    dx = 1; % step length to find a proper point between
    dy = 1;

    for i = 1 : 1 :length(r)
        flag = 0; % indicator of obstables
        if ( norm(( p(i+1,:) - p(i,:) ), 2) >= r(i+1) + r(i) )
            slope = ( ( p(i+1,2)-p(i,2) ) ./ ( p(i+1,1)-p(i,1) ) );
            slope_p = -slope^-1;

            if (slope > 0 && slope < inf) % slope is positive
                P0 = [p(i+1,1), p(i,2)]; % supplement point between i th and i+1 th
                while ( img(P0(2), P0(1)) == 0 )
                    flag = flag + 1;
                    if (flag == 2 ) % wrong direction to move points
                        flag = 0;
                        slope_p = -slope_p; % turn around
                    end
                    P0 = P0 + slope_p * [dy, -dx];
                end
                
                [r0, P_trouble, P0] = findRadius(P0, img);
                p = [p(1:i,:); P0; p(i+1:end,:)];
                r = [r(1:i), r0, r(i+1:end)];

            elseif (slope > -inf && slope < 0) % slope is negative
                P0 = [p(i+1,1), p(i,2)];
                while ( img(P0(2), P0(1)) == 0 )
                    flag = flag + 1;
                    if (flag == 2 )
                        flag = 0;
                        slope_p = -slope_p;
                    end
                    P0 = P0 + slope_p * [dy, dx];
                end
                [r0, P_trouble, P0] = findRaduis(P0, img);
                p = [p(1:i,:); P0; p(i+1:end,:)];
                r = [r(1:i), r0, r(i+1:end)];
            end
            

        end
    end
end

