classdef BirdsEyePlot < matlab.System
    % Untitled Add summary here
    %
    % This template includes the minimum set of functions required
    % to define a System object with discrete state.
    
    % Public, tunable properties
    properties
        
    end
    
    properties(Nontunable)
        
    end
    
    properties(DiscreteState)
        
    end
    
    % Pre-computed constants
    properties(Access = private)
        BEP
        worldCoordEgoVehicle
        scenario
        ScenarioObj
        olPlotter
    end
    
    methods(Access = protected)
        function setupImpl(obj)
            obj.ScenarioObj = evalin("base","Scenario");
            obj.worldCoordEgoVehicle = obj.ScenarioObj.Actors(1);
            
            sensors = cell(2,1);
            
            for m = 1:numel(sensors)
                sensors{m}.SensorLocation = [2.1, 0];
                sensors{m}.SensorIndex = m;
                sensors{m}.Yaw = 0;
            end
            
            % Radar Sensor
            sensors{1}.Height = 0.2;
            sensors{1}.MaxRange = 100;
            sensors{1}.FieldOfView = [90, 0];
            
            % Vision Sensor
            sensors{2}.Height = 1.1;
            sensors{2}.MaxRange = 150;
            sensors{2}.FieldOfView = [38, 0];
            
            % Create the display and return a handle to the bird's-eye plot
            obj.BEP = createDemoDisplay(obj.worldCoordEgoVehicle, sensors);
            
            function BEP = createDemoDisplay(egoCar, sensors)
                % Make a figure
                hFigure = figure('Position', [0, 0, 1200, 640], 'Name', 'Sensor Fusion with Synthetic Data Example');
                movegui(hFigure, [0 -1]); % Moves the figure to the left and a little down from the top
                
                % Add a car plot that follows the ego vehicle from behind
                hCarViewPanel = uipanel(hFigure, 'Position', [0 0 0.5 0.5], 'Title', 'Chase Camera View');
                hCarPlot = axes(hCarViewPanel);
                chasePlot(egoCar, 'Parent', hCarPlot);
                
                % Add a car plot that follows the ego vehicle from a top view
                hTopViewPanel = uipanel(hFigure, 'Position', [0 0.5 0.5 0.5], 'Title', 'Top View');
                hCarPlot = axes(hTopViewPanel);
                chasePlot(egoCar, 'Parent', hCarPlot, 'ViewHeight', 130, 'ViewLocation', [0 0], 'ViewPitch', 90);
                
                % Add a panel for a bird's-eye plot
                hBEVPanel = uipanel(hFigure, 'Position', [0.5 0 0.5 1], 'Title', 'Bird''s-Eye Plot');
                
                % Create bird's-eye plot for the ego vehicle and sensor coverage
                hBEVPlot = axes(hBEVPanel);
                frontBackLim = 60;
                BEP = birdsEyePlot('Parent', hBEVPlot, 'Xlimits', [-frontBackLim frontBackLim], 'Ylimits', [-35 35]);
                
                % Plot the coverage areas for radar
                cap = coverageAreaPlotter(BEP,'FaceColor','red','EdgeColor','red');
                plotCoverageArea(cap, sensors{1}.SensorLocation,...
                    sensors{1}.MaxRange, sensors{1}.Yaw, sensors{1}.FieldOfView(1));
                
                % Plot the coverage areas for vision sensor
                cap = coverageAreaPlotter(BEP,'FaceColor','blue','EdgeColor','blue');
                plotCoverageArea(cap, sensors{2}.SensorLocation,...
                    sensors{2}.MaxRange, sensors{2}.Yaw, sensors{2}.FieldOfView(1));
                
                % Create a vision detection plotter put it in a struct for future use
                detectionPlotter(BEP, 'DisplayName','vision', 'MarkerEdgeColor','blue', 'Marker','^');
                
                % Combine all radar detections into one entry and store it for later update
                detectionPlotter(BEP, 'DisplayName','radar', 'MarkerEdgeColor','red');
                
                % Add road borders to plot
                laneMarkingPlotter(BEP, 'DisplayName','lane markings');
                
                % Add the tracks to the bird's-eye plot. Show last 10 track updates.
                trackPlotter(BEP, 'DisplayName','track', 'HistoryDepth',10);
                
                axis(BEP.Parent, 'equal');
                xlim(BEP.Parent, [-frontBackLim frontBackLim]);
                ylim(BEP.Parent, [-40 40]);
                
                % Add an outline plotter for ground truth
                obj.olPlotter = outlinePlotter(BEP, 'Tag', 'Ground truth');
            end
        end
        
        function stepImpl(obj, fusedTracks, rdrDetections, visionDetections, Actors, glbCoordEgoVehicle)
%             while advance(obj.ScenarioObj) && ishghandle(obj.BEP.Parent)                
                % use 2D selectors since plot does not support 3D
                positionSelector = [1 0 0 0 0 0; 0 0 1 0 0 0]; % Position selector
                velocitySelector = [0 1 0 0 0 0; 0 0 0 1 0 0]; % Velocity selector

                radarDets = rdrDetections.Detections(1:rdrDetections.NumDetections);
                cameraDets = visionDetections.Detections(1:visionDetections.NumDetections);
                combinedDetections = [radarDets;cameraDets];          

                validTracks = fusedTracks.Tracks(1:fusedTracks.NumTracks);

                updateBEP(obj, obj.BEP, obj.worldCoordEgoVehicle, combinedDetections, validTracks, positionSelector, velocitySelector, Actors, glbCoordEgoVehicle);
%             end
       end
        
        function resetImpl(obj)
            % Initialize / reset discrete-state properties
        end
    end
    methods
        function overrideActorInfo(obj,Actor,ID)
            % set ego pose
            obj.ScenarioObj.Actors(ID).Position = Actor.Position;
            obj.ScenarioObj.Actors(ID).Velocity = Actor.Velocity;
            obj.ScenarioObj.Actors(ID).Roll = Actor.Roll;
            obj.ScenarioObj.Actors(ID).Pitch = Actor.Pitch;
            obj.ScenarioObj.Actors(ID).Yaw = Actor.Yaw;
            obj.ScenarioObj.Actors(ID).AngularVelocity = Actor.AngularVelocity;
        end
        
        function updateBEP(obj, BEP, worldCoordEgoCar, detections, confirmedTracks, psel, vsel, Actors, glbCoordEgoCar)
            % Update road boundaries and their display
            [lmv, lmf] = laneMarkingVertices(worldCoordEgoCar);
            plotLaneMarking(findPlotter(BEP,'DisplayName','lane markings'),lmv,lmf);
            
            % update ground truth data
            [position, yaw, length, width, originOffset, color] = targetOutlines(worldCoordEgoCar);
            plotOutline(findPlotter(BEP,'Tag','Ground truth'), position, yaw, length, width, 'OriginOffset', originOffset, 'Color', color);
            
            % Prepare and update detections display
            N = numel(detections);
            detPos = zeros(N,2);
            isRadar = true(N,1);
            for i = 1:N
                detPos(i,:) = detections(i).Measurement(1:2)';
                if detections(i).SensorIndex > 1 % Vision detections
                    isRadar(i) = false;
                end
            end
            plotDetection(findPlotter(BEP,'DisplayName','vision'), detPos(~isRadar,:));
            plotDetection(findPlotter(BEP,'DisplayName','radar'), detPos(isRadar,:));
            
            % Prepare and update tracks display
            trackIDs = {confirmedTracks.TrackID};
            labels = cellfun(@num2str, trackIDs, 'UniformOutput', false);
            [tracksPos, tracksCov] = getTrackPositions(confirmedTracks, psel);
            tracksVel = getTrackVelocities(confirmedTracks, vsel);
            plotTrack(findPlotter(BEP,'DisplayName','track'), tracksPos, tracksVel, tracksCov, labels);
            
            [actorPositions,actorYaw,actorLength,actorWidth,actorOriginOffset,actorColor] = targetOutlines(worldCoordEgoCar);
            
            plotOutline(obj.olPlotter, actorPositions, actorYaw, actorLength, actorWidth,  ...
                'OriginOffset', actorOriginOffset,  'Color', actorColor);
            
            overrideActorInfo(obj, glbCoordEgoCar, 1);
            
            for i=uint16(1):Actors.NumActors
                % ActorID that are above 10000 belongs to barriers so no
                % need to override those 
                if(Actors.Actors(i).ActorID < 10000)
                    overrideActorInfo(obj,Actors.Actors(i),Actors.Actors(i).ActorID)
                end
            end
            updatePlots(obj.ScenarioObj);
        end      
        
    end
end

