function [radius, center] = Bubblegeneration(waypoints, workspace)
% Given a set of reference waypoints and a workspace with obstacles, this
% function attempts to compute a set of bubbles centered at waypoints, one
% for each, that is collision-free. 
% If such a bubble cannot found, we move the center until a collision free
% bubble is found.
% A lower bound r_l is considered for the radius of the bubbles so that the
% generated bubbles have intersection with neighboring bubbles, in other
% words, there is no gap between them. Also an upper bound r_u (10% of
% workspace) is considered to keep the optimization local.

        step = 1;           %moving step
        radius = 0;         %radius of the bubble
        r_l = 5;            %lower bound of the radius
        r_u = 50;           %upper bound of the radius
        limit = size (workspace);   %size of the workspace
        center = waypoints;
        
        while ((center(2) - step > 0) && (center(2) + step < limit(1)) && (center(1) - step > 0) && (center(1) + step < limit(2))) 
            radius = radius + 1;
            step = radius;
            right = workspace(center(2)+step, center(1));
            left = workspace(center(2)-step, center(1)); 
            upper = workspace(center(2), waypoints(1)-step);
            lower = workspace(center(2), waypoints(1)+step);
            upper_right = workspace(center(2)+step, center(1)-step);
            upper_low = workspace(center(2)-step, center(1)-step); 
            lower_right = workspace(center(2)+step, center(1)+step);
            lower_low = workspace(center(2)-step, center(1)+step);  
             
            %collision detection
            if ((right & left & upper & lower & upper_right & upper_low & lower_right & lower_low) == 0 )
                radius = radius - 1;
                
                %Applying upper bound limit
                if (radius > r_u)
                    radius = r_u;
                end
                 
                %Applying lower bound limit
                if (radius < r_l)
                     move_step = 1;
                     if ( upper == 0 || upper_right == 0 || right == 0 )
                         center =  center + [-move_step, -move_step];
                     elseif ( upper== 0 || upper_left == 0 || left== 0 )
                         center =  center + [move_step, -move_step];
                     elseif ( right== 0 || lower_right == 0 || lower== 0 )
                         center =  center + [-move_step, move_step];
                     elseif ( left== 0 || lower_left == 0 || lower == 0 )
                         center =  center + [move_step, move_step];
                     end
                    radius = r_l;
                end
                break;
            end
        end
end