load('rectangularTargetDetections.mat','detections','truthState');

tgtState = [3;48;0;60;0;5;1.9];
zExp = ctrectmeas(tgtState,detections);

theaterP = theaterPlot;
stateP = trackPlotter(theaterP,'DisplayName','State','MarkerFaceColor','g');
truthP = trackPlotter(theaterP,'DisplayName','Truth','MarkerFaceColor', 'b');
detP = detectionPlotter(theaterP,'DisplayName','Detections','MarkerFaceColor','r');
expDetP = detectionPlotter(theaterP,'DisplayName','Expected Detections','MarkerFaceColor','y');
l = legend(theaterP.Parent);
l.AutoUpdate = 'on';
hold on;
assignP = plot(theaterP.Parent,NaN,NaN,'-.','DisplayName','Association');

inDets = [detections{:}];
inMeas = horzcat(inDets.Measurement);
detP.plotDetection(inMeas');

zExpPlot = reshape(zExp,3,[]);
expDetP.plotDetection(zExpPlot');

zLines = nan(2,numel(detections)*3);
zLines(1,1:3:end) = zExpPlot(1,:);
zLines(2,1:3:end) = zExpPlot(2,:);
zLines(1,2:3:end) = inMeas(1,:);
zLines(2,2:3:end) = inMeas(2,:);
assignP.XData = zLines(1,:);
assignP.YData = zLines(2,:);

truthPos = [truthState(1:2);0];
truthDims = struct('Length',truthState(6),...
    'Width',truthState(7),...
    'Height', 0,...
    'OriginOffset', [0 0 0]);
truthOrient = quaternion([truthState(4) 0 0],'eulerd', 'ZYX','frame');
truthP.plotTrack(truthPos',truthDims,truthOrient);

statePos = [tgtState(1:2);0];
stateDims = struct('Length',tgtState(6),...
    'Width',tgtState(7),...
    'Height',0,...
    'OriginOffset', [0 0 0]);
stateOrient = quaternion([tgtState(4) 0 0],'eulerd', 'ZYX','frame');
stateP.plotTrack(statePos', stateDims, stateOrient);