classdef helperPreProcessDetections < matlabshared.tracking.internal.SimulinkBusUtilities
    % helperPreProcessDetections
    %

    properties(Nontunable)
        % sensors Sensor properties
        sensors = struct

        % sensorConfigs Sensor configurations
        sensorConfigs = struct
    end

    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 = {'BusPreProcessedDets', 'BusSensorConfig'};
    end
    properties(Nontunable)
        %BusName2Source Source of config bus name
        BusName2Source = 'Auto'
        %BusName2 Specify config bus name
        BusName2 = char.empty(1,0)
    end
    properties(Hidden,Constant)
        BusName2SourceSet = matlab.system.StringSet({'Auto','Property'});
    end

    properties( Access=protected)
        sampleStruct
        numMeasSize
        IsValidTime = false;
        NumMaxConfig = 8;
    end

    methods(Access = protected)
        function [out, argsToBus] = defaultOutput(obj,busIdx,varargin)

            if isequal(busIdx,1)
                busName = propagatedInputBus(obj, 1);
                if isempty(busName)
                    return
                end
                Dets = matlabshared.tracking.internal.SimulinkBusUtilities.bus2struct(busName);

                st = Dets.Detections(1);
                obj.numMeasSize = 3;
                st.Measurement=double(zeros(obj.numMeasSize,1));
                st.MeasurementNoise=double(zeros(obj.numMeasSize,obj.numMeasSize));
                st.MeasurementParameters =  repmat(st.MeasurementParameters(1),2,1);
                out = st;
                obj.sampleStruct = st;
            else
                stParam = repmat(struct( ...
                    'Frame',drivingCoordinateFrameType.Invalid, ...
                    'HasRange', false ,...
                    'HasElevation', false,...
                    'HasVelocity', false,...
                    'OriginPosition',double(zeros(3,1)),...
                    'OriginVelocity',double(zeros(3,1)),...
                    'Orientation',double(zeros(3,3)), ...
                    'IsParentToChild',true),1,2);
                out = struct('SensorIndex',double(1),'IsValidTime',true,...
                    'SensorTransformParameters',stParam);
            end

            argsToBus = {};
        end

        % Currently only support simulink environment, so nothing to do
        % here
        function y = sendToBus(obj,data,busIdx,varargin)
            if isequal(busIdx,1)
                sampleDet = obj.sampleStruct;
                y = struct('NumDetections',numel(data),'IsValidTime',obj.IsValidTime,...
                    'Detections',repmat(sampleDet,400,1));
                y.Detections(1:numel(data))=data;
            else
                if isstruct(data)
                    y = struct('Configurations',repmat(data,obj.NumMaxConfig,1),'NumConfigurations',obj.NumMaxConfig);
                else
                    defaultData = obj.defaultOutput(2);
                    [newx,num] = conv2Struct(data,defaultData,obj.NumMaxConfig);
                    y = struct('Configurations',newx,'NumConfigurations',num);
                end
            end
        end
        function [out1,out2] = stepImpl(obj,dets, egoVehicle)

            obj.IsValidTime = dets.IsValidTime;
            detections = dets.Detections(1:dets.NumDetections);
            preProcessedDets = repmat(obj.sampleStruct,dets.NumDetections,1);

            for i = 1:dets.NumDetections
                det = detections(i);
                isValidTime(det.SensorIndex) = true; %#ok<AGROW>
                preProcessedDets(i).Time= det.Time;
                preProcessedDets(i).SensorIndex= det.SensorIndex;
                preProcessedDets(i).ObjectClassID= det.ObjectClassID;
                preProcessedDets(i).ObjectAttributes= det.ObjectAttributes;
                if ismember(det.SensorIndex,[1,2,3,4,5,6])
                    preProcessedDets(i).Measurement = det.Measurement(1:obj.numMeasSize);
                    preProcessedDets(i).MeasurementParameters(1)= det.MeasurementParameters;
                    hasRangeRate = obj.sensors(det.SensorIndex).HasRangeRate;
                    if isequal(det.MeasurementParameters(1).Frame,drivingCoordinateFrameType.Spherical)
                        if hasRangeRate
                            preProcessedDets(i).MeasurementNoise(1:3,1:3) = diag((1/2*[obj.sensors(det.SensorIndex).AzimuthResolution obj.sensors(det.SensorIndex).RangeResolution obj.sensors(det.SensorIndex).RangeRateResolution]).^2);
                        else
                            preProcessedDets(i).MeasurementNoise(1:2,1:2) = diag(([obj.sensors(det.SensorIndex).AzimuthResolution obj.sensors(det.SensorIndex).RangeResolution]/2).^2);
                        end
                    else
                        % x y noise set to range resolution for Cartesian
                        % frame.
                        if hasRangeRate
                            preProcessedDets(i).MeasurementNoise = diag([obj.sensors(det.SensorIndex).RangeResolution obj.sensors(det.SensorIndex).RangeResolution 0.4 10 10 10].^2);
                        else
                            preProcessedDets(i).MeasurementNoise(1:3,1:3) = diag([obj.sensors(det.SensorIndex).RangeResolution obj.sensors(det.SensorIndex).RangeResolution 0.4].^2);
                        end
                    end
                end
                % The camera measurement noise is roughly set to a
                % reasonable value
                if ismember(det.SensorIndex,[7,8])
                    % Only raw position measurement is used. filtered
                    % velocity measurement is removed.
                    preProcessedDets(i).Measurement(1:3) = det.Measurement(1:3);
                    preProcessedDets(i).MeasurementNoise(1:3,1:3) = det.MeasurementNoise(1:3,1:3);
                    preProcessedDets(i).MeasurementNoise(1:3,1:3) = blkdiag(0.5*eye(2),0.4);
                    preProcessedDets(i).MeasurementParameters(1) = det.MeasurementParameters(1);
                    preProcessedDets(i).MeasurementParameters(1).HasVelocity = false;
                end

                % Add information about egoVehicle
                egoPos = egoVehicle.Position(:);
                egoVel = egoVehicle.Velocity(:);
                egoOrient = rotmat(quaternion([egoVehicle.Yaw egoVehicle.Pitch egoVehicle.Roll],'eulerd','ZYX','frame'),'frame');
                % The sensors do not move with respect to ego vehicle
                preProcessedDets(i).MeasurementParameters(1).OriginVelocity = zeros(3,1);
                % Parameters for scenario to ego transformation
                egoParams = preProcessedDets(i).MeasurementParameters(1);
                egoParams.Frame = drivingCoordinateFrameType.Rectangular;
                egoParams.OriginPosition = egoPos;
                egoParams.OriginVelocity = egoVel;
                egoParams.Orientation = egoOrient';
                preProcessedDets(i).MeasurementParameters = [preProcessedDets(i).MeasurementParameters(1);egoParams];
            end
            out1 = sendToBus(obj,preProcessedDets,1);

            % Part of the configuration that changes with time to update
            % with trackerPHD.
            transientConfigs= struct;
            for i = 1:numel(obj.sensorConfigs)
                SensorTransformParameters = obj.sensorConfigs(i).SensorTransformParameters;
                SensorTransformParameters(2).OriginPosition = egoPos;
                SensorTransformParameters(2).OriginVelocity = egoVel;
                SensorTransformParameters(2).Orientation = egoOrient;

                transientConfigs(i).SensorTransformParameters = SensorTransformParameters;
                transientConfigs(i).IsValidTime = isValidTime(i);
                transientConfigs(i).SensorIndex = i;
            end

            out2 = sendToBus(obj,{transientConfigs},2);
        end

        function [out1,out2] = getOutputDataTypeImpl(obj)
            [out1,out2] = getOutputDataTypeImpl@matlabshared.tracking.internal.SimulinkBusUtilities(obj);
        end

        function str = getIconImpl(~)
            str = sprintf('Helper\nPreprocess Detections');
        end

        function [name,name2] = getInputNamesImpl(~)
            % Return input port names for System block
            name = sprintf('Sensor\nDetections');
            name2 = sprintf('Ego\nVehicle');
        end

        function varargout = getOutputNamesImpl(~)
            varargout{1} = sprintf('Preprocessed\nDetections');
            varargout{2} = sprintf('Sensor\nConfig');
        end

        function [out1,out2] = isOutputFixedSizeImpl(~)
            out1 = true;
            out2 = true;

        end
        function [out1,out2] = getOutputSizeImpl(~)
            out1 = [1 1];
            out2 = [1 1];
        end
        function numOutput = getNumOutputsImpl(~)
            numOutput = 2;
        end
        function [out1,out2] = isOutputComplexImpl(~)
            out1 = false;
            out2 = false;
        end
    end
    methods(Static, Access=protected)
        function groups = getPropertyGroupsImpl
            pSection = matlab.system.display.Section('PropertyList',{'sensors','sensorConfigs'});
            slBusSection = getPropertyGroupsImpl@matlabshared.tracking.internal.SimulinkBusUtilities;
            busPropList = slBusSection.PropertyList;
            portsList = busPropList(:)';
            busSection = matlab.system.display.Section('Title','Bus Setting','PropertyList',portsList);
            busSection2 = matlab.system.display.Section('Title','','PropertyList',{'BusName2Source','BusName2'});
            groups = [pSection, busSection,busSection2];
        end
        function header = getHeaderImpl
            % Define header panel for System block dialog
            header = matlab.system.display.Header(...
                'Title', 'helperPreProcessDetections', ...
                'Text','A helper block to pre-process detections and genrate sensor configurations.' );
        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 [yDet,num] = conv2Struct(det,defaultData,NumMaxConfig)
yDet = repmat(defaultData,NumMaxConfig,1);
num = numel(det{1});
for i = 1:num
    for j = 1: numel(det{1}(i).SensorTransformParameters)
        yDet(i).SensorTransformParameters(j).Frame = drivingCoordinateFrameType(det{1}(i).SensorTransformParameters(j).Frame);
        yDet(i).SensorTransformParameters(j).IsParentToChild = det{1}(i).SensorTransformParameters(j).IsParentToChild;
        yDet(i).SensorTransformParameters(j).OriginPosition = det{1}(i).SensorTransformParameters(j).OriginPosition;
        yDet(i).SensorTransformParameters(j).OriginVelocity = det{1}(i).SensorTransformParameters(j).OriginVelocity;
        yDet(i).SensorTransformParameters(j).Orientation = det{1}(i).SensorTransformParameters(j).Orientation;
        yDet(i).SensorTransformParameters(j).HasElevation = det{1}(i).SensorTransformParameters(j).HasElevation;
        yDet(i).SensorTransformParameters(j).HasVelocity = det{1}(i).SensorTransformParameters(j).HasVelocity;
        yDet(i).SensorTransformParameters(j).HasRange = det{1}(i).SensorTransformParameters(j).HasRange;
    end
    yDet(i).IsValidTime = det{1}(i).IsValidTime;
    yDet(i).SensorIndex =  det{1}(i).SensorIndex;
end
end