function [r_c, P_trouble, P_c] = findRadius(P_c, img)
% This function finds the biggest radius of a circle centered at given point without
% colliding the obstable. If there is any points on the obstables, return
% them. Also, return points that are translated away from obstables.
% 
% Input:
%                 P_c - The center of the circle
%                 img - Environment with obstables
% Output:
%                 r_c - The biggest radius of the circle (avoid any obstables)
%           P_trouble - Points that lie on the obstables
%                 P_c - Translated or untranslated point
%
% 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.

P_trouble = []; 
    if ( img(P_c(2),P_c(1)) == 0 )
        r_c = NaN;
        disp('--- This point lies on the obstable!');
        P_trouble = [P_trouble [P_c(1), P_c(2)]] % show points that lie on the obstable
        return
    end

        s = size (img);
        y_lim = s(1);
        x_lim = s(2);
        
        x = P_c(1) ;
        y = P_c(2) ;
        
        radius = 0;
        r_min = 3; % lower bound of radius
        dx = 1;
        dy = 1;
        
        while ((y - dy > 0) && (y + dy < y_lim) && (x - dx > 0) && (x + dx < x_lim)) % confine pixel to the interior
            radius = radius + 1;
            dx = 1 * radius;
            dy = 1 * radius;
           
            up_r = img(y+dy, x-dx); %img(cols, rows)
               r = img(y+dy, x   ); 
           low_r = img(y+dy, x+dx);
              up = img(y   , x-dx);
             low = img(y   , x+dx);
            up_l = img(y-dy, x-dx); 
               l = img(y-dy, x   ); 
           low_l = img(y-dy, x+dx);
    
            if ( (up_r & r & low_r & up & low & up_l & l & low_l) == 0 )
                r_c = radius - 1;
                
                if (r_c <= 2)
                    kernel = [up_r, r, low_r, up, low, up_l, l, low_l];
                    P_c = translatePoint(kernel, P_c);
                    r_c = r_min;
                end
                
                break;
            end
        end
end