%% Generate Object and Lane Boundary Detections
% Create a driving scenario containing an ego vehicle and a target vehicle
% traveling along a three-lane road. Detect the lane boundaries by using a
% vision detection generator.
scenario = drivingScenario;

%%
% Create a three-lane road by using lane specifications.
roadCenters = [0 0 0; 60 0 0; 120 30 0];
lspc = lanespec(3);
road(scenario,roadCenters,'Lanes',lspc);
%%
% Specify that the ego vehicle follows the center lane at 30 m/s.
egovehicle = vehicle(scenario,'ClassID',1);
egopath = [1.5 0 0; 60 0 0; 111 25 0];
egospeed = 30;
smoothTrajectory(egovehicle,egopath,egospeed);

%%
% Specify that the target vehicle travels ahead of the ego vehicle at 40
% m/s and changes lanes close to the ego vehicle.
targetcar = vehicle(scenario,'ClassID',1);
targetpath = [8 2; 60 -3.2; 120 33];
targetspeed = 40;
smoothTrajectory(targetcar,targetpath,targetspeed);

%%
% Display a chase plot for a 3-D view of the scenario from behind the ego vehicle.
chasePlot(egovehicle)

%%
% Create a vision detection generator that detects lanes and objects. The
% pitch of the sensor points one degree downward.
visionSensor = visionDetectionGenerator('Pitch',1.0);
visionSensor.DetectorOutput = 'Lanes and objects';
visionSensor.ActorProfiles = actorProfiles(scenario);

%%
% Run the simulation.
%
% # Create a bird's-eye plot and the associated plotters.
% # Display the sensor coverage area.
% # Display the lane markings.
% # Obtain ground truth poses of targets on the road.
% # Obtain ideal lane boundary points up to 60 m ahead.
% # Generate detections from the ideal target poses and lane boundaries.
% # Display the outline of the target.
% # Display object detections when the object detection is valid.
% # Display the lane boundary when the lane detection is valid.
%
bep = birdsEyePlot('XLim',[0 60],'YLim',[-35 35]);
caPlotter = coverageAreaPlotter(bep,'DisplayName','Coverage area', ...
    'FaceColor','blue');
detPlotter = detectionPlotter(bep,'DisplayName','Object detections');
lmPlotter = laneMarkingPlotter(bep,'DisplayName','Lane markings');
lbPlotter = laneBoundaryPlotter(bep,'DisplayName', ...
    'Lane boundary detections','Color','red');
olPlotter = outlinePlotter(bep);
plotCoverageArea(caPlotter,visionSensor.SensorLocation,...
    visionSensor.MaxRange,visionSensor.Yaw, ...
    visionSensor.FieldOfView(1));
while advance(scenario)
    [lmv,lmf] = laneMarkingVertices(egovehicle);
    plotLaneMarking(lmPlotter,lmv,lmf)
    tgtpose = targetPoses(egovehicle);
    lookaheadDistance = 0:0.5:60;
    lb = laneBoundaries(egovehicle,'XDistance',lookaheadDistance,'LocationType','inner');
    [obdets,nobdets,obValid,lb_dets,nlb_dets,lbValid] = ...
        visionSensor(tgtpose,lb,scenario.SimulationTime); 
    [objposition,objyaw,objlength,objwidth,objoriginOffset,color] = targetOutlines(egovehicle);
    plotOutline(olPlotter,objposition,objyaw,objlength,objwidth, ...
        'OriginOffset',objoriginOffset,'Color',color)    
    if obValid
        detPos = cellfun(@(d)d.Measurement(1:2),obdets,'UniformOutput',false);
        detPos = vertcat(zeros(0,2),cell2mat(detPos')');
        plotDetection(detPlotter,detPos)
    end
    if lbValid
        plotLaneBoundary(lbPlotter,vertcat(lb_dets.LaneBoundaries))
    end
end

%% 
% Copyright 2012 The MathWorks, Inc.