classdef HelperVisualization < matlab.System
    % HelperVisualization Creates and updates the display
    %
    % This is a helper block for example purposes and may be removed or
    % modified in the future.
    %
    % Copyright 2021 The MathWorks, Inc.

    %#codegen
    properties(Nontunable)
        %Number of Sensors
        NumSensors = 6;
    end

    properties (Access = protected)
        pIsLegendOn
        pFig
        pBEP
        pFrontView
        pGridView
        pSensorViews
        pBlockName
        pSimulinkUIToolbar
        pScenario
        pEgoVehicle
        pLidars
        pSensorData
        pGridTracks
        pVisual
    end

    methods
        function obj = HelperVisualization(varargin)
            % Constructor
            setProperties(obj,nargin,varargin{:});
        end
    end

    methods(Access=protected)
        function setupImpl(obj,varargin)
            rng(2020);
            obj.pSensorData = get_param([bdroot,'/Scenario and Sensor Simulation/HelperConcatenateSensorData'],'RuntimeObject');
            obj.pGridTracks = get_param([bdroot,'/Grid-Based Multi Object Tracker'],'RuntimeObject');
            obj.pEgoVehicle = get_param([bdroot,'/Scenario and Sensor Simulation/Scenario Reader'],'RuntimeObject');
            obj.pBlockName = [gcs,'/','Visualization'];
            hfig = figure('Name',obj.pBlockName,'Tag','Visualization', ...
                'CloseRequestFcn',@helperCloseReq,'Visible','off');
            obj.pFig = hfig;
            obj.pFig.Position = [40 90 1250 600];
            obj.pScenario = evalin('base', 'scenario');
            setuplidar(obj);
            if ~coder.internal.is_defined(obj.pFrontView)
                obj.pFrontView = createFrontView(obj,obj.pFig);
                obj.pSensorViews = createSensorViews(obj,obj.pFig,obj.pLidars);
                obj.pGridView = createGridView(obj,obj.pFig);
                updatePlotColors(obj, obj.pScenario);
            end
%             obj.pVisual = VideoWriter('GridBasedMultiLidarTrackingResults','MPEG-4');
%             obj.pVisual.FrameRate = 18;
%             open(obj.pVisual);
        end

        function setuplidar(obj)
            dialogPrms = get_param([bdroot,'/Scenario and Sensor Simulation/Lidar Point Cloud Generator1'], 'DialogParameters');
            dialogPrmNames = fieldnames(dialogPrms);
            profiles = actorProfiles(obj.pScenario);
            dialogPrmNames([10,12,13]) = [];
            s = cell(numel(dialogPrmNames)*2,1);
            for num = 1:obj.NumSensors
                count = 1;
                for idx = 1:2:(numel(dialogPrmNames)*2)
                    if strcmpi(dialogPrmNames{count},'DetectionCoordinates')
                        s{idx} = dialogPrmNames{count};
                        s{idx+1} = get_param([bdroot,'/Scenario and Sensor Simulation/Lidar Point Cloud Generator',num2str(num)], dialogPrmNames{count});
                        count = count+1;
                    elseif any(strcmpi(dialogPrmNames{count},{'HasRoadsInputPort','HasEgoVehicle','HasNoise'}))
                        s{idx} = dialogPrmNames{count};
                        logicVal = get_param([bdroot,'/Scenario and Sensor Simulation/Lidar Point Cloud Generator',num2str(num)], dialogPrmNames{count});
                        if strcmpi(logicVal,'on')
                            s{idx+1} = true;
                        else
                            s{idx+1} = false;
                        end
                        count = count+1;
                    elseif any(strcmpi(dialogPrmNames{count},{'SensorLocation', ...
                            'AzimuthLimits','ElevationLimits'}))
                        s{idx} = dialogPrmNames{count};
                        s{idx+1} = [str2num(get_param([bdroot,'/Scenario and Sensor Simulation/Lidar Point Cloud Generator',num2str(num)], dialogPrmNames{count}))]; %#ok<NBRAK,ST2NM>
                        count = count+1;
                    else
                        s{idx} = dialogPrmNames{count};
                        s{idx+1} = str2double(get_param([bdroot,'/Scenario and Sensor Simulation/Lidar Point Cloud Generator',num2str(num)], dialogPrmNames{count}));
                        count = count+1;
                    end
                end
                obj.pLidars{num} = lidarPointCloudGenerator(s{:}, ...
                    'ActorProfiles',profiles);
                s = cell(numel(dialogPrmNames)*2,1);
            end
        end

        function gridView = createGridView(obj,fig)
            p4 = uipanel(fig,'Units','normalized','Position',[0.5 0 0.5 1],...
                'Title','Grid-based Tracker');

            snapInfo = struct('TrackID',1,...
                'SnapTimes',1:0.5:5,...
                'Snapshots',{{}});

            snapInfo(2) = snapInfo(1);
            snapInfo(2).TrackID = 2;
            snapInfo(2).SnapTimes = 6:0.5:10;

            snapInfo(3) = snapInfo(1);
            snapInfo(3).TrackID = 4;
            snapInfo(3).SnapTimes = 3:0.5:7;

            snapInfo(4) = snapInfo(1);
            snapInfo(4).TrackID = 7;
            snapInfo(4).SnapTimes = 10:0.5:14;

            snapInfo(5) = snapInfo(1);
            snapInfo(5).TrackID = 8;
            snapInfo(5).SnapTimes = 8:0.5:12;

            gridView = helperGridTrackingDisplayPanelSimulink('Parent',p4,...
                'ViewLocation',[0 0 160],...
                'ViewAngles',[0 90 0],...
                'PlotTruth',false,...
                'PlotRoads',false,...
                'PlotCoverage',false,...
                'DarkMode',true,...
                'PanWithEgo',false,...
                'PlotPointClouds',false,...
                'PlotCoverage',true,...
                'ReferenceEgo',true);

            gridView.SnapInformation = snapInfo;
        end

        function frontView = createFrontView(obj, fig)
            p1 = uipanel(fig,'Units','normalized','Position',[0 0.5 0.5 0.5],...
                'Title','Ground truth - Front View');

            frontView = helperGridTrackingDisplayPanelSimulink('Parent',p1,...
                'ViewLocation',[-11.75 0 2.1],...
                'ViewAngles',[0 0 0],...
                'PlotCoverage',false,...
                'PlotPointClouds',false,...
                'PlotTracks',false,...
                'ShowLegend',false);
        end

        function updatePlotColors(obj, scenario)
            for i = 2:numel(scenario.Actors)
                a = scenario.Actors(i);
                v = a.Velocity;
                if norm(v) <= 0
                    rgb = 0.4*[1 1 1];
                    a.PlotColor(:) = rgb;
                end
            end
        end

        function sensorViews = createSensorViews(obj, fig, lidars)
            order = [3 2 4 1 6 5];
            lidars = lidars([3 2 4 1 6 5]);
            str = ["Left Rear Lidar","Left Front Lidar","Rear Lidar","Front Lidar","Right Rear Lidar","Right Front Lidar"];
            xpositions = linspace(0,0.5,4);
            ypositions = linspace(0,0.5,3);
            dx = diff(xpositions);
            dy = diff(ypositions);
            dx = dx(1);
            dy = dy(1);
            idx = 1;
            for i = 1:3
                for k = 1:2
                    p = uipanel(fig,'Units','normalized','Position',[xpositions(i) ypositions(k) dx dy],...
                        'Title',str(idx));
                    sensorToPlot = false(numel(lidars),1);
                    sensorToPlot(order(idx)) = true;
                    loc = [lidars{idx}.SensorLocation(:);lidars{idx}.Height];
                    angles = [lidars{idx}.Yaw lidars{idx}.Pitch lidars{idx}.Roll];
                    move = [-11.75 0 2.1];
                    q = quaternion(angles,'eulerd','ZYX','frame');
                    r = rotatepoint(q,move);
                    loc = loc + r(:);

                    sensorViews{idx} =  helperGridTrackingDisplayPanelSimulink('Parent',p,...
                        'ViewLocation',loc,...
                        'ViewAngles',angles,...
                        'PlotCoverage',false,...
                        'PlotTruth',false,...
                        'PlotRoads',false,...
                        'PlotPointClouds',true,...
                        'PlotTracks',false,...
                        'ShowLegend',false,...
                        'DarkMode',true,...
                        'SensorToPlot',sensorToPlot);
                    idx = idx + 1;
                end
            end
        end

        function resetImpl(obj)
            % Bring the figure to front, set it to visible, but prevent
            % others from plotting to it
            isDirty = get_param(bdroot,'Dirty');
            set_param(obj.pBlockName,'UserData',obj.pFig)
            set_param(obj.pBlockName,'ModelCloseFcn','helperCloseAll(gcb)');
            set_param(bdroot,'Dirty',isDirty);
        end

        function releaseImpl(obj)
            % Release resources, such as file handles
            if ~isempty(obj.pFig.UserData)
                modelName = bdroot;
                isFastRestart = strcmp(get_param(modelName,'FastRestart'),'on');
                if isFastRestart %In fast restart mode, SimulationStatus is still 'running' at the end
                    setStoppedIcon(obj.pFig.UserData.SimulinkUIToolbar);
                else
                    update(obj.pFig.UserData.SimulinkUIToolbar);
                end
            end
            dirtyFlag = get_param(bdroot,'Dirty');
            set_param(obj.pBlockName,'OpenFcn','');
            set_param(bdroot,'Dirty',dirtyFlag);
            restart(obj.pScenario);
%             close(obj.pVisual);
        end

        function s = saveObjectImpl(obj)
            s = saveObjectImpl@matlab.System(obj);

            if isLocked(obj)
                s.pFig = obj.pFig;
                s.pBEP = obj.pBEP;
                s.pPlotter              = obj.pPlotter;
                s.pIsLegendOn           = obj.pIsLegendOn;
                s.pSimulinkUIToolbar    = saveobj(obj.pSimulinkUIToolbar);
            end
        end

        function loadObjectImpl(obj,s,wasLocked)
            if wasLocked
                obj.pFig = s.pFig;
                obj.pBEP = s.pBEP;
                obj.pPlotter                = s.pPlotter;
                obj.pSimulinkUIToolbar      = loadobj(s.pSimulinkUIToolbar);
                s = rmfield(s,{'pFig','pBEP', ...
                    'pPlotter','pSimulinkUIToolbar'});
            end
            loadObjectImpl@matlab.System(obj,s,wasLocked);
        end

        function stepImpl(obj)
            SensorDataInput = obj.pSensorData.OutputPort(1).Data.SensorData(1:obj.pSensorData.OutputPort(1).Data.NumSensors);
            tracks = obj.pGridTracks.OutputPort(1).Data.Tracks(1:obj.pGridTracks.OutputPort(1).Data.NumTracks) ;
            ptCloud = cell(1,obj.NumSensors);
            for i = 1:obj.NumSensors
                ptCloud{i}.Location = SensorDataInput(i).Measurement;
            end
            % Poses of objects with respect to ego vehicle
            tgtPoses = obj.pEgoVehicle.OutputPort(1).Data.Actors(1:obj.pEgoVehicle.OutputPort(1).Data.NumActors);
            egoVehicle = obj.pScenario.Actors(1);
            time = getCurrentTime(obj);
            % Simulate point cloud from each sensor
            for i = 1:numel(obj.pLidars)
                ptClouds1{i} = step(obj.pLidars{i},tgtPoses,time);
            end
            obj.pFrontView(obj.pScenario, egoVehicle, ...
                obj.pLidars,ptCloud,ptCloud, tracks);
            obj.pGridView(obj.pScenario, egoVehicle, ...
                obj.pLidars,ptCloud,ptCloud, tracks);
            for i = 1:numel(obj.pSensorViews)
                this = obj.pSensorViews{i};
                this(obj.pScenario, egoVehicle, ...
                    obj.pLidars,ptClouds1,ptCloud, tracks);
            end
            % Advance the scenario one time step
            axes(obj.pGridView.Axes);
            set(gca, 'Ylim', [-60, 60],'Xlim',[-60, 60], 'Zlim', [-20, 20]);
            advance(obj.pScenario);
%             frame = getframe(gcf);
%             writeVideo(obj.pVisual,frame);
        end
    end

    % Simulink interface
    methods(Access=protected)
        function str = getIconImpl(~)
            str = sprintf('Helper\nScenario-Output\nVisualization');
        end

        function num = getNumInputsImpl(~)
            num = 0;
        end
    end

    methods(Access = protected, Static)
        function header = getHeaderImpl
            % Define header panel for System block dialog
            header = matlab.system.display.Header(...
                'Title', 'Visualization',...
                'Text', getHeaderText());
        end

        function simMode = getSimulateUsingImpl
            % Return only allowed simulation mode in System block dialog
            simMode = 'Interpreted execution';
        end

        function flag = showSimulateUsingImpl
            % Return false if simulation mode hidden in System block dialog
            flag = false;
        end
    end
end

function str = getHeaderText
str = sprintf('Visualize grid-based target tracking output');
end