classdef(Hidden) MountedDevice < matlab.System & ...
        fusion.internal.remotesensors.mixin.ValidationUtils
    %   This class is used to define device mounts
    %
    %   This class is for internal use only. It may be removed in the future.
    
    %   Copyright 2018-2020 The MathWorks, Inc.
    
    %#codegen
    
    properties(Nontunable)
        %UpdateRate System's update rate
        %   Rate at which the system updates, 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(Dependent, Nontunable)
        %FieldOfView System's angular field of view
        %   The system'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 system during a system
        %   update in degrees and lies in the interval (0 360] for azimuth 
        %   and (0,180] for elevation.
        %
        %   Default: [1;5]
        FieldOfView
    end
    properties(Access = private)
        pFieldOfView = [1;5]
    end
    
    % Set in constructor only (i.e. don't need save/load)
    properties(Nontunable, Access = protected)
        pIsFieldOfViewReadOnly = false
    end
    
    properties(Nontunable)
        %MountingLocation  System's (x,y,z) mounted location on platform (m)
        %   A 1-by-3 vector defining the offset of the system's origin from
        %   the origin of its platform. The default value leaves the
        %   system'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 system 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 system is mounted to the system'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]
    end
    
    properties(Dependent,Access=protected)
        %MountingOrientation Orientation matrix
        %   Specifies the frame rotation matrix defining the mounted
        %   orientation of the system.
        MountingOrientation
    end

    % system step times
    properties(Access = protected)
        pHasFirstUpdate = false
        pTimeLastUpdate = 0
        pTimeLastQuery = 0
    end
    
    % Save version
    properties(Access = protected)
        pVersion = ver('fusion');
    end
    
    % -------------------
    % Setters and getters
    % -------------------
    methods
        function set.UpdateRate(obj, val)
            obj.checkScalarRealPositiveFinite(val,mfilename,'UpdateRate');
            obj.UpdateRate = val;
        end
        
        function set.FieldOfView(obj, val)
            cond = obj.pIsFieldOfViewReadOnly;
            coder.internal.errorIf(cond,'MATLAB:class:SetProhibited','FieldOfView',class(obj));
            
            obj.checkVectorRealFinite(val,mfilename,'FieldOfView',2);
            
            % Check that values lie in the interval (0, 360] for azimuth
            % and (0 180] for elevation.
            coder.internal.assert(val(1) > 0 && val(1) <= 360, 'shared_radarfusion:RemoteSensors:invalidAzFOV');
            coder.internal.assert(val(2) > 0 && val(2) <= 180, 'shared_radarfusion:RemoteSensors:invalidElFOV');
            
            obj.pFieldOfView = val(:);
        end
        function val = get.FieldOfView(obj)
            val = getFieldOfViewImpl(obj);
        end
    end
    methods(Access = protected)
        function val = getFieldOfViewImpl(obj)
            val = obj.pFieldOfView;
        end
    end
    
    methods
        function set.MountingLocation(obj,val)
            obj.checkVectorRealFinite(val,mfilename,'MountingLocation',3);
            obj.MountingLocation = val(:)';
        end
        
        function set.MountingAngles(obj,val)
            obj.checkVectorRealFinite(val,mfilename,'MountingAngles',3);
            obj.MountingAngles = val(:)';
        end
        
        function val = get.MountingOrientation(obj)
            val = fusion.internal.frames.ypr2rotmat(obj.MountingAngles,'degrees');
        end
    end
    methods(Access=protected)
        function val = getLookOrientation(obj)
            val = obj.MountingOrientation;
        end
    end
    
    % -----------
    % Constructor
    % -----------
    methods
        function obj = MountedDevice(varargin)
            % Support name-value pair arguments when constructing object
            setProperties(obj,nargin,varargin{:})
        end
    end
    
    % --------------
    % Implementation
    % --------------
    methods(Access = protected)
        function resetImpl(obj)
            % Initialize / reset discrete-state properties
            obj.pHasFirstUpdate = false;
            obj.pTimeLastUpdate = 0;
            obj.pTimeLastQuery = 0;
        end
    end
    
    methods(Hidden)
        function flag = isSignalGenerator(~)
            % Returns true if device returns signals
            flag = false;
        end
        
        function flag = isDetectionGenerator(~)
            % Returns true if device returns detections
            flag = false;
        end
        
        function flag = requiresSignals(~)
            % Returns true if the device requires a signal input
            flag = false;
        end
        
        function flag = requiresConfig(~)
            % Returns true if the device requires a config input
            flag = false;
        end
        
        function flag = supportsSignals(~)
            % Returns true if the device can accept a signal input, but a
            % signal input is not required (i.e. it has a HasInterference
            % property)
            flag = false;
        end
        
        function flag = supportsINS(~)
            % Returns true if the device can accept an INS input (i.e. it
            % has a HasINS property)
            flag = false;
        end
    end
    
    % ----------------
    % Display settings
    % ----------------
    methods(Access = protected, Static)
        function groups = getPropertyGroupsImpl
            mainList = {'UpdateRate'};
            mainGroup = matlab.system.display.Section('PropertyList',mainList);

            mntList = {'MountingLocation','MountingAngles'};
            mntGroup = matlab.system.display.Section('PropertyList',mntList);
            
            limitList = {'FieldOfView'};
            limitGroup = matlab.system.display.Section('PropertyList',limitList);
            
            groups = [mainGroup,mntGroup,limitGroup];
        end
    end
    
    % -------------
    % Save and load
    % -------------
    methods(Access = protected)
        function loadObjectImpl(obj,s,wasLocked)
            % Set properties in object obj to values in structure s

            % Set private and protected properties
            % (use "if isfield" to support old data files where this object
            % was saved without these properties)
            obj.pFieldOfView = s.pFieldOfView; 
            if isfield(s,'pHasFirstUpdate')
                obj.pHasFirstUpdate = s.pHasFirstUpdate;
            end
            if isfield(s,'pTimeLastUpdate')
                obj.pTimeLastUpdate = s.pTimeLastUpdate;
            end
            if isfield(s,'pTimeLastQuery')
                obj.pTimeLastQuery = s.pTimeLastQuery;
            end
            if isfield(s,'pVersion')
                obj.pVersion = s.pVersion;
            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
            s.pFieldOfView = obj.pFieldOfView;
            s.pHasFirstUpdate = obj.pHasFirstUpdate;
            s.pTimeLastUpdate = obj.pTimeLastUpdate;
            s.pTimeLastQuery = obj.pTimeLastQuery;
            s.pVersion = obj.pVersion;
            
        end
    end
    
    % hooks for trackingScenario
    methods (Access = {?matlab.unittest.TestCase,?radarfusion.internal.scenario.Platform,?radarfusion.internal.scenario.mixin.SampledDevice})
        function newTime = nextValidUpdateTime(obj, currentTime)
            % Returns the next admissible time by the sensor
            % if the sensor has not yet started, return currentTime.
            if obj.pHasFirstUpdate
                elapsedInterval = currentTime-obj.pTimeLastQuery;
                rate = obj.UpdateRate;
                numInts = ceil((elapsedInterval+obj.pSmallValue)*rate);
                newTime = obj.pTimeLastQuery + numInts/rate;
            else
                newTime = currentTime;
            end
            obj.pTimeLastQuery = newTime;
        end
    end
    
    % ----------------------------
    % Input parsing and validation
    % ----------------------------
    methods(Access = protected)
        function flag = isValidUpdateTime(obj, time)
            % Returns true if time is a valid update time (an integer
            % multiple of the system's update interval), otherwise false is
            % returned
            
            % If an interval is skipped, allow for update on the next one.
            % Should check "modulo" the update interval.
            elapsedInterval = time-obj.pTimeLastUpdate;
            rate = obj.UpdateRate;
            numInts = round(elapsedInterval*rate);
            timeOffset = elapsedInterval-numInts/rate; % Offset from closest interval
            
            flag = ~obj.pHasFirstUpdate || ...
                elapsedInterval>=-obj.pSmallValue && (abs(timeOffset) <= obj.pSmallValue);
        end
        
        function validateInputsImpl(obj, time)
            % Validate inputs to the step method at initialization (or
            % whenever an input size changes)
            obj.checkScalarRealNonnegativeFinite(time,mfilename,'time');
        end
    end
    
    % ---------------------
    % Coordinate transforms
    % ---------------------
    methods(Access = protected)
        function [xNED, cNED] = bodyToScenario(~, ins, xBody, cBody)
            % xNED = bodyToScenario(obj, ins, xBody) transforms the 3-by-L
            % matrix of body coordinates, xBody, defined as [xb; yb; zb] to
            % NED coordinates, xNED.
            %
            % 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.
            %
            % [xNED, cNED] = bodyToScenario(obj, ins, xBody, cBody) also
            % rotates the 3-by-3-by-L covariance matrix, cBody, into NED
            % coordinates.
            %
            % If cBody is a 6-by-6-by-L covariance matrix, then cBody is
            % rotated by: R = blkdiag(R0,R0), where R0 corresponds to the
            % 3-by-3 rotation matrix defining the frame rotation from
            % body coordinates to NED coordinates.
            %
            % When xBody is 3-by-L, cBody must also be 3-by-3-by-L.
            % When xBody is 6-by-L, cBody must also be 6-by-6-by-L.
            %
            % ins is a struct defining the transformation from body to NED
            % coordinates with the fields:
            %   Position        3-element vector
            %   Velocity        3-element vector
            %   Orientation     3x3 orientation (rotation) matrix which
            %                   rotates the NED axes into the axes of the
            %                   body frame
            
            orient = ins.Orientation;
            offset = ins.Position(:);
            
            hasVel = size(xBody,1)>3;
            if hasVel
                offset = [offset;ins.Velocity(:)];
            end
            
            hasCov = nargin>2;
            if hasCov
                [xNED, cNED] = fusion.internal.frames.transformChildToParent(xBody,cBody,orient,offset);
            else
                stateSz = size(xBody,1);
                numVals = size(xBody,2);
                unused = NaN(stateSz,stateSz,numVals,'like',xBody);
                [xNED, cNED] = fusion.internal.frames.transformChildToParent(xBody,unused,orient,offset);
            end
        end
    end
end
