function exampleHelperTurtleBotShowGrid(sub)
    %exampleHelperTurtleBotShowGrid - Display bin and sort steps of VFH+ algorithm
    %   exampleHelperTurtleBotShowGrid(SUB) - Takes a subscriber to the laser scan topic and
    %   displays the obstacle grid created through histogram binning.
    %   Additionally the gaussian-smoothed grid is displayed, along with the
    %   final angle bin histogram, which is used to help decide which direction
    %   to travel
    %
    %   See also TurtleBotObstacleAvoidanceExample, exampleHelperTurtleBotComputeTargetAngle
    %
    %   Reference: Ulrich I., and Borenstein, J. "VFH+: Reliable Obstacle
    %   Avoidance for Fast Mobile Robots." IEEE International Conference on
    %   Robotics & Automation. Leuven, Belgium. May 1998.
    
    %   Copyright 2014-2015 The MathWorks, Inc.
    
    persistent ax 
    
    robotSize = 0.2; % Robot radius size
    
    if isempty(sub) || isempty(sub.LatestMessage)
        return;
    end
    
    scan = sub.LatestMessage;
    data = readCartesian(scan) * [0 1; -1 0];
    
    % Set bin step size for angular and grid maps
    bin.step = 0.05;
    bin.angStep = pi/48;
    
    % Set min and max bin values and create x and y bins for grid map
    binxMin = -1.5;
    binxMax = 1.5;
    bin.x = binxMin:bin.step:binxMax;
    binyMin = 0;
    binyMax = 3;
    bin.y = binyMin:bin.step:binyMax;
    
    % Create bins for angular map
    binAngMin = 0;
    binAngMax = pi;
    bin.ang = binAngMin:bin.angStep:binAngMax;
    
    % Create vector of angles corresponding to the bin
    angDist = bin.ang + bin.angStep/2 - pi/2;
    % Map all grid cells to their appropriate angles
    [xMeans, yMeans] = meshgrid(bin.x + bin.step/2, bin.y + bin.step/2);
    beta = atan2(yMeans,xMeans);
    [~, ~, indAng] = histcounts(beta,bin.ang);
    % Determine the L2 distance to each grid cell
    distmat = sqrt(xMeans.^2 + yMeans.^2);
    
    % Build a 2D histogram map of obstacle locations
    c = zeros(numel(bin.y),numel(bin.x));
    [~, ~, indX] = histcounts(data(:,1),bin.x);
    for j = 1:numel(bin.x)
        c(:,j) = [histcounts(data(j==indX,2),bin.y)'; 0];
    end
    
    figName = 'Histograms';
    fig = findall(0,'Type','Figure','Name',figName);
    if isempty(fig) || isempty(ax)
        % Create the figure and set all subplot labels.
        
        fig = figure('Name','Histograms','Position',[50,50,500,800]);
        
        ax(1) = subplot(3,1,1,'Parent',fig);
        xlabel(ax(1), 'x bins');
        ylabel(ax(1), 'y bins')
        title(ax(1), 'Raw Obstacles');
        
        ax(2) = subplot(3,1,2,'Parent',fig);
        xlabel(ax(2), 'x bins');
        ylabel(ax(2), 'y bins')
        title(ax(2), 'Smoothed Obstacles');
        
        ax(3) = subplot(3,1,3,'Parent',fig);
        xlabel(ax(3), 'Angle bin (radians)');
    end
    
    surf(ax(1), c);
    
    % Generate a gaussian filter and smooth the occupancy image to account
    % for robot radius
    sigma = robotSize/bin.step;
    gaussFilter = exampleHelperTurtleBotGaussian([ceil(3*sigma) ceil(3*sigma)],sigma);
    cSmooth = conv2(c,gaussFilter,'same');
    
    surf(ax(2), cSmooth);
    
    % Get parameters a and b for Equation (2) in Ulrich
    dmax = max(distmat(cSmooth~=0));  % equal to a/b
    b = 1;
    a = b*dmax.^2;
    
    % Computing the magnitude (m) for each cell in the obstacle histogram.
    % Equation (2) in Ulrich
    % Also expressing the result as a 1D vector histogram for occupancy at
    % possible angle trajectories (angHist), Equation (5) in Ulrich
    angHist = zeros(1,numel(bin.ang));
    if ~isempty(a)
        m = (cSmooth.^2).*(a*ones(numel(bin.y),numel(bin.x)) - b*(distmat.^2));
        for j = 1:numel(bin.ang)
            angHist(j) = sum(sum(m(indAng==j)),2);
        end
    end
    
    bar(ax(3), angDist,angHist);
end

