% Create tracking scenario
scenario = trackingScenario;

% Add a stationary platform to model the ATC tower
tower = platform(scenario);

rpm = 12.5;
fov = [1.4;10];
scanrate = rpm*360/60;  % deg/s
updaterate = scanrate/fov(1); % Hz

radar = monostaticRadarSensor(1, 'Rotator', ...
    'UpdateRate', updaterate, ...           % Hz
    'FieldOfView', fov, ...                 % [az;el] deg
    'MaxMechanicalScanRate', scanrate, ...  % deg/sec
    'AzimuthResolution', fov(1), ...        % deg
    'ReferenceRange', 111e3, ...            % m
    'ReferenceRCS', 0, ...                  % dBsm
    'RangeResolution', 135, ...             % m
    'HasINS', true, ...
    'DetectionCoordinates', 'Scenario');

% Mount radar at the top of the tower
radar.MountingLocation = [0 0 -15];
tower.Sensors = radar;

% Enable elevation scanning
radar.HasElevation = true;

% Set mechanical elevation scan to begin at 2 degrees above the horizon
elFov = fov(2);
tilt = 2; % deg
radar.MechanicalScanLimits(2,:) = [-fov(2) 0]-tilt; % deg

radar.FieldOfView(2) = elFov+1e-3;

% Duration of scenario
sceneDuration = 60; % s

% Inbound airliner
ht = 3e3;
spd = 900*1e3/3600; % m/s
wp = [-5e3 -40e3 -ht;-5e3 -40e3+spd*sceneDuration -ht];
traj = waypointTrajectory('Waypoints',wp,'TimeOfArrival',[0 sceneDuration]);
platform(scenario,'Trajectory', traj);

% Outbound airliner
ht = 4e3;
spd = 700*1e3/3600; % m/s
wp = [20e3 10e3 -ht;20e3+spd*sceneDuration 10e3 -ht];
traj = waypointTrajectory('Waypoints',wp,'TimeOfArrival',[0 sceneDuration]);
platform(scenario,'Trajectory', traj);

% Tangential airliner
ht = 4e3;
spd = 300*1e3/3600; % m/s
wp = [-20e3 -spd*sceneDuration/2 -ht;-20e3 spd*sceneDuration/2 -ht];
traj = waypointTrajectory('Waypoints',wp,'TimeOfArrival',[0 sceneDuration]);
platform(scenario,'Trajectory', traj);

tracker = trackerGNN( ...
    'Assignment', 'Auction', ...
    'AssignmentThreshold',50, ...
    'FilterInitializationFcn',@initFilter);

origin = [42.366978, -71.022362, 50];
mapViewer = helperATCMap('ReferenceLocation',origin);
setCamera(mapViewer, origin + [0 0 1e5]);
showScenario(mapViewer,scenario);
snap(mapViewer);

% Set simulation to advance at the update rate of the radar
scenario.UpdateRate = radar.UpdateRate;

% Create a buffer to collect the detections from a full scan of the radar
scanBuffer = {};

% Initialize the track array
tracks = [];

% Save visualization snapshots for each scan
allsnaps = {};
scanCount = 0;

% Set random seed for repeatable results
s = rng;
rng(2020)

while advance(scenario)
    
    % Update airliner positions
    plotTarget(mapViewer, scenario.Platforms([2 3 4]));
    
    % Generate detections on targets in the radar's current field of view
    [dets,config] = detect(scenario);
    scanBuffer = [scanBuffer;dets]; %#ok<AGROW>
    % Plot beam and detections
    plotCoverage(mapViewer,coverageConfig(scenario))
    plotDetection(mapViewer,scanBuffer);
    
    
    % Update tracks when a 360 degree scan is complete
    simTime = scenario.SimulationTime;
    isScanDone = config.IsScanDone;
    if isScanDone
        scanCount = scanCount+1;
        % Update tracker
        [tracks,~,~,info] = tracker(scanBuffer,simTime);
        % Clear scan buffer for next scan
        scanBuffer = {};
    elseif isLocked(tracker)
        % Predict tracks to the current simulation time
        tracks = predictTracksToTime(tracker,'confirmed',simTime);
    end
    
    % Update map and take snapshots
    allsnaps = snapPlotTrack(mapViewer,tracks,isScanDone, scanCount, allsnaps);

end
allsnaps = [allsnaps, {snap(mapViewer)}];

figure
imshow(allsnaps{1});

figure
imshow(allsnaps{2});

figure
imshow(allsnaps{3});

figure
imshow(allsnaps{4});

figure
imshow(allsnaps{5});

figure
imshow(allsnaps{6});

truthTrackTable = tabulateData(scenario, tracks) %#ok<NOPTS>

% Reposition and orient the camera to show the 3-D nature of the map
camPosition = origin + [0.367, 0.495, 1.5e4];
camOrientation = [0, -17, 235]; %Looking south west, 17 degrees below the horizon
setCamera(mapViewer, camPosition, camOrientation);

snap(mapViewer);

function filter = initFilter(detection)
filter = initcvekf(detection);
classToUse = class(filter.StateCovariance);

% Airliners can move at speeds around 900 km/h. The velocity is
% initialized to 0, but will need to be able to quickly adapt to
% aircraft moving at these speeds. Use 900 km/h as 1 standard deviation
% for the initialized track's velocity noise.
spd = 900*1e3/3600; % m/s
velCov = cast(spd^2,classToUse);
cov = filter.StateCovariance;
cov(2,2) = velCov;
cov(4,4) = velCov;
filter.StateCovariance = cov;

% Set filter's process noise to match filter's update rate
scaleAccelHorz = cast(1,classToUse);
scaleAccelVert = cast(1,classToUse);
Q = blkdiag(scaleAccelHorz^2, scaleAccelHorz^2, scaleAccelVert^2);
filter.ProcessNoise = Q;
end

function truthTrackTable = tabulateData(scenario, tracks)
% Process truth data
platforms = scenario.Platforms(2:end); % Platform 1 is the radar
numPlats = numel(platforms);
trueAlt = zeros(numPlats,1);
trueSpd = zeros(numPlats,1);
trueHea = zeros(numPlats,1);
for i = 1:numPlats
    traj = platforms{i}.Trajectory;
    waypoints = traj.Waypoints;
    times = traj.TimeOfArrival;
    trueAlt(i) = -waypoints(end,3);
    trueVel = (waypoints(end,:) - waypoints(end-1,:)) / (times(end)-times(end-1));
    trueSpd(i) = norm(trueVel) * 3600 / 1000; % Convert to km/h
    trueHea(i) = atan2d(trueVel(1),trueVel(2));
end
trueHea = mod(trueHea,360);

% Associate tracks with targets
atts = [tracks.ObjectAttributes];
tgtInds = [atts.TargetIndex];

% Process tracks assuming a constant velocity model
numTrks = numel(tracks);
estAlt = zeros(numTrks,1);
estSpd = zeros(numTrks,1);
estHea = zeros(numTrks,1);
truthTrack = zeros(numTrks,7);
for i = 1:numTrks
    estAlt(i) = -round(tracks(i).State(5));
    estSpd(i) = round(norm(tracks(i).State(2:2:6)) * 3600 / 1000); % Convert to km/h;
    estHea(i) = round(atan2d(tracks(i).State(2),tracks(i).State(4)));
    estHea(i) = mod(estHea(i),360);
    platID = tgtInds(i);
    platInd = platID - 1;
    truthTrack(i,:) = [tracks(i).TrackID, trueAlt(platInd), estAlt(i), trueHea(platInd), estHea(i), ...
        trueSpd(platInd), estSpd(i)];
end

% Organize the data in a table
names = {'TrackID','TrueAlt','EstimatedAlt','TrueHea','EstimatedHea','TrueSpd','EstimatedSpd'};
truthTrackTable = array2table(truthTrack,'VariableNames',names);
truthTrackTable = mergevars(truthTrackTable, (6:7), 'NewVariableName', 'Speed', 'MergeAsTable', true);
truthTrackTable.(6).Properties.VariableNames = {'True','Estimated'};
truthTrackTable = mergevars(truthTrackTable, (4:5), 'NewVariableName', 'Heading', 'MergeAsTable', true);
truthTrackTable.(4).Properties.VariableNames = {'True','Estimated'};
truthTrackTable = mergevars(truthTrackTable, (2:3), 'NewVariableName', 'Altitude', 'MergeAsTable', true);
truthTrackTable.(2).Properties.VariableNames = {'True','Estimated'};
truthTrackTable.TrackID = "T" + string(truthTrackTable.TrackID);
end

function allsnaps = snapPlotTrack(mapViewer,tracks,isScanDone, scanCount,allsnaps)
% Save snapshots during first 4 scans
if isScanDone && any(scanCount == [2 3])
    allsnaps = [allsnaps, {snap(mapViewer)}];
    %move camera
    if scanCount == 2
        % Show the outbound airliner
        setCamera(mapViewer, [42.5650  -70.8990 7e3]);
        allsnaps = [allsnaps, {snap(mapViewer)}];
    end
    
end

% Update display with current track positions
plotTrack(mapViewer,tracks);

if isScanDone && any(scanCount == [2 3])
    % Take a snapshot of confirmed track
    allsnaps = [allsnaps, {snap(mapViewer)}];
    % Reset Camera view to full scene
    if scanCount == 3
        origin = [42.366978, -71.022362, 50];
        setCamera(mapViewer, origin + [0 0 1e5]);
    end
end
end