classdef (StrictDefaults)Scanner < matlab.System
        
    %   This class is used to define scanner for scanning mounts
    %
    %   This class is for internal use only. It may be removed in the future.
    
    %   Copyright 2017-2019 The MathWorks, Inc.
    
    %#codegen
    
    % Combined properties
    properties(Nontunable)
        %UpdateRate Sensor update rate
        %   Rate at which sensor updates are generated, specified as a
        %   positive scalar. The reciprocal of the UpdateRate must be an
        %   integer multiple of the simulation time interval. Units are in
        %   hertz.
        %
        %   Default: 1
        UpdateRate = 1
    end
    
    properties
        %FieldOfView Sensor's angular field of view
        %   Specify the sensor's azimuthal and elevation fields of view as
        %   the 2-by-1 column vector, [azFOV; elFOV]. Field of view defines
        %   the total angular extent observed by the sensor during a sensor
        %   update in degrees and must lie in the interval (0,360] for
        %   azimuth and (0 180] for elevation.
        %
        %   Default: [1;20]
        FieldOfView = [1;20]
    end
    
    properties(Nontunable)
        %MountingLocation  Sensor's (x,y,z) mounted location on platform (m)
        %   A 1-by-3 vector defining the offset of the sensor's origin from
        %   the origin of its platform. The default value leaves the
        %   sensor's origin at the same location as the origin of its
        %   platform.
        %
        %   Default: [0 0 0]
        MountingLocation = [0 0 0]
        
        %MountingAngles  Mounting rotation angles of sensor mounted on platform (deg)
        %   Specify a 3-element vector corresponding to the intrinsic Euler
        %   angle rotations which align the body axes of the platform on
        %   which the sensor is mounted to the sensor's axes. The 3
        %   elements define the rotations around the z, y, and x axes
        %   respectively, in that order. The first rotation, rotates the
        %   body axes around the z-axis. Because these angles define
        %   intrinsic rotations, the second rotation is performed around
        %   the y-axis in its new position resulting from the previous
        %   rotation. The final rotation around the x-axis is performed
        %   around the x-axis as rotated by the first two rotations in the
        %   intrinsic system. These 3 Euler angles are sometimes referred to
        %   as yaw, pitch, and roll, however in this case, these rotations
        %   are relative to the platform's body frame and do not refer to
        %   the scenario frame. The angles are in degrees.
        %
        %   Default: [0 0 0]
        MountingAngles = [0 0 0]
    
        %HasMechanicalScan
        %
        %   Default: true
        HasMechanicalScan (1, 1) logical = true
        
        %HasElectronicScan
        %
        %   Default: true
        HasElectronicScan (1, 1) logical = true
    end
    
    properties(Dependent)
        MountingOrientation
        LookOrientation
    end
    
    properties(Dependent,SetAccess=private)
        CurrentMechanicalAngle
        CommandMechanicalAngle
        LookAngle
        ScanLimits
    end

    % Electronic properties
    properties
        %ElectronicScanLimits
        %
        %   Default: [-45 45;-20 20]
        ElectronicScanLimits = [-45 45;-20 20]
    end
    
    properties(Dependent,SetAccess=private)
        ElectronicAngle
    end
    
    % Mechanical properties
    properties
        %MechanicalScanLimits
        %
        %   Default: [-15 15;-5 5]
        MechanicalScanLimits = [-15 15;-5 5]
        
        %MaxMechanicalScanRate
        %
        %   Default: [45;0]
        MaxMechanicalScanRate = [45;0]
    end
    
    properties(Dependent,SetAccess=private)
        CurrentMechanicalScanRate
    end

    properties(Nontunable,Access=private)
        % Cached properties
        pUpdateRate = 1
        pFieldOfView = [1;20]
        pMountingLocation = [0 0 0]
        pMountOrient = eye(3)
    end
    
    properties(Access=private)
        pIsFirstScan = true
        pIsScanDone = false
        pHasFirstDwell = false
        pIs360 = false
        
        pLastTime = NaN
        pLastDwellTime = NaN

        pScanLimits = [-45 45;-20 20]
        
        pElectronicScanLimits = [-45 45;-20 20]

        pElecScanGridLookAzs = []
        pElecScanGridLookEls = []
        pElecScanGridLookAges = []
        pElecScanLookAz = NaN
        pElecScanLookEl = NaN

        pMechScanNum = uint32(0)
        pNumRaster = uint32(0)
        pNumAng = uint32(0)
        
        pMechanicalScanLimits = [-15 15;-5 5]
        pMaxMechanicalScanRate = [45;0]
        
        pCurrentMechanicalAngle = [0;0]
        pMechScan1GridAngs = []
        pMechScan2GridAngs = []
        pMechanicalGridIdx = uint32(1)
        pMechanicalScanForward = true
        pCurrentMechanicalScanRate = [45;0]
    end
    
    properties(Constant, Access=private)
        SmallNumber = 1e-6
    end

    % Constructor
    methods
        function obj = Scanner(varargin)
            setProperties(obj,numel(varargin),varargin{:});
        end
    end
    
    % ---------------
    % Setters/getters
    % ---------------
    
    % Combined
    methods
        function val = get.MountingOrientation(obj)
            if isLocked(obj)
                val = obj.pMountOrient;
            else
                val = fusion.internal.frames.ypr2rotmat(obj.MountingAngles,'degrees');
            end
        end
        
        function val = get.LookOrientation(obj)
            mountOrient = obj.MountingOrientation;
            lookAng = obj.LookAngle;
            
            ypr = zeros(1,3,'like',lookAng);
            ypr(1:2) = lookAng;
            ypr(2) = -ypr(2); % Elevation is opposite the sign of pitch
            lookRot = fusion.internal.frames.ypr2rotmat(ypr,'degrees');
            val = lookRot*mountOrient;
        end
        
        function val = get.CurrentMechanicalAngle(obj)
            % Current azimuth and elevation of sensor mount relative to it's mounted orientation
            if isLocked(obj) 
                if hasMechScan(obj)
                    val = obj.pCurrentMechanicalAngle;
                else
                    val = zeros(2,1);
                end
            else
                val = zeros(2,1);
            end
        end
        
        function val = get.CommandMechanicalAngle(obj)
            % Command azimuth and elevation of sensor mount relative to it's mounted orientation
            
            % Scan numbers start at 0 and then iterate between 1 and
            % 2. 0 is the initial scan, 1 is the even scans and 2 is
            % odd scans
            if isLocked(obj)
                if hasMechScan(obj)
                    numRaster = size(obj.pMechScan1GridAngs,3);
                    if (numRaster==1 && obj.pIs360) || obj.pMechScanNum==0 || obj.pMechScanNum==2
                        val = obj.pMechScan1GridAngs(:,obj.pMechanicalGridIdx);
                    else
                        val = obj.pMechScan2GridAngs(:,obj.pMechanicalGridIdx);
                    end
                else
                    val = zeros(2,1);
                end
            else
                val = zeros(2,1);
            end
        end
        
        function val = get.LookAngle(obj)
            val = obj.CurrentMechanicalAngle;
            if hasElecScan(obj)
                val = val+obj.ElectronicAngle;
            end
            val = fusion.internal.units.interval(val,[-180 180]);
        end
        
        function val = get.ScanLimits(obj)
            val = scanLimitsImpl(obj);
        end
    end
    
    % Electronic properties
    methods
        function val = get.ElectronicAngle(obj)
            if isLocked(obj)
                if hasElecScan(obj)
                    val = [obj.pElecScanLookAz;obj.pElecScanLookEl]-obj.CurrentMechanicalAngle;
                    val = fusion.internal.units.interval(val,[-180 180]);
                else
                    val = zeros(2,1);
                end
            else
                val = zeros(2,1);
            end
        end
    end
    
    % Mechanical properties
    methods
        function val = get.CurrentMechanicalScanRate(obj)
            if isLocked(obj)
                if hasMechScan(obj)
                    val = obj.pCurrentMechanicalScanRate;
                else
                    val = zeros(2,1);
                end
            else
                val = zeros(2,1);
            end
        end
    end
    
    % Implementation
    methods
        function flag = isScanDone(obj)
            % Returns true when a scan is completed
            if ~hasElecScan(obj) && ~hasMechScan(obj) % No scanning
                flag = true;
            else
                flag = obj.pIsScanDone;
            end
        end
        
        function [mechAng,elecAng,angVel] = scan(obj,time)
            % Wrapper around step for MountPoint.scan
            
            mechAng = zeros(1,2,'like',time);
            elecAng = zeros(1,2,'like',time);
            
            step(obj,time);
            
            mechAng(:) = obj.CurrentMechanicalAngle;
            elecAng(:) = obj.ElectronicAngle;
            
            % not implemented
            angVel = zeros(3,1,'like',time);
        end
    end
    
    methods(Access = protected)
        function lookAng = stepImpl(obj,time)
            % Implement algorithm. Calculate y as a function of input u and
            % discrete states.
            if obj.pIsFirstScan || isGreaterThanOrEqual(obj,time,obj.pLastTime)
                % Time is in the future
                
                if hasMechScan(obj)
                    tStep = timestep(obj,time);
                    mechanicalScan(obj,tStep);
                end
                
                if hasElecScan(obj)
                    electronicScan(obj,time);
                end
                
                obj.pIsFirstScan = false;
                obj.pLastTime = time;
                
                if ~obj.pHasFirstDwell
                    obj.pLastDwellTime = time;
                    obj.pHasFirstDwell = true;
                elseif isDwellDone(obj,time)
                    tDwell = 1/obj.UpdateRate;
                    obj.pLastDwellTime = obj.pLastDwellTime+tDwell;
                end
            end
            
            lookAng = obj.LookAngle;
        end
        
        function setupImpl(obj)
            % Perform one-time calculations, such as computing constants
            obj.pMountOrient = fusion.internal.frames.ypr2rotmat(obj.MountingAngles,'degrees');

            setupMechanicalScanGrid(obj);
            setupElectronicScanGrid(obj);
        end

        function resetImpl(obj)
            % Initialize / reset discrete-state properties
            obj.pIsFirstScan = true;
            obj.pHasFirstDwell = false;
            obj.pIsScanDone = false;
        end

        function loadObjectImpl(obj,s,wasLocked)
            % Set properties in object obj to values in structure s

            % Set private and protected properties
            if wasLocked
                obj.pMountOrient = s.pMountOrient;
                
                obj.pIsFirstScan = s.pIsFirstScan;
                obj.pIsScanDone = s.pIsScanDone;
                obj.pHasFirstDwell = s.pHasFirstDwell;
                obj.pIs360 = s.pIs360;
                
                obj.pLastTime = s.pLastTime;
                obj.pLastDwellTime = s.pLastDwellTime;
                
                obj.pElecScanGridLookAzs = s.pElecScanGridLookAzs;
                obj.pElecScanGridLookEls = s.pElecScanGridLookEls;
                obj.pElecScanGridLookAges = s.pElecScanGridLookAges;
                obj.pElecScanLookAz = s.pElecScanLookAz;
                obj.pElecScanLookEl = s.pElecScanLookEl;
                
                obj.pMechScanNum = s.pMechScanNum;
                
                obj.pCurrentMechanicalAngle = s.pCurrentMechanicalAngle;
                obj.pMechScan1GridAngs = s.pMechScan1GridAngs;
                obj.pMechScan2GridAngs = s.pMechScan2GridAngs;
                if isfield(s,'pNumRaster')
                    obj.pNumAng = s.pNumAng;
                    obj.pNumRaster = s.pNumRaster;
                else
                    % For backwards compatibility, early versions did not
                    % save this information
                    obj.pNumAng = size(obj.pMechScan1GridAngs,2);
                    obj.pNumRaster = size(obj.pMechScan1GridAngs,3);
                end
                obj.pMechanicalGridIdx = s.pMechanicalGridIdx;
                obj.pMechanicalScanForward = s.pMechanicalScanForward;
                obj.pCurrentMechanicalScanRate = s.pCurrentMechanicalScanRate;
            end
            
            % Set public properties and states
            loadObjectImpl@matlab.System(obj,s,wasLocked);
        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);

            % Set private and protected properties
            if isLocked(obj)
                s.pMountOrient = obj.pMountOrient;
                
                s.pIsFirstScan = obj.pIsFirstScan;
                s.pIsScanDone = obj.pIsScanDone;
                s.pHasFirstDwell = obj.pHasFirstDwell;
                s.pIs360 = obj.pIs360;
                
                s.pLastTime = obj.pLastTime;
                s.pLastDwellTime = obj.pLastDwellTime;
                
                s.pElecScanGridLookAzs = obj.pElecScanGridLookAzs;
                s.pElecScanGridLookEls = obj.pElecScanGridLookEls;
                s.pElecScanGridLookAges = obj.pElecScanGridLookAges;
                s.pElecScanLookAz = obj.pElecScanLookAz;
                s.pElecScanLookEl = obj.pElecScanLookEl;
                
                s.pMechScanNum = obj.pMechScanNum;
                
                s.pCurrentMechanicalAngle = obj.pCurrentMechanicalAngle;
                s.pMechScan1GridAngs = obj.pMechScan1GridAngs;
                s.pMechScan2GridAngs = obj.pMechScan2GridAngs;
                s.pNumRaster = obj.pNumRaster;
                s.pNumAng = obj.pNumAng;
                s.pMechanicalGridIdx = obj.pMechanicalGridIdx;
                s.pMechanicalScanForward = obj.pMechanicalScanForward;
                s.pCurrentMechanicalScanRate = obj.pCurrentMechanicalScanRate;
            end
        end
    end
    
    % Combined
    methods(Access=private)
        function flag = hasMechScan(obj)
            flag = obj.HasMechanicalScan && ...
                ~all(isSmall(obj,obj.MechanicalScanLimits(:))) && ...
                ~all(isSmall(obj,obj.MaxMechanicalScanRate));
        end
        
        function flag = hasElecScan(obj)
            flag = obj.HasElectronicScan && ...
                ~all(isSmall(obj,obj.ElectronicScanLimits(:)));
        end
        
        function limits = scanLimitsImpl(obj)
            % Returns the azimuth and elevation surveillance limits
            
            limits = zeros(2,2);
            if hasElecScan(obj)
                limits = limits+obj.ElectronicScanLimits;
            end
            if hasMechScan(obj)
                limits = limits+obj.MechanicalScanLimits;
            end
            
            is360 = isGreaterThanOrEqual(obj,diff(limits,1,2),360);
            limits = fusion.internal.units.interval(limits,[-180 180]);
            
            for m = 1:numel(is360)
                if is360(m)
                    limits(m,:) = [-180 180];
                end
            end
            
            % Bound limits
            limits(1,1) = max(min(limits(1,:)),-180);
            limits(1,2) = min(max(limits(1,:)),180);

            limits(2,1) = max(min(limits(2,:)),-90);
            limits(2,2) = min(max(limits(2,:)),90);
        end
        
        function [axis1,axis2] = scanAxes(obj)
            % Scan axes are opposite of the slew axes
            
            if hasElecScan(obj)
                [axis1,axis2] = electronicScanAxes(obj);
            else            
                [axis2,axis1] = slewAxes(obj);
            end
        end
        
        function dt = timestep(obj,time)
            % Returns the time step since the last scan of the array
            if obj.pHasFirstDwell
                dt = time-obj.pLastTime;
            else
                dt = zeros(1);
            end
        end
        
        function flag = isDwellDone(obj,time)
            % Returns true when the dwell is done
            
            if ~obj.pHasFirstDwell
                flag = true;
            else
                tDwell = 1/obj.UpdateRate;
                tElapsed = time-obj.pLastDwellTime;
                flag = isGreaterThanOrEqual(obj,tElapsed,tDwell);
            end
        end
    end
    
    % Electronic
    methods(Access=private)
        function electronicScan(obj,time)
            dt = timestep(obj,time);
            
            % Age dwell positions
            obj.pElecScanGridLookAges = obj.pElecScanGridLookAges+dt;
            iAz = obj.pElecScanGridLookAzs==obj.pElecScanLookAz;
            iEl = obj.pElecScanGridLookEls==obj.pElecScanLookEl;
            obj.pElecScanGridLookAges(iEl,iAz) = zeros(1);
            
            if isDwellDone(obj,time)
                
                % Find look angles that can be reached by electronic scan
                % from current sensor orientation
                escanLim = obj.ElectronicScanLimits;
                
                % For mechanically scanning sensors, look angles are
                % relative to the platform, for electronically scanning
                % arrays, look angles are the electronic scan angles
                mechAng = obj.CurrentMechanicalAngle;
                
                % Which dimensions have electronic scan?
                dAng = diff(escanLim,1,2);
                dAng = fusion.internal.units.interval(dAng,[-180 180]);
                hasEscan = ~isSmall(obj,dAng);
                
                % Find azimuth beam positions that can be reached from
                % current array orientation
                escanAzs = obj.pElecScanGridLookAzs-mechAng(1);
                escanAzs = fusion.internal.units.interval(escanAzs,[-180 180]);
                
                if hasEscan(1)
                    iAz = find(isGreaterThanOrEqual(obj,escanAzs,escanLim(1,1)) & isGreaterThanOrEqual(obj,escanLim(1,2),escanAzs));
                else
                    span = obj.FieldOfView(1);
                    iAz = find(isGreaterThanOrEqual(obj,escanAzs,-span/2) & isGreaterThanOrEqual(obj,span/2,escanAzs));
                end
                
                % Find elevation beam positions that can be reached from
                % current array orientation
                escanEls = obj.pElecScanGridLookEls-mechAng(2);
                escanEls = fusion.internal.units.interval(escanEls,[-180 180]);
                
                if hasEscan(2)
                    iEl = find(isGreaterThanOrEqual(obj,escanEls,escanLim(2,1)) & isGreaterThanOrEqual(obj,escanLim(2,2),escanEls));
                else
                    span = obj.FieldOfView(2);
                    iEl = find(isGreaterThanOrEqual(obj,escanEls,-span/2) & isGreaterThanOrEqual(obj,span/2,escanEls));
                end

                % Select scan angle with greatest age
                [iEl,iAz] = meshgrid(iEl,iAz);
                ages = obj.pElecScanGridLookAges;
                sz = size(ages);
                ind = sub2ind(sz,iEl(:),iAz(:));
                agesVal = ages(:);
                agesVal = agesVal(ind);

                % Select the scan angle with the greatest age and closest
                % to the origin of the raster scan pattern
                mAge = max(agesVal);
                iMin = agesVal==mAge;
                iElMin = iEl(iMin);
                iAzMin = iAz(iMin);
                
                if coder.target('MATLAB')
                    escanElMin = escanEls(iElMin);
                    escanAzMin = escanAzs(iAzMin);
                else
                    numVal = numel(iElMin);
                    escanElMin = NaN(numVal,1);
                    escanAzMin = NaN(numVal,1);
                    for m = 1:numVal
                        escanElMin(m) = escanEls(iElMin(m));
                        escanAzMin(m) = escanAzs(iAzMin(m));
                    end
                end
                
                % Primary axis
                axis1 = scanAxes(obj);
                if axis1==1
                    mVal = min(escanElMin);
                    iMin = escanElMin==mVal;
                    iEl = iElMin(iMin);
                    iAz = iAzMin(iMin);
                    escanElMin = escanElMin(iMin);
                    escanAzMin = escanAzMin(iMin);
                    [~,iMin] = min(escanAzMin);
                else
                    mVal = min(escanAzMin);
                    iMin = escanAzMin==mVal;
                    iEl = iElMin(iMin);
                    iAz = iAzMin(iMin);
                    escanElMin = escanElMin(iMin);
                    escanAzMin = escanAzMin(iMin);
                    [~,iMin] = min(escanElMin);
                end
                iEl = iEl(iMin);
                iAz = iAz(iMin);
            else
                % Stay at current scan position
                dAng = obj.pElecScanGridLookAzs-obj.pElecScanLookAz;
                dAng = fusion.internal.units.interval(dAng,[-180 180]);
                iAz = find(isSmall(obj,abs(dAng)),1);
                
                dAng = obj.pElecScanGridLookEls-obj.pElecScanLookEl;
                dAng = fusion.internal.units.interval(dAng,[-180 180]);
                iEl = find(isSmall(obj,abs(dAng)),1);
            end
            
            obj.pElecScanLookAz = obj.pElecScanGridLookAzs(iAz(1));
            obj.pElecScanLookEl = obj.pElecScanGridLookEls(iEl(1));
            obj.pIsScanDone = ~obj.pIsFirstScan && iEl(1)==1 && iAz(1)==1;
        end
        
        function setupElectronicScanGrid(obj)
            % Setup the look angles comprising the electronic scan grid
            scanLim = obj.ScanLimits;
            fov = obj.FieldOfView;
            eLim = obj.ElectronicScanLimits;
            eSpan = diff(eLim,1,2);
            eSpan = fusion.internal.units.interval(eSpan,[-180 180]);
            
            if isSmall(obj,eSpan(1))
                % No electronic scan. 
 
                if hasMechScan(obj)
                    % Look angles are the mechanical raster angles
                    az1 = obj.pMechScan1GridAngs(1,:);
                    az2 = obj.pMechScan2GridAngs(1,:);
                    lookAzs = unique([az1(:);az2(:)])';
                else
                    % Look angle is boresight
                    lookAzs = zeros(1);
                end
            else
                lookAzs = lookAngles(scanLim(1,:),fov(1));
            end
            
            if isSmall(obj,eSpan(2))
                % No electronic scan.
                
                if hasMechScan(obj)
                    % Look angles are the mechanical raster angles
                    el1 = obj.pMechScan1GridAngs(2,:);
                    el2 = obj.pMechScan2GridAngs(2,:);
                    lookEls = unique([el1(:);el2(:)])';
                else
                    % Look angle is boresight
                    lookEls = zeros(1);
                end
            else
                lookEls = lookAngles(scanLim(2,:),fov(2));
            end
            
            numAz = numel(lookAzs);
            numEl = numel(lookEls);
            ages = zeros(numEl,numAz);
            
            obj.pElecScanGridLookAzs = lookAzs;
            obj.pElecScanGridLookEls = lookEls;
            obj.pElecScanGridLookAges = ages;
            
            obj.pElecScanLookAz = lookAzs(1);
            obj.pElecScanLookEl = lookEls(1);
            obj.pIsScanDone = false;
        end
        
        function [axis1,axis2] = electronicScanAxes(obj)
            % Returns the primary and secondary scan axes
            numAz = numel(obj.pElecScanGridLookAzs);
            numEl = numel(obj.pElecScanGridLookEls);
            if numEl>numAz
                axis1 = 2;
                axis2 = 1;
            else
                axis1 = 1;
                axis2 = 2;
            end
        end
    end
    
    % Mechanical
    methods(Access=private)
        function mechanicalScan(obj,tStep)
            % Continue scanning until no time remains
            wasScanCompleted = false;
            while isGreaterThan(obj,tStep)
                numAng = obj.pNumAng;
                
                % Was last command completed?
                if isCommandDone(obj)
                    % Command angle has been reached. Find the next command angle
                    
                    % Is scan done?
                    numRaster = obj.pNumRaster;
                    if numRaster==2
                        % 2 raster bars is a special case where we
                        % continuously trace out a rectangle.
                        numGrid = numAng;
                    else
                        numRaster = max(numRaster-1,1);
                        numGrid = numAng*numRaster;
                    end
                    lastIdx = obj.pMechanicalGridIdx;
                    isScanPatternDone = lastIdx==numGrid;
                    [axis1, axis2] = slewAxes(obj);
                    nextBar = false;
                    
                    if isScanPatternDone
                        % Yes, start next scan pattern
                        obj.pMechScanNum = mod(obj.pMechScanNum,2)+1;
                        nextIdx = ones(1,'like',lastIdx);
                        if numRaster>1
                            scanAxis = axis2;
                            nextBar = true;
                        else
                            scanAxis = axis1;
                        end
                    else
                        % No, continue in current scan pattern
                        nextIdx = lastIdx+ones(1,'like',lastIdx);
                        if numRaster>1 && mod(nextIdx,numAng)==1
                            % Step to new raster bar
                            scanAxis = axis2;
                            nextBar = true;
                        else
                            % Stay on current raster bar
                            scanAxis = axis1;
                        end
                    end
                    
                    % Set next command angle
                    obj.pMechanicalGridIdx = nextIdx;
                    
                    % Update scan rate
                    obj.pCurrentMechanicalScanRate = mechanicalSlewRate(obj,scanAxis);
                    
                    % For 360 degree scan, the scan direction along the
                    % primary axis does not change. Allow simultaneous scan
                    % along primary and secondary axes when changing to
                    % next raster bar
                    if obj.pIs360 && nextBar
                        axis2 = mod(scanAxis,2)+1;
                        scanRate = mechanicalSlewRate(obj,axis2);
                        obj.pCurrentMechanicalScanRate(axis2) = scanRate(axis2);
                    end
                end
                
                % Move antenna to commanded angle
                tStep = move(obj,tStep);

                % Is a scan completed?
                if isCommandDone(obj)
                    numRaster = obj.pNumRaster;
                    % Is initial scan complete? Initial scan is scan number 0.
                    if obj.pMechScanNum>0 || (numRaster==1 && obj.pIs360)
                        if numRaster>1 || obj.pIs360
                            wasScanCompleted = wasScanCompleted || ... % Latched to true
                                (obj.pMechanicalGridIdx==numAng); % End of scan pattern's first raster bar
                        else
                            wasScanCompleted = wasScanCompleted || ... % Latched to true
                                (obj.pMechanicalGridIdx==1); % Start of scan pattern's raster bar
                        end
                    end
                end
                
                % Is sensor staring?
                if numAng==1
                    % Yes. No time remains for this step
                    tStep = zeros(1);
                end
            end
            obj.pIsScanDone = wasScanCompleted;
        end
        
        function [axis1, axis2] = slewAxes(obj)
            % Returns the primary and secondary raster axes which produce a
            % raster pattern with the least number of raster bars
            span = obj.FieldOfView;
            mechanicalScanLimits = obj.MechanicalScanLimits;
            
            numDwell = ceil(diff(mechanicalScanLimits,1,2)./span);
            tDwell = 1/obj.UpdateRate;
            slewRate = span/tDwell;
            slewRate = min([slewRate obj.MaxMechanicalScanRate],[],2);
            tDwell = span./slewRate;
            tScan = (numDwell-1).*tDwell + tDwell(end:-1:1);
            
            [~,axis1] = max(tScan);
            axis2 = mod(axis1,2)+1;
        end
        
        function slewRate = mechanicalSlewRate(obj,axis)
            % Returns the raster slew rate for the specified axis
            fov = obj.FieldOfView;
            tDwell = 1/obj.UpdateRate;
            numDwell = 1;
            
            if hasElecScan(obj) % Mech and elec scan
                axis2 = mod(axis,2)+1;
                eScan = obj.ElectronicScanLimits(axis2,:);
                eSpan = diff(eScan);
                if ~isSmall(obj,eSpan)
                    numDwell = eSpan/fov(axis2);
                end
            end
            
            slewRate = zeros(2,1);
            maxScanRate = obj.MaxMechanicalScanRate(axis);
            slewRate(axis) = min(fov(axis)/(numDwell*tDwell),maxScanRate);
        end
        
        function setupMechanicalScanGrid(obj)
            % Setup the scan angles comprising the mechanical scan grid
            
            % Is this a 360 degree scan mode?
            azLim = obj.MechanicalScanLimits(1,:);
            azSpan = diff(azLim);
            is360 = isSmall(obj,azSpan-360);
            obj.pIs360 = is360;
            
            % Find the primary and secondary mechanical slew axes
            [axis1,axis2] = slewAxes(obj);

            azAngs = mechanicalRasterAngles(obj,1);
            elAngs = mechanicalRasterAngles(obj,2);
            
            numAz = numel(azAngs);
            numEl = numel(elAngs);
            if axis1==1
                % Azimuth is the primary slew axis
                numRaster = numEl;
                numAng = numAz;
                ang1 = azAngs;
                ang2 = elAngs;
            else
                % Elevation is the primary slew axis
                numRaster = numAz;
                numAng = numEl;
                ang1 = elAngs;
                ang2 = azAngs;
            end
            
            if numRaster>1
                % Compute raster grid
                numGrid = numAng*(numRaster-1);
                scan1Grid = zeros(2,numGrid);
                scan2Grid = scan1Grid;
                
                % Assign 1st and 2nd raster scan patterns
                for iRaster = 1:(numRaster-1)
                    idx = (1:numAng)+(iRaster-1)*numAng;
                    scan1Grid(axis2,idx) = ang2(iRaster)*ones(1,numAng);
                    scan2Grid(axis2,idx) = ang2(numRaster-iRaster+1)*ones(1,numAng);
                    if is360
                        % Don't change direction along primary axis for 360
                        % degree scan
                        scan1Grid(axis1,idx) = ang1;
                        scan2Grid(axis1,idx) = ang1;
                    else
                        if numRaster==1
                            % Only one raster bar:
                            % Scan Pattern 1: Go there
                            % Scan Pattern 2: Return
                            scan1Grid(axis1,idx) = ang1;
                            scan2Grid(axis1,idx) = ang1(end:-1:1);
                        else % More than one raster bar
                            if mod(iRaster,2) % Odd raster bar
                                scan1Grid(axis1,idx) = ang1;
                                if mod(numRaster,2) % Odd number of raster bars
                                    scan2Grid(axis1,idx) = ang1;
                                else % Even number of raster bars
                                    scan2Grid(axis1,idx) = ang1(end:-1:1);
                                end
                            else % Even raster bar
                                scan1Grid(axis1,idx) = ang1(end:-1:1);
                                if mod(numRaster,2) % Odd number of raster bars
                                    scan2Grid(axis1,idx) = ang1(end:-1:1);
                                else % Even number of raster bars
                                    scan2Grid(axis1,idx) = ang1;
                                end
                            end
                        end
                    end
                end
            else
                if numAng>1
                    % Scanning: only a single raster bar
                    if ~is360
                        numAng = numAng-1;
                    end
                    numGrid = numAng;
                    scan1Grid = zeros(2,numGrid);
                    scan2Grid = scan1Grid;
                    
                    scan1Grid(axis2,:) = repmat(ang2,1,numGrid);
                    scan2Grid(axis2,:) = repmat(ang2,1,numGrid);
                    
                    if is360
                        % Don't change direction along primary axis for 360
                        % degree scan
                        scan1Grid(axis1,:) = ang1;
                        scan2Grid(axis1,:) = ang1;
                    else
                        scan1Grid(axis1,:) = ang1(1:end-1);
                        scan2Grid(axis1,:) = ang1(end:-1:2);
                    end
                else
                    % No scanning: starring at a single angle
                    numAng = 1;
                    numGrid = numAng;
                    scan1Grid = zeros(2,numGrid);
                    scan1Grid(axis1) = ang1;
                    scan1Grid(axis2) = ang2;
                    scan2Grid = scan1Grid;
                end
            end
            
            obj.pMechScan1GridAngs = reshape(scan1Grid,2,numAng,numGrid/numAng);
            obj.pMechScan2GridAngs = reshape(scan2Grid,2,numAng,numGrid/numAng);
            
            obj.pNumRaster = numRaster;
            obj.pNumAng = numAng;
            
            % Initial mechanical angle and scan direction
            obj.pMechanicalGridIdx = uint32(1);
            obj.pMechScanNum = uint32(0);
            obj.pMechanicalScanForward = true;
            obj.pCurrentMechanicalAngle = obj.pMechScan1GridAngs(:,obj.pMechanicalGridIdx);
            
            % Initial scan rate along primary axis
            obj.pCurrentMechanicalScanRate = mechanicalSlewRate(obj,axis1);
        end
        
        function cmdAngsOut = mechanicalRasterAngles(obj,axis)
            % Returns the raster scan command angles for the specified axis
            if ~hasMechScan(obj)
                cmdAngsOut = zeros(1);
                return
            end
            
            mScanLim = obj.MechanicalScanLimits(axis,:);
            scanLim = obj.ScanLimits(axis,:);
            angStep = obj.FieldOfView(axis);

            if hasElecScan(obj) % Mech and elec scan
                eScan = scanLim-mScanLim;
                eSpan = diff(eScan);
                if ~isSmall(obj,eSpan)
                    angStep = eSpan;
                end
            end
            
            cmdAngs = mScanLim(1):angStep:mScanLim(2);
            cmdAngs = cmdAngs-mean(cmdAngs)+mean(mScanLim);
            cmdAngs = fusion.internal.units.interval(cmdAngs,[-180 180]);
            if ~isscalar(cmdAngs) && cmdAngs(1)==cmdAngs(end)
                cmdAngsOut = cmdAngs(1:end-1);
            else
                cmdAngsOut = cmdAngs;
            end
        end
        
        function flag = isCommandDone(obj)
            % Returns true when the sensor's mount has finished slewing to
            % the commanded angle
            dAng = obj.CommandMechanicalAngle-obj.CurrentMechanicalAngle;
            dAng = fusion.internal.units.interval(dAng,[-180 180]);
            isScanning = obj.pCurrentMechanicalScanRate>0;
            flag = ~any(isScanning) || all(isSmall(obj,dAng(isScanning)));
        end
        
        function dt = move(obj,tStep)
            % Mechanically slews antenna by requested time step or until
            % the command angle is reached. Whichever occurs first.
            
            % Time required to reach command angle
            tReq = timerequired(obj);
            
            iFnd = find(isfinite(tReq));
            if isempty(iFnd)
                dt = zeros(1);
            else
                if isCommandDone(obj)
                    % If the command angle is the same as the current
                    % angle, then don't call again
                    dt = zeros(1);
                else
                    tSlew = max(tReq(iFnd));
                    if tSlew<tStep
                        tCmd = tSlew;
                        dt = tStep-tSlew; % Remaining time
                    else
                        tCmd = tStep;
                        dt = zeros(1);
                    end
                    
                    % Slew the mechanical mount
                    slew(obj,tCmd,1); % in azimuth
                    slew(obj,tCmd,2); % in elevation
                end
            end
        end
        
        function slewTime = timerequired(obj)
            % Returns the time required to slew the sensor's mount to the
            % requested command angle for a specific mechanical slew rate.
            dAng = obj.CommandMechanicalAngle-obj.CurrentMechanicalAngle;
            dAng = fusion.internal.units.interval(dAng,[-180 180]);
            slewTime = abs(dAng./obj.pCurrentMechanicalScanRate);
        end
        
        function remainingTime = slew(obj,dt,axis)
            % Rotate array around the sensor's mount axis until the
            % command angle is reached. Returns the remaining time needed
            % to reach the command angle for the requested axis
            remainingTime = zeros(1);
            
            % Has command angle been reached?
            if ~isCommandDone(obj)
                % How long to slew sensor mount
                curAng = obj.pCurrentMechanicalAngle(axis);
                cmdAng = obj.CommandMechanicalAngle(axis);
                dAng = cmdAng-curAng;
                dAng = fusion.internal.units.interval(dAng,[-180 180]);
                
                mechScanRate = obj.pCurrentMechanicalScanRate(axis);
                remainingTime = abs(dAng/mechScanRate);
                dt = min(dt,remainingTime);
                
                % Rotate sensor around the mount's axis
                dir = sign(dAng);
                dAng = dir*mechScanRate*dt;
                
                newAng = curAng+dAng;
                newAng = fusion.internal.units.interval(newAng,[-180 180]);
                obj.pCurrentMechanicalAngle(axis) = newAng;
                
                % Remaining slew time for this axis
                curAng = newAng;
                dAng = cmdAng-curAng;
                dAng = fusion.internal.units.interval(dAng,[-180 180]);
                remainingTime = abs(dAng/mechScanRate);
            end
        end
    end
    
    % ----------------------------
    % Coordinate frame conversions
    % ----------------------------
    methods
        function xSensor = bodyToSensor(obj, xBody)
            % xSensor = bodyToSensor(obj, xBody) transforms the 3-by-L
            % matrix of body coordinates, xBody, defined as [xb; yb; zb] to
            % sensor coordinates, xSensor.
            %
            % If xBody is a 6-by-L matrix, xBody is defined as
            % [xb; yb; zb; vxb; vyb; vzb], where vxb, vyb, and vzb define
            % the 3 components of the L velocity vectors in body
            % coordinates.
            %
            % sigSensor = bodyToSensor(obj, sigBody) transforms the
            % L-element struct array of signals in body coordinates,
            % sigBody, to sensor coordinates, sigSensor.
            
            orient = toRotmat(obj.LookOrientation);
            offsetIn = obj.MountingLocation(:);
            
            isSignal = isstruct(xBody) || isa(xBody,'fusion.internal.interfaces.BaseEmission');
            if isSignal
                numSig = numel(xBody);
                xSensor = xBody;
                for iSig = 1:numSig
                    thisSig = xSensor(iSig);
                    
                    pos = thisSig.OriginPosition(:);
                    vel = thisSig.OriginVelocity(:);
                    
                    hasFaces = isfield(thisSig,'IlluminatedSurface');
                    if hasFaces
                        faces = thisSig.IlluminatedSurface;
                    end
                    [sigOrient, wasQuat] = toRotmat(thisSig.Orientation);
                    
                    % Rotate into receiving sensor frame
                    pos = orient*(pos-offsetIn);
                    vel = orient*vel; % Sensor is stationary relative to the platform's body
                    
                    if hasFaces
                        faces = bsxfun(@minus,faces,offsetIn);
                        faces(:,:) = orient*faces(:,:);
                    end
                    sigOrient = sigOrient*orient';
                    
                    if wasQuat
                        sigOrientOut = quaternion(sigOrient,'rotmat','frame');
                    else
                        sigOrientOut = sigOrient;
                    end
                    
                    thisSig.OriginPosition(:) = pos;
                    thisSig.OriginVelocity(:) = vel;
                    if hasFaces
                        thisSig.IlluminatedSurface(:) = faces;
                    end
                    thisSig.Orientation(:) = sigOrientOut;
                    
                    xSensor(iSig) = thisSig;
                end
            else
                
                hasVel = size(xBody,1)>3;
                if hasVel
                    offset = [offsetIn;zeros(3,1,'like',offsetIn)];
                else
                    offset = offsetIn;
                end
                
                xSensor = fusion.internal.frames.transformParentToChild(xBody,orient,offset);
            end
        end
        
        function [xBody, cBody] = sensorToBody(obj, xSensor, cSensor)
            % xBody = sensorToBody(obj, xSensor) transforms the 3-by-L
            % matrix of sensor coordinates, xSensor, defined as [xs; ys;
            % zs] to body coordinates, xBody.
            %
            % If xSensor is a 6-by-L matrix, xSensor is defined as
            % [xs; ys; zs; vxs; vys; vzs], where vxs, vys, and vzs define
            % the 3 components of the L velocity vectors in sensor
            % coordinates.
            %
            % [xBody, cBody] = sensorToBody(obj, xSensor, cSensor) also
            % rotates the 3-by-3-by-L covariance matrix, cSensor, into body
            % coordinates.
            %
            % If cSensor is a 6-by-6-by-L covariance matrix, then cSensor
            % is rotated by: R = blkdiag(R0,R0), where R0 corresponds to
            % the 3-by-3 rotation matrix defining the frame rotation from
            % sensor coordinates to body coordinates.
            %
            % When xSensor is 3-by-L, cSensor must also be 3-by-3-by-L.
            % When xSensor is 6-by-L, cSensor must also be 6-by-6-by-L.
            orient = obj.LookOrientation;
            offsetIn = obj.MountingLocation(:);
            
            hasVel = size(xSensor,1)>3;
            if hasVel
                offset = [offsetIn;zeros(3,1,'like',offsetIn)];
            else
                offset = offsetIn;
            end
            
            hasCov = nargin>2;
            if hasCov
                [xBody, cBody] = fusion.internal.frames.transformChildToParent(xSensor,cSensor,orient,offset);
            else
                stateSz = size(xSensor,1);
                numVals = size(xSensor,2);
                unused = NaN(stateSz,stateSz,numVals,'like',xSensor);
                [xBody, cBody] = fusion.internal.frames.transformChildToParent(xSensor,unused,orient,offset);
            end
        end
    end
    
    % ---------
    % Utilities
    % ---------
    methods(Access=private)
        function flag = isSmall(obj,x)
            % Returns true when x is a small number (almost zero)
            tol = obj.SmallNumber;
            flag = abs(x)<=tol;
        end
        
        function flag = isGreaterThan(obj,x,y)
            % Returns true when x is greater than y. If y is not provided,
            % y is 0.
            if nargin<3
                y = zeros(1,'like',x);
            end
            tol = obj.SmallNumber;
            flag = x>y+tol;
        end
        
        function flag = isGreaterThanOrEqual(obj,x,y)
            % Returns true when x is greater than or equal to y. If y is
            % not provided, y is 0.
            if nargin<3
                y = zeros(1,'like',x);
            end
            tol = obj.SmallNumber;
            flag = x>=y-tol;
        end
        
        function flag = isLessThan(obj,x,y)
            % Returns true when x is less than y. If y is not provided,
            % y is 0.
            if nargin<3
                y = zeros(1,'like',x);
            end
            flag = isGreaterThan(obj,-x,y);
        end
        
        function flag = isLessThanOrEqual(obj,x,y)
            % Returns true when x is less than or equal to y. If y is
            % not provided, y is 0.
            if nargin<3
                y = zeros(1,'like',x);
            end
            flag = isGreaterThanOrEqual(obj,-x,y);
        end
    end
    
    methods(Static, Hidden)    
        function flag = isAllowedInSystemBlock
            flag = false;
        end
    end
end

% Local helper functions
function angs = lookAngles(limits,angStep)
% Returns the angles spanning the requested limits at the specified step size
angs = limits(1):angStep:limits(2);
angs = angs-mean(angs)+mean(limits);
numAngs = numel(angs);

if numAngs==0
    angs = zeros(1,'like',limits);
end
end

function [orientOut, wasQuat] = toRotmat(orientIn)
wasQuat = isa(orientIn, 'quaternion');
if wasQuat
    orientOut = rotmat(orientIn, 'frame');
else
    orientOut = orientIn;
end
end
