% Reproducible target locations
rng(2022);

% Create a trackingScenario object
scene = trackingScenario('UpdateRate',10,'StopTime',10);
numTgts = 5;

% Initialize position and velocity of each target
x = 100*(2*rand(numTgts,1) - 1);
y = 100*(2*rand(numTgts,1) - 1);
z = zeros(numTgts,1);
vx = 5*randn(numTgts,1);
vy = 5*randn(numTgts,1);
vz = zeros(numTgts,1);

% Add platforms to scenario with given positions and velocities.
for i = 1:numTgts
    thisTgt = platform(scene);
    thisTgt.Trajectory.Position = [x(i) y(i) z(i)];
    thisTgt.Trajectory.Velocity = [vx(i) vy(i) vz(i)];
end

% Add a detecting platform to the scene.
detectingPlatform = platform(scene);
detectingPlatform.Trajectory.Position = [-200 0 0];

% Simulate 2-D radar using a monostaticRadarSensor.
radar = monostaticRadarSensor(1,...
    'UpdateRate',scene.UpdateRate,...
    'DetectionProbability',0.9,...% Pd
    'FalseAlarmRate',1e-3,...% Pfa
    'FieldOfView',[120 1],...
    'ScanMode','No Scanning',...
    'DetectionCoordinates','scenario',...% Report in scenario frame
    'HasINS',true,...                    % Enable INS to report in scenario frame
    'ReferenceRange',300,...             % Short-range
    'ReferenceRCS',10,...                % Default RCS of platforms
    'MaxUnambiguousRange',400,...        % Short-range
    'RangeResolution',1,...
    'AzimuthResolution',1,...
    'HasFalseAlarms',true);              % Reports false alarms

% Mount the radar on the detecting platform.
detectingPlatform.Sensors = radar;

% Transform function and limits
sensorTransformFcn = @(x,params)x;
sensorTransformParameters = struct;
sensorLimits = [-inf inf].*ones(6,1);

% Detection probability for sensor configuration
Pd = radar.DetectionProbability;

config = trackingSensorConfiguration('SensorIndex',1,...
    'IsValidTime',true,...% Update every step
    'MaxNumDetsPerObject',1,...
    'SensorTransformFcn',sensorTransformFcn,...
    'SensorTransformParameters',sensorTransformParameters,...
    'SensorLimits',sensorLimits,...
    'DetectionProbability',Pd);

% Sensor Parameters to calculate Volume of the resolution bin
Rm = radar.MaxUnambiguousRange/2; % mean -range
dTheta = radar.ElevationResolution;
% Bias fractions reduce the "effective" resolution size
dPhi = radar.AzimuthBiasFraction*radar.AzimuthResolution;
dR = radar.RangeBiasFraction*radar.RangeResolution;

% Cell volume
VCell = 2*((Rm+dR)^3 - Rm^3)/3*(sind(dTheta/2))*deg2rad(dPhi);

% False alarm rate 
Pfa = radar.FalseAlarmRate;

% Define clutter density
config.ClutterDensity = Pfa/VCell;

config.FilterInitializationFcn = @initcvgmphd;

% Use number of radar cells to compute number of false alarms per unit time.
NCells = radar.MaxUnambiguousRange/radar.RangeResolution*radar.FieldOfView(1)/radar.AzimuthResolution;
numFalse = Pfa*NCells;

% Choose an initial weight of 0.05 for each new component. As number of new

% targets is not known, new number of components are simply used as number
% of false alarms. 0.1 is the update rate.
birthRate = 0.05*(numFalse)/0.1;

% Create a tracker with the defined sensor configuration.
tracker = trackerPHD('BirthRate',birthRate,...
    'SensorConfigurations', config);

% Create gospa metric object.
gospa = trackGOSPAMetric;

% Initialize variable for storing metric at each step.
gospaMetric = zeros(0,1);
loc = zeros(0,1);
mt = zeros(0,1);
ft = zeros(0,1);

% Create a display
display = helperClutterTrackingDisplay(scene);

count = 1; % Counter for storing metric data
rng(2018); % Reproducible run

while advance(scene)
    % Current time
    time = scene.SimulationTime;
    
    % Current detections
    detections = detect(scene);
    
    % Tracks
    tracks = tracker(detections, time);
    
    % Update display
    display(scene, detections, tracks);
    
    % Compute GOSPA. getTruth function is defined below.
    truths = getTruth(scene);
    [~,gospaMetric(count),~,loc(count),mt(count),ft(count)] = gospa(tracks, truths);
    count = count + 1;
end

figure('Units','normalized','Position',[0.1 0.1 0.8 0.8])
subplot(2,2,[1 2]); plot(gospaMetric,'LineWidth',2); title('Total GOSPA');
ylabel('GOSPA Metric'); xlabel('Time step'); grid on;
subplot(2,2,3); plot(mt,'LineWidth',2); title('Missed Target GOSPA');
ylabel('Missed Target Metric'); xlabel('Time step'); grid on;
subplot(2,2,4); plot(ft,'LineWidth',2); title('False Track GOSPA');
ylabel('False Target Metric'); xlabel('Time step'); grid on;

% Turn on flag to run Monte Carlo Simulations
Pfas = 10.^linspace(-4,-3,4);
runMonteCarlo = false;
if runMonteCarlo
    numRunsPerPfa = 50; %#ok<UNRCH> 
    gospaMCs = zeros(4,numRunsPerPfa,4);
    for i = 1:4
        gospaMCs(i,:,:) = helperRunMonteCarloAnalysis(scene, Pfas, Pfas(i), numRunsPerPfa);
    end
    save clutterTrackingMCRuns.mat gospaMCs;
else 
    % Reload results from last run
    load('clutterTrackingMCRuns.mat','gospaMCs');
end

% Plot results
figure('Units','normalized','Position',[0.1 0.1 0.8 0.8])
subplot(2,2,[1 2]); plot(gospaMCs(:,:,1)','LineWidth',2); title('GOSPA');
legend(strcat('Pfa = ',string(Pfas)),'Orientation','horizontal','Location','NorthOutside');
ylabel('Average GOSPA Metric'); xlabel('Monte Carlo Run'); grid on;
subplot(2,2,3); plot(gospaMCs(:,:,3)','LineWidth',2); title('Missed Target GOSPA');
ylabel('Average Missed Target Metric'); xlabel('Monte Carlo Run'); grid on;
subplot(2,2,4); plot(gospaMCs(:,:,4)','LineWidth',2); title('False Track GOSPA');
ylabel('Average False Track Metric'); xlabel('Monte Carlo Run'); grid on;

function gospaMC = helperRunMonteCarloAnalysis(scene, Pfas, Pfai, numRuns)

clear clutterSimTracker_kernel;

% Compute which tracker to run based on provided Pfa
settingToRun = find(Pfas == Pfai,1,'first');

% Initialize ospa for each Monte Carlo run
gospaMC = zeros(numRuns,4);

% Use parfor for parallel simulations
parfor i = 1:numRuns
    rng(i, 'twister');
    
    % Restart scenario before each run
    restart(scene);
    
    % metrics vs time in this scenario
    gospaMetric = zeros(0,4);
    
    % Counter
    count = 0;
    
    % Advance scene and run
    while advance(scene)
        count = count + 1;
        
        % Use radar sensor using targetPoses function
        own = scene.Platforms{end};
        tgtPoses = targetPoses(own,'rotmat');
        insPose = pose(own);
        poses = platformPoses(scene,'rotmat');
        truths = poses(1:end-1);
        time = scene.SimulationTime;
        
        % Reset tracker at the end of step so at the next call tracker is
        % reset to time 0.
        systemToReset = settingToRun*(abs(time - scene.StopTime) < 1e-3);
        
        % Store ospa metric. Tracks and detections can be outputted to run
        % scenario in code generation without needing Monte Carlo runs.
        [~,~,gospaMetric(count,:)] = clutterSimTracker_kernel(Pfas, tgtPoses, insPose, truths, time, settingToRun, systemToReset);
    end
    
    % Compute average of GOSPA in this run
    % Start from time step 20 to allow track establishment.
    gospaMC(i,:) = mean(gospaMetric(20:end,:));
end

end

function helperGenerateCode(scene,Pfas) %#ok<DEFNU>

% Generate sample pose using platformPoses
poses = platformPoses(scene,'rotmat');

% Generate sample inputs for the kernel function
%
% Pfas must be compile-time constant and they cannot change with time
Pfas = coder.Constant(Pfas);

% Target poses as a variable size arrray of max 20 elements
tgtPoses = coder.typeof(poses(1),[20 1],[1 0]);

% Scalar ins pose
insPose = pose(scene.Platforms{1},'true');

% Truth information with maximum 20 targets
truths = coder.typeof(poses(1),[20 1],[1 0]);

% Time
time = 0;

% Which false-alarm setting to run and which to reset. Reset is necessary
% after each run to reinitialize the tracker
systemToRun = 1;
systemToReset = 1;

inputs = {Pfas, tgtPoses, insPose, truths, time, systemToRun, systemToReset}; %#ok<NASGU>

% Same name as MATLAB file allows to shadow the MATLAB function when
% MEX file is available and execute code in MEX automatically.
codegen clutterSimTracker_kernel -args inputs -o clutterSimTracker_kernel;

end

function truths = getTruth(scenario)
platPoses = platformPoses(scenario); % True Information
truths = platPoses(1:end-1); % Last object is ownship
end