%% Import HERE HDLM Roads Using Specified Coordinates
% Import HERE HDLM road network data that is nearest to the coordinates of a 
% specified driving route into a driving scenario. Plot a vehicle following
% this route in the driving scenario.
%%
% Load a sequence of geographic coordinates that correspond to a driving
% route. 

data = load('geoSequence.mat');
lat = data.latitude; 
lon = data.longitude;
%% 
% Display the route by streaming the coordinates on a geographic player.
% Set the zoom level to 14 and configure the player to display all points
% in its history. To speed up the streaming, plot only every tenth
% coordinate in the route.

zoomLevel = 14;
player = geoplayer(lat(1),lon(1),zoomLevel,'HistoryDepth',Inf);
timestep = 10;

for i = 1:timestep:length(lat)
    plotPosition(player,lat(i),lon(i));
end
%% 
% Create a driving scenario. Import the HERE HDLM road data that is nearest
% to the driving route into the scenario.
scenario = drivingScenario;
roadNetwork(scenario,'HEREHDLiveMap',lat,lon);
%% 
% Use the |<docid:driving_ref#mw_80e7a72c-09f3-40cc-a4c7-2472f5c534d9
% latlon2local>| function to convert the driving route from geographic
% coordinates to local east-north-up (ENU) Cartesian coordinates used in
% the driving scenario. For the origin of the ENU coordinate system, use
% the geographic road network origin stored in the |GeoReference| property
% of the scenario. The origin is the first coordinate specified in the
% driving route. Because the driving route contains only latitudinal and
% longitudinal data, set the altitude to |0|.

alt = 0;
origin = scenario.GeoReference;
[xEast,yNorth,zUp] = latlon2local(lat,lon,alt,origin);
%% 
% Add a vehicle to the driving scenario. Specify the converted driving
% route as the trajectory of the vehicle. Set a vehicle speed of 30 meters
% per second.

v = vehicle(scenario,'ClassID',1);
speed = 30;
smoothTrajectory(v,[xEast,yNorth,zUp],speed);
%% 
% Plot the scenario and pause every 0.01 seconds to slow down the
% simulation. To maintain the same alignment with geographic coordinate
% displays, the _X_-axis is on the bottom and the _Y_-axis is on the left.
% In driving scenarios not imported from maps, the _X_-axis is on the left
% and the _Y_-axis is on the bottom. This alignment is consistent with the
% Automated Driving Toolbox(TM) world coordinate system.
% 
% After a few seconds, the vehicle appears to drive underneath the road.
% This issue occurs because the converted trajectory contains no altitude
% data but the imported road network does. To avoid this issue, if you are
% specifying a driving route recorded from a GPS, include the altitude
% data.

plot(scenario)
while advance(scenario)
    pause(0.01)
end
%% 
% Copyright 2019 The MathWorks, Inc.