function controlAngle = exampleHelperTurtleBotComputeTargetAngle( goal, data, pose, anglePrev, gains )
    %exampleHelperTurtleBotComputeTargetAngle - uses the VFH+ algorithm to steer a robot to a desired
    %   location
    %   C = exampleHelperTurtleBotComputeTargetAngle(GOAL,DATA,POSE,ANGLE) returns the desired
    %   control angle determined through the VFH+ algorithm. The method
    %   steers toward the goal point while avoiding obstacles indicated by
    %   data, a Nx2 vector of x and y coordinates of obstacles (in the pose
    %   frame of reference).
    %
    %   See also TurtleBotObstacleAvoidanceExample
    %
    %   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 The MathWorks, Inc.
    
    robotSize = 0.3; % Robot radius size
    maxKinectAngle = pi/6;
    
    % Get bin properties, distance, angle, and angle index matrices
    bin = setBin();
    [distmat, angleMap, indAng] = makeMappings(bin);
    
    % 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
    
    % Discard entries for obstacles beyond goal point
    c(distmat>norm(goal(1:2)-pose(1:2),2)) = 0;
    
    % 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');
    
    % 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
    
    % Compute the cost according to Ulrich, Equation (16)
    goalCost = generateCost(angleMap,goal,pose,anglePrev,angHist,gains);
    
    % Eliminate angles outside Kinect viewing range
    goalCost(abs(angleMap) > maxKinectAngle) = inf;
    
    % Extract the minimum cost angle
    [~, targetIndex] = min(goalCost);
    
    controlAngle = angleMap(targetIndex);
end

function binStruct = setBin()
    % SETBIN - Sets all the properties for the histogram bins
    persistent b;
    
    if isempty(b)
        % Set bin step size for angular and grid maps
        b.step = 0.05; % 0.2
        b.angStep = pi/48; % pi/24
        
        % Set min and max bin values and create x and y bins for grid map
        binxMin = -1.5;
        binxMax = 1.5;
        b.x = binxMin:b.step:binxMax;
        binyMin = 0;
        binyMax = 3;
        b.y = binyMin:b.step:binyMax;
        
        % Create bins for angular map
        binAngMin = 0;
        binAngMax = pi;
        b.ang = binAngMin:b.angStep:binAngMax;
    end
    
    binStruct = b;
end

function [dist, ang, index] = makeMappings(b)
    % MAKEMAPPINGS - Creates distance, angle, and index matrices
    % corresponding to the spacial occupancy grid
    
    persistent distmat;
    persistent angDist;
    persistent indAng;
    
    if isempty(distmat)
        % Create vector of angles corresponding to the bin
        angDist = b.ang + b.angStep/2 - pi/2;
        % Map all grid cells to their appropriate angles
        [xMeans, yMeans] = meshgrid(b.x + b.step/2, b.y + b.step/2);
        beta = atan2(yMeans,xMeans);
        [~, ~, indAng] = histcounts(beta,b.ang);
        % Determine the L2 distance to each grid cell
        distmat = sqrt(xMeans.^2 + yMeans.^2);
    end
    
    dist = distmat;
    ang = angDist;
    index = indAng;
    
end

function cost = generateCost(angMap, g, p, angPrev, angleHist, gains)
    % GENERATECOST - Creates a cost distribution over the angle histogram
    % bins
    
    if isempty(angPrev)
        angPrev = 0;
    end
    
    % Determine angle to get to goal directly
    targetPoint = g(1:2) - p(1:2);
    targetAngleGlobal = atan2(targetPoint(2),targetPoint(1));
    targetAngleRobot = targetAngleGlobal - p(3);
    
    targetVect = repmat(targetAngleRobot,[1,numel(angMap)]);
    angPrevVect = repmat(angPrev,[1,numel(angMap)]);
    
    % Determine angles for goal targeting and continuous trajectory
    goalAng = angMap-targetVect;
    goalAng(goalAng<-pi) = goalAng(goalAng<-pi)+2*pi;
    goalAng(goalAng>pi) = goalAng(goalAng>pi)-2*pi;
    
    contAng = angMap-angPrevVect;
    contAng(contAng<-pi) = contAng(contAng<-pi)+2*pi;
    contAng(contAng>pi) = contAng(contAng>pi)-2*pi;
    
    % Compute cost incorporating four terms
    cost = gains.goalTargeting*(abs(goalAng)) + gains.forwardPath*sqrt(abs(angMap)) ...
        + gains.continuousPath*sqrt(abs(contAng)) + gains.obstacleAvoid*angleHist;
    
end