function exampleHelperROSValetPlannerCallback(msg, planning, valet)
% exampleHelperROSValetPlannerCallback Callback of the planner
% This implements the planner functionality

% Copyright 2019 The MathWorks, Inc.

if ~msg.Data
    return;
end

currentVelMsg = planning.CurVelSub.LatestMessage;
currentVel = currentVelMsg.Data;

currentPoseMsg = planning.CurPoseSub.LatestMessage;
currentPose = [currentPoseMsg.X, currentPoseMsg.Y, currentPoseMsg.Theta];

if reachedDestination(valet.behavioralPlanner)
    return;
end

[nextGoal, plannerConfig, speedConfig] = requestManeuver(valet.behavioralPlanner, ...
    currentPose, currentVel);

% Configure the motion planner
configurePlanner(valet.motionPlanner, plannerConfig);

% Plan a reference path using RRT* planner to the next goal pose
refPath = plan(valet.motionPlanner, currentPose, nextGoal);

% Check if the path is valid. If the planner fails to compute a path,
% or the path is not collision-free because of updates to the map, the
% system needs to re-plan. This scenario uses a static map, so the path
% will always be collision-free.
isReplanNeeded = ~checkPathValidity(refPath, valet.costmap);
if isReplanNeeded
    warning('Unable to find a valid path. Attempting to re-plan.')
    
    % Request behavioral planner to re-plan
    replanNeeded(valet.behavioralPlanner);
    return;
end

% Retrieve transition poses and directions from the planned path
[transitionPoses, directions] = interpolate(refPath);

% Smooth the path
numSmoothPoses   = round(refPath.Length / valet.approxSeparation);
[refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses);

speedMsg = planning.SpeedPub.rosmessage;
speedMsg.Data = [speedConfig.StartSpeed, speedConfig.EndSpeed, speedConfig.MaxSpeed];
send(planning.SpeedPub, speedMsg);

directionsMsg = planning.DirPub.rosmessage;
directionsMsg.Data = directions;
send(planning.DirPub, directionsMsg);

smoothpathMsg = planning.PathPub.rosmessage;
smoothpathMsg.Data = reshape(refPoses, 1, numel(refPoses));
send(planning.PathPub, smoothpathMsg);

nextGoalMsg = planning.NxtPub.rosmessage;
nextGoalMsg.X = nextGoal(1);
nextGoalMsg.Y = nextGoal(2);
nextGoalMsg.Theta = nextGoal(3);
send(planning.NxtPub, nextGoalMsg);

% Generate a velocity profile
refVelocities = exampleHelperROSValetGenerateVelocityProfile(directions, cumLengths, curvatures, valet.startSpeed, valet.endSpeed, valet.maxSpeed);
velprofileMsg = planning.VelPub.rosmessage;
velprofileMsg.Data = refVelocities;
send(planning.VelPub, velprofileMsg);
end


function configurePlanner(pathPlanner, config)
%configurePlanner
% Configure the path planner object, pathPlanner, with settings specified
% in struct config.

fieldNames = fields(config);
for n = 1 : numel(fieldNames)
    if ~strcmpi(fieldNames{n}, 'IsParkManeuver')
        pathPlanner.(fieldNames{n}) = config.(fieldNames{n});
    end
end
end