function [scenario, egoCar] = helperCreateExtendedObjectTrackingScenario
% Scenario generation comprises generating a road network, defining
% vehicles that move on the roads, and moving the vehicles.
% 
% In this example, you test the ability of the sensor fusion to track a
% vehicle that is passing on the left of the ego vehicle. The scenario
% simulates a highway setting, and additional vehicles are in front of and
% behind the ego vehicle.

% Copyright 2021 The Mathworks, Inc.

% Define an empty scenario.
scenario = drivingScenario;
scenario.SampleTime = 0.1;

%% 
% Add a stretch of 500 meters of typical highway road with two lanes. The 
% road is defined using a set of points, where each point defines the center of 
% the road in 3-D space. 
roadCenters = [0 0; 50 0; 100 0; 250 20; 400 35];
road(scenario, roadCenters, 'lanes',lanespec(3));

%% 
% Create the ego vehicle and three cars around it: one that overtakes the
% ego vehicle and passes it on the left, one that drives right in front of
% the ego vehicle and one that drives right behind the ego vehicle. All the
% cars follow the trajectory defined by the road waypoints by using the
% |trajectory| driving policy. The passing car will start on the right
% lane, move to the left lane to pass, and return to the right lane.

% Create the ego vehicle that travels at 25 m/s along the road.  Place the
% vehicle on the right lane by subtracting off half a lane width (1.8 m)
% from the centerline of the road.
egoCar = vehicle(scenario, 'ClassID', 1);
trajectory(egoCar, roadCenters(2:end,:) - [0 0], 25); % On right lane

% Add a truck in front of the ego vehicle
truck = vehicle(scenario, 'ClassID', 1, 'Length', 8.1, 'Width', 2.45, 'Height',4);
trajectory(truck, [86 0;roadCenters(3:end,:)] + [0 -3.6], 25); % On center lane

% % Add a passing car that travels at 35 m/s along the road
passingCar = vehicle(scenario, 'ClassID', 1, 'Length', 4.7, 'Width', 1.8, 'Height', 1.4);
waypoints = [0 0; 50 1.8; 100 1.8; 250 21.8; 400 36.8] + [0 1.8];
trajectory(passingCar, waypoints, 35);

% % Add a car behind the ego vehicle
chaseCar = vehicle(scenario, 'ClassID', 1, 'Length', 4.7, 'Width', 1.8, 'Height', 1.4);
trajectory(chaseCar, [30 0; roadCenters(2:end,:)] - [0 0], 25); % On right lane

% A car in front of ego
followCar = vehicle(scenario, 'ClassID', 1, 'Length', 4.7, 'Width', 1.8, 'Height', 1.4);
trajectory(followCar, [75 0; roadCenters(3:end,:)] - [0 0], 25); % On right lane
end