classdef helperLidarSensorConfigurations < matlabshared.tracking.internal.SimulinkBusUtilities

    % This is a helper block for example purposes and may be removed or
    % modified in the future.
    %
    % Copyright 2021 The MathWorks, Inc.

    properties(Constant, Access=protected)
        %pBusPrefix A string that captures the base output bus name
        %   An output bus name are created by the object. It will have the
        %   name given here, appended by the number of tracks.
        %   Additional sub buses are created as well, e.g., for the tracks.
        pBusPrefix = 'BusLidarSensor'
    end

    properties(Nontunable)
        %Number of Lidar Sensors
        NumSensors = 6;
        Frame = fusionCoordinateFrameType(1);
    end

    properties(Access = private)
        % Initial Lidars sensors configurations
        pLidars

        % Current Pose of ego vehicle
        pEgoVehicle
    end

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

    methods(Access=protected)
        function [out, argsToBus] = defaultOutput(~)
            stParam = repmat(struct('Frame',fusionCoordinateFrameType(1),...
                'OriginPosition',double(zeros(1,3)),...
                'Orientation',double(zeros(3,3)),...
                'IsParentToChild', false),1,2);
            out = struct('SensorTransformParameters',stParam,...
                'IsValidTime',false,...
                'SensorIndex',double(1), ...
                'SensorLimits',[-45 45;0 100], ...
                'SensorResolution', [1;1]);
            argsToBus = {out};
        end

        % Currently only support simulink environment, so nothing to do
        % here
        function y = sendToBus(~,x,varargin)
            if isstruct(x)
                y = struct('Configurations',repmat(x,6,1),'NumConfigurations',6);
            else
                [newx,num] = conv2Struct(x);
                y = struct('Configurations',newx,'NumConfigurations',num);
            end
        end

        function [config] = stepImpl(obj)
            time = get_param(bdroot,'SimulationTime');
            % Poses of objects with respect to ego vehicle
            tgtPoses = obj.pEgoVehicle.OutputPort(1).Data.Actors(1:obj.pEgoVehicle.OutputPort(1).Data.NumActors);
            egoVehicle = obj.pEgoVehicle.OutputPort(2).Data;
            % Simulate point cloud from each sensor
            for i = 1:obj.NumSensors
                [ptClouds{i}, isValidTime] = step(obj.pLidars{i},tgtPoses,time); %#ok<*NASGU>
                sensorConfigs{i} = helperGetLidarConfig(obj.pLidars{i},egoVehicle);
            end
            config = sendToBus(obj,sensorConfigs);
        end

        function setupImpl(obj, varargin)
            obj.pEgoVehicle = get_param([bdroot,'/Scenario and Sensor Simulation/Scenario Reader'],'RuntimeObject');
            dialogPrms = get_param([bdroot,'/Scenario and Sensor Simulation/Lidar Point Cloud Generator1'], 'DialogParameters');
            dialogPrmNames = fieldnames(dialogPrms);
            inputData = evalin('base', 'scenario');
            profiles = actorProfiles(inputData);
            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 loadObjectImpl(obj,s,wasLocked)
            % Set public properties and states
            loadObjectImpl@matlab.System(obj,s,wasLocked);
        end

        function status = isEndOfData(obj)
            % Return true if end of data has been reached
            status = (obj.pCurrentIndex == obj.pMaxIndex);
        end

        function s = saveObjectImpl(obj)
            % Set properties in structure s to values in object obj

            % Set public properties and states
            s = saveObjectImpl@matlab.System(obj);
        end

        function varargout = getOutputDataTypeImpl(obj)
            varargout{1} = getOutputDataTypeImpl@matlabshared.tracking.internal.SimulinkBusUtilities(obj);
        end

        function str = getIconImpl(~)
            str = sprintf('HelperLidarSensor\nConfigurations');
        end

        function varargout = getOutputNamesImpl(~)
            varargout{1} = sprintf('Sensor\nConfigurations');
        end

        function varargout = getInputNamesImpl(~)
            varargout{1} = sprintf('In');
        end

        function [out1] = isOutputFixedSizeImpl(~)
            out1 = true;
        end

        function [out1] = getOutputSizeImpl(~)
            out1 = [1 1];
        end

        function [out1] = isOutputComplexImpl(~)
            out1 = false;
        end
    end
    methods(Static, Hidden)
        function flag = isAllowedInSystemBlock
            flag = true;
        end

    end
    methods(Static, Access=protected)
        function groups = getPropertyGroupsImpl
            pList = 'DataFile';
            pSection = matlab.system.display.Section('PropertyList',{pList});

            slBusSection = getPropertyGroupsImpl@matlabshared.tracking.internal.SimulinkBusUtilities;

            groups = [pSection, slBusSection];
        end

        function header = getHeaderImpl
            % Define header panel for System block dialog
            header = matlab.system.display.Header(...
                'Title', 'helperLidarSensorConfigurations', ...
                '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('This block generates the real time sensor configurations for the lidar sensors');
end

function [yDet,num] = conv2Struct(det)
stParam = repmat(struct('Frame',fusionCoordinateFrameType(1),...
    'OriginPosition',double(zeros(1,3)),...
    'Orientation',double(zeros(3,3)),...
    'IsParentToChild', false),1,2);
out = struct('SensorTransformParameters',stParam,...
    'IsValidTime',false,...
    'SensorIndex',double(1), ...
    'SensorLimits',eye(2), ...
    'SensorResolution', [1;1]);
yDet = repmat(out,6,1);

num = numel(det);
for i =1:num
    for index = 1:2
        measPram(index) = struct('Frame',det{i}.SensorTransformParameters(index).Frame,...
            'OriginPosition',det{i}.SensorTransformParameters(index).OriginPosition',...
            'Orientation', det{i}.SensorTransformParameters(index).Orientation,...
            'IsParentToChild', det{i}.SensorTransformParameters(index).IsParentToChild);
    end
    y(i) = struct('SensorTransformParameters',measPram,...
        'IsValidTime',det{i}.IsValidTime,...
        'SensorIndex',det{i}.SensorIndex, ...
        'SensorLimits', det{i}.SensorLimits, ...
        'SensorResolution', det{i}.SensorResolution);
    yDet(i) = y(i);
end
end

function config = helperGetLidarConfig(lidar, ego)
% Define transformation from sensor to ego
senToEgo = struct('Frame',fusionCoordinateFrameType(1),...
    'OriginPosition',[lidar.SensorLocation(:);lidar.Height],...
    'Orientation',rotmat(quaternion([lidar.Yaw lidar.Pitch lidar.Roll],'eulerd','ZYX','frame'),'frame'),...
    'IsParentToChild',true);

% Define transformation from ego to tracking coordinates
egoToScenario = struct('Frame',fusionCoordinateFrameType(1),...
    'OriginPosition',ego.Position(:),...
    'Orientation',rotmat(quaternion([ego.Yaw ego.Pitch ego.Roll],'eulerd','ZYX','frame'),'frame'),...
    'IsParentToChild',true);

% Assemble using trackingSensorConfiguration.
config = trackingSensorConfiguration(...
    'SensorIndex',lidar.SensorIndex,...
    'IsValidTime', true,...
    'SensorLimits',[lidar.AzimuthLimits;0 lidar.MaxRange],...
    'SensorTransformParameters',[senToEgo;egoToScenario],...
    'DetectionProbability',0.95);
end
