%rosAutomatedValetExampleHelperSetupGlobals
%
% Helper script that setsup all the globals for the example

%% Clear valet if it exists
clear valet;

%% Get costmap and vehicleDims
valet.mapLayers = loadParkingLotMapLayers;
valet.costmap = combineMapLayers(valet.mapLayers);
valet.vehicleDims      = vehicleDimensions;
valet.maxSteeringAngle = 35; % in degrees
valet.data = load('exampleHelperROSValetRoutePlan.mat');
valet.routePlan = valet.data.routePlan;
valet.currentPose = [4 12 0]; % [x, y, theta]


%% Create the vehicle simulator
valet.vehicleSim = ExampleHelperROSValetVehicleSimulator(valet.costmap, valet.vehicleDims);
valet.vehicleSim.setVehiclePose(valet.currentPose);

%%
% Create the behavioral planner helper object. The |requestManeuver| method
% requests a stream of navigation tasks from the behavioral planner until
% the destination is reached.
valet.behavioralPlanner = ExampleHelperROSValetBehavioralPlanner(valet.routePlan, valet.maxSteeringAngle);
valet.motionPlanner = pathPlannerRRT(valet.costmap, 'MinIterations', 1000, ...
    'ConnectionDistance', 10, 'MinTurningRadius', 20);

%%
% Plan a path from the current pose to the first goal by using the |plan|
% function. The returned
% |<docid:driving_ref#mw_9c84c0bf-efb6-4b92-bb73-32580caa3408
% driving.Path>| object, |refPath|, is a feasible and collision-free
% reference path.
valet.goalPose = valet.routePlan{1, 'EndPose'};
valet.refPath = plan(valet.motionPlanner, valet.currentPose, valet.goalPose);

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

%% Reset velocity
valet.currentVel  = 0; % meters/second
valet.vehicleSim.setVehicleVelocity(valet.currentVel);

%% Specify number of poses to return using a separation of approximately 0.1 m
valet.approxSeparation = 0.1; % meters
valet.numSmoothPoses   = round(valet.refPath.Length / valet.approxSeparation);

% Specify initial, maximum, and terminal speeds so that the vehicle starts
% stationary, accelerates to a speed of 5 meters/second, and comes to a
% stop.
valet.maxSpeed   = 5; % in meters/second
valet.startSpeed = 0; % in meters/second
valet.endSpeed   = 0; % in meters/second

% Return discretized poses along the smooth path
[valet.refPoses, valet.directions, valet.cumLengths, valet.curvatures] = smoothPathSpline(valet.transitionPoses, valet.directions, valet.numSmoothPoses);
% Generate a velocity profile
valet.refVelocities = exampleHelperROSValetGenerateVelocityProfile(valet.directions, valet.cumLengths, valet.curvatures, valet.startSpeed, valet.endSpeed, valet.maxSpeed);


%% 
% Create a |HelperLongitudinalController| object to control the velocity of the
% vehicle and specify the sample time.
valet.sampleTime = 0.1;
valet.lonController = ExampleHelperROSValetLongitudinalController('SampleTime', valet.sampleTime);

%%
% Use the |HelperFixedRate| object to ensure fixed-rate execution of the feedback
% controller. Use a control rate to be consistent with the longitudinal
% controller.
valet.controlRate = ExampleHelperROSValetFixedRate(1/valet.sampleTime); % in Hertz

%%
% Create a |HelperPathAnalyzer>| object to compute reference pose, reference velocity
% and driving direction for the controller.
valet.pathAnalyzer = ExampleHelperROSValetPathAnalyzer(valet.refPoses, valet.refVelocities, valet.directions, ...
    'Wheelbase', valet.vehicleDims.Wheelbase);

%%
% Define desired pose for the parking spot, returned by the V2X system
valet.parkPose = [36 44 90];


%% Supporting Functions
%%%
% *

%%%
% *loadParkingLotMapLayers*
% Load environment map layers for parking lot
function mapLayers = loadParkingLotMapLayers()
mapLayers.StationaryObstacles = imread('exampleHelperROSValetStationary.bmp');
mapLayers.RoadMarkings        = imread('exampleHelperROSValetRoadMarkings.bmp');
mapLayers.ParkedCars          = imread('exampleHelperROSValetParkedCars.bmp');
end

%%%
% *combineMapLayers*
% Combine map layers into a single costmap
function costmap = combineMapLayers(mapLayers)
combinedMap = mapLayers.StationaryObstacles + mapLayers.RoadMarkings + ...
    mapLayers.ParkedCars;
combinedMap = im2single(combinedMap);

res = 0.5; % meters
costmap = vehicleCostmap(combinedMap, 'CellSize', res);
end
