function [measurement, bounds] = ctmeas(state, varargin)
% CTMEAS   Measurement based on the constant turn rate (CT) motion model
%   Both trackingEKF and trackingUKF require a definition of a
%   MeasurementFcn property. CTMEAS provides the position measurement of
%   the state used in the constant turn rate motion function, constturn.
%
%   MEASUREMENT = CTMEAS(STATE) provides the position measurement of a
%   state used in the constant turn rate motion model in rectangular
%   coordinates relative to a stationary sensor located at the origin. The
%   state vector is either [x; vx; y; vy; omega] for a 2D motion model or
%	[x; vx; y; vy; omega; z; vz] for a 3D motion model.
%   If the input, state, is specified as a matrix, states must be
%   concatenated along columns, where each column represents a state
%   following the convention specified above. The output will be a matrix 
%   with the same number of columns as the input, where each column 
%   represents the measurement from the corresponding state.
%
%   Positions are in meters; velocities are in meters/second; omega is the
%   turn rate in degrees/second.
%	To produce measurements in 3D even when the state does not contain a
%	z-component, the value along the z axis is assumed to be 0.
%
%   MEASUREMENT = CTMEAS(STATE, FRAME) measurement is in spherical
%   coordinates if FRAME is 'spherical' and in rectangular coordinates if
%   FRAME is 'rectangular' (default). 
%   If using rectangular coordinates, the three elements in measurement
%   represent (x,y,z) in meters. If the coordinates are in spherical form,
%   the measurement contains four elements (az, el, r, rr), where azimuth
%   (az, in degrees) is measured from x axis toward y axis, elevation (el,
%   in degrees) is measured from x-y plane toward z axis, r (in meters) is
%   the range, and rr (in meters/second) is the range rate. Range rate is
%   positive for a target moving away from the sensor.
%
%   MEASUREMENT = CTMEAS(STATE, FRAME, SENSORPOS, SENSORVEL) allows the
%   definition of the sensor position and sensor velocity.
%
%   MEASUREMENT = CTMEAS(STATE, FRAME, SENSORPOS, SENSORVEL, LAXES)
%   specifies the axes, in LAXES, of the local coordinate system. LAXES is
%   a 3-by-3 orthonormal orientation matrix whose columns specify the
%   directions of the local x, y, and z axes in the global Cartesian frame.
%
%   MEASUREMENT = CTMEAS(STATE, MEASPARAMS) specifies MEASPARAMS, the
%   measurement parameters as a struct. The struct must have the following
%   fields (or a subset of them):
%       Frame          - either 'rectangular' or 'spherical' or an enum 
%                        with the same values.
%       OriginPosition - a 3-by-1 real vector.
%       OriginVelocity - a 3-by-1 real vector.
%       Orientation    - a 3-by-3 orthonormal orientation matrix.
%       HasAzimuth     - a logical scalar, true if azimuth is measured.
%       HasElevation   - a logical scalar, true if elevation is measured.
%       HasVelocity    - a logical scalar, true if velocity is measured.
%       HasRange       - a logical scalar, true if range is measured.
%		IsParentToChild - a logical scalar, true if the orientation is 
%						  given as a parent to child frame rotation.
%
%   [..., BOUNDS] = CTMEAS(...), additionally returns the measurement
%   bounds, which are used by the filter in residual calculations.
%   The function returns the following bounds based on the frame.
%       Rectangular: An M-by-2 matrix where M is the number of elements in
%                    the measurement and each row is [-Inf Inf], indicating
%                    no wrapping.
%       Spherical:   An M-by-2 matrix where M is the number of elements in
%                    the measurement. The rows are in the following order:
%                       When HasAzimuth = true,   [-180 180]
%                       When HasElevation = true, [-90 90]
%                       When HasRange = true,     [-Inf Inf]
%                       When HasRangeRate = true, [-Inf Inf]
%
%   When Frame is 'rectangular', HasVelocity determines if the measurement
%   is [x;y;z;vx;vy;vz] or [x;y;z].
%
%   When Frame is 'spherical', the measurement order is [az,el,r,rr]
%   HasAzimuth   - determines if measurement contains azimuth measurement 
%   HasElevation - determines if measurement contains elevation measurement
%   HasRange     - determines if measurement contains range measurement
%   If HasRange is true:
%   HasVelocity  - determines if measurement contains range-rate.
%   If HasRange is false:
%   HasVelocity  - no effect on measurement. 
%
%   Class support
%   -------------
%   * state must be a finite real vector of the dimensions specified above,
%     or a matrix with states concatenated along columns, either double or 
%     single precision.
%   * frame must be specified as either 'spherical' or 'rectangular'.
%   * sensorpos and sensorvel must be a 3 elements finite real column 
%     vector of the same type as state.
%   * laxes must be a 3x3 real rotation matrix of the same type as state.
%
%   Example 1: Constant-turn measurement with a 2D state:
%   -----------------------------------------------------
%   % Define a state vector for constant turn motion
%   state = [1;10;2;20;5]; 
%
%   % Measurement in rectangular frame
%   measurement = CTMEAS(state) 
%
%   % Measurement in spherical frame
%   measurement = CTMEAS(state, 'spherical')
%
%   % Measurement in spherical frame relative to a stationary sensor 
%   % located at [1;-2;0]
%   measurement = CTMEAS(state, 'spherical', [1;-2;0], [0;0;0]) 
%
%   % Measurement in spherical frame relative to a stationary sensor 
%   % located at [1;-2;0] that is rotated by 90 degrees about the z axis 
%   % relative to the global frame
%   laxes = [0 -1 0; 1 0 0; 0 0 1];
%   measurement = CTMEAS(state, 'spherical', [1;-2;0], [0;0;0], laxes)
%
%   % Measurements from multiple 2D states in a spherical frame specified 
%   % in the last statement.
%   states = [1 2 4; 10 20 30; 2 4 5; 20 10 5; 5 10 12];
%   measurements = CTMEAS(states, 'spherical', [1;-2;0], [0;0;0], laxes)
%
%   Example 2: Constant-turn measurement with a 3D state:
%   -----------------------------------------------------
%   % Define a state vector for constant turn motion
%   state = [1;10;2;20;5;10;1]; 
%
%   % Measurement in rectangular frame
%   measurement = CTMEAS(state) 
%
%   % Measurement in spherical frame
%   measurement = CTMEAS(state, 'spherical')
%
%   % Measurement in spherical frame relative to a stationary sensor 
%   % located at [1;-2;0]
%   measurement = CTMEAS(state, 'spherical', [1;-2;0], [0;0;0]) 
%
%   % Measurement in spherical frame relative to a stationary sensor 
%   % located at [1;-2;0] that is rotated by 90 degrees about the z axis 
%   % relative to the global frame
%   laxes = [0 -1 0; 1 0 0; 0 0 1];
%   measurement = CTMEAS(state, 'spherical', [1;-2;0], [0;0;0], laxes) 
%
%   % Measurements from multiple 3D states in a spherical frame specified 
%   % in the last statement.
%   states = [1 2 4; 10 20 30; 2 4 5; 20 10 5; 5 10 12; 10 12 15; 1 4 2];
%   measurements = CTMEAS(states, 'spherical', [1;-2;0], [0;0;0], laxes)
%   
%   See also trackingEKF, trackingUKF, initctekf, initctukf, constturn,
%   cameas, cvmeas, ctmeasjac

%   Copyright 2016-2021 The MathWorks, Inc.

%#codegen

% Validate state input
validateattributes(state, {'numeric'},...
    {'real', 'finite', '2d', 'nonsparse'}, 'ctmeas', 'state', 1);

% Check for row or column Orientation
[rowSize,colSize] = size(state);
isRowOrient = (colSize == 5) || (colSize == 7);
isColOrient = (rowSize == 5) || (rowSize == 7);

% Only column orientation is supported for vectorized state
cond = (~isRowOrient || ~isvector(state)) && ~isColOrient;
coder.internal.errorIf(cond, 'shared_tracking:motion:incorrectStateSizeWithInfo','[5 7]');

if ~isColOrient
    stateCol = state'; 
else
    stateCol = state;
end

[numStates,numStateVec] = size(stateCol);

hasZStates = numStates == 7;

classToUse = class(state);

if numel(varargin) > 0
    candidateStruct = varargin{1};
    if isstruct(candidateStruct) 
        argsinCell = matlabshared.tracking.internal.fusion.measmodelsvalidateoptionalstruct('ctmeas', classToUse, candidateStruct);
    else %assume that the arguments are in a cell array
        argsinCell = {varargin{:}};
    end
else
    argsinCell = {};
end

[isRect, sensorpos, sensorvel, localAxes, hasAz, hasEl, hasVel, hasRange] = ...
    matlabshared.tracking.internal.fusion.measmodelsvalidateoptionalinput('ctmeas', classToUse, argsinCell{:});

% Calculate measurement
if ~hasZStates
    tgtpos = [stateCol(1,:); stateCol(3,:); zeros(1,numStateVec,classToUse)];   
    tgtvel = [stateCol(2,:); stateCol(4,:); zeros(1,numStateVec,classToUse)];
else
    tgtpos = [stateCol(1,:); stateCol(3,:); stateCol(6,:)];
    tgtvel = [stateCol(2,:); stateCol(4,:); stateCol(7,:)];
end

if isRect
    if ~hasVel % Only position is measured
        measurement = matlabshared.tracking.internal.fusion.global2localcoord(tgtpos,'rr',...
            sensorpos,localAxes);
    else % Both position and velocity are measured
        posMeas = matlabshared.tracking.internal.fusion.global2localcoord(tgtpos,'rr',...
            sensorpos,localAxes);
        velMeas = matlabshared.tracking.internal.fusion.global2localcoord(tgtvel,'rr',...
            sensorvel,localAxes);
        measurement = [posMeas;velMeas];
    end
else % Spherical coordinate system.
    
    % Range-rate available if hasRange is true
    hasRangeRate = hasVel && hasRange;
    
    % Define the size of the measurement
    measSize = hasAz + hasEl + hasRange + hasRangeRate;
    
    measurement = zeros(measSize,numStateVec,classToUse);
    
    meas = matlabshared.tracking.internal.fusion.global2localcoord(tgtpos,'rs',...
        sensorpos,localAxes);
    
    measLogicalIndex = [hasAz hasEl hasRange];
    
    if ~hasRangeRate
        % Preserve data type, size of measurement
        measurement(:) = meas(measLogicalIndex,:);
    else
        measurement(1:end-1,:) = meas(measLogicalIndex,:);
        measurement(end,:) = -matlabshared.tracking.internal.fusion.radialspeed(tgtpos, tgtvel, sensorpos, sensorvel);
    end
end

if nargout == 2
    bounds = matlabshared.tracking.internal.fusion.measurementBounds(isRect,hasAz,hasEl,hasRange,hasVel,classToUse);
end