function jacobian = ctmeasjac(state, varargin)
% CTMEASJAC   Jacobian of the measurement using the constant turn rate (CT)
% model
%   The trackingEKF object allows you to specify the MeasurementJacobianFcn
%   property. CTMEASJAC provides the Jacobian of the constant turn rate
%   measurement function, ctmeas, with respect to the state.
%
%   jacobian = CTMEASJAC(state) provides the Jacobian of the measurement in
%   rectangular coordinates relative to the state. 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.
%	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.
%
%   jacobian = CTMEASJAC(state, frame) calculates the Jacobian of the
%   measurement in spherical coordinates if frame is 'spherical' and in
%   rectangular coordinates if frame is 'rectangular' (default). 
%   In ctmeas, the measurement is [x; y; z] in rectangular frame and 
%   [az; el; range; range-rate] in spherical frame. Therefore, the size of
%   the measurement Jacobian is:
%       In the rectangular case: 3-by-5 or 3-by-7, depending on the state
%       In the spherical case: 4-by-5 or 4-by-7, depending on the state
%
%   jacobian = CTMEASJAC(state, frame, sensorpos, sensorvel) allows the
%   definition of the sensor position and sensor velocity.
%
%   jacobian = CTMEASJAC(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.
%
%   jacobian = CTMEASJAC(state, measurementParameters) specifies 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 range rate 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.
%
%   When Frame is 'rectangular', HasVelocity determines if the measurement
%   is [x;y;z;vx;vy;vz] or [x;y;z].
%
%   When Frame is 'spherical': 
%   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,
%     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 Jacobian with a 2D state:
%   --------------------------------------------------------------
%   % Define a 2D state vector for constant turn motion
%   state = [1;10;2;20;10]; 
%
%   % Measurement Jacobian in rectangular frame
%   jacobian = CTMEASJAC(state) 
%
%   % Measurement Jacobian in spherical frame
%   jacobian = CTMEASJAC(state, 'spherical')
%
%   % Measurement Jacobian in spherical frame relative to a stationary  
%   % sensor located at [1;-2;0]
%   jacobian = CTMEASJAC(state, 'spherical', [1;-2;0], [0;0;0]) 
%
%   % Measurement Jacobian 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]; 
%   jacobian = CTMEASJAC(state, 'spherical', [1;-2;0], [0;0;0], laxes) 
%
%   Example 2: Constant-turn Jacobian with 3D state:
%   ----------------------------------------------
%   % Define a 3D state vector for constant turn motion
%   state = [1;10;2;20;10;10;1]; 
%
%   % Measurement Jacobian in rectangular frame
%   jacobian = CTMEASJAC(state) 
%
%   % Measurement Jacobian in spherical frame
%   jacobian = CTMEASJAC(state, 'spherical')
%
%   % Measurement Jacobian in spherical frame relative to a stationary  
%   % sensor located at [1;-2;0]
%   jacobian = CTMEASJAC(state, 'spherical', [1;-2;0], [0;0;0]) 
%
%   % Measurement Jacobian 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]; 
%   jacobian = CTMEASJAC(state, 'spherical', [1;-2;0], [0;0;0], laxes) 
%
%   See also trackingEKF, initctekf, constturn, ctmeas, cameasjac,
%   cvmeasjac

%   Copyright 2016-2018 The MathWorks, Inc.

%#codegen

% Validate inputs
validateattributes(state, {'numeric'},...
    {'real', 'finite', 'vector', 'nonsparse'}, 'ctmeasjac', 'state', 1);

numStates = numel(state);
classToUse = class(state);
cond = (numStates == 5 || numStates == 7);
coder.internal.errorIf(~cond, 'shared_tracking:motion:incorrectStateVecWithInfo','[5 7]');

hasZStates = (numStates == 7);

if numel(varargin) > 0
    candidateStruct = varargin{1};
    if isstruct(candidateStruct) 
        argsinCell = matlabshared.tracking.internal.fusion.measmodelsvalidateoptionalstruct('ctmeasjac', 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('ctmeasjac', classToUse, argsinCell{:});

% Calculate the Jacobian
if isRect
    if ~hasVel
        jacobian = zeros(3, numStates, classToUse);
        % Essentially, eye(3) * localAxes', where eye(3) is the Jacobian of
        % measurement to [x;y;z]
        A = localAxes'; 
		for i = 1:5/2
			jacobian(:, 2*i-1) = A(:, i);
		end
		if hasZStates
			jacobian(:, 6) = A(:, 3);
		end
    else
        jacobian = zeros(6, numStates, classToUse);
        % Essentially, eye(3) * localAxes', where eye(3) is the Jacobian of
        % measurement to both [x;y;z] and [vx;vy;vz]
        A = localAxes'; 
		for i = 1:5/2
			jacobian(1:3, 2*i-1) = A(:, i);
			jacobian(4:6, 2*i) = A(:, i);
		end
		if hasZStates
			jacobian(1:3, 6) = A(:, 3);
			jacobian(4:6, 7) = A(:, 3);
		end
    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;
    
    jacobian = zeros(measSize, numStates, classToUse); %Up to 4 outputs (az, el, r, rr) by n states
    
    tgtpos = zeros(3,1,classToUse);
	if ~hasZStates
		tgtpos(1:2) = state([1,3]);
	else
		tgtpos(1:3) = state([1,3,6]);
	end
    A = matlabshared.tracking.internal.fusion.global2localcoordjac(tgtpos, sensorpos, localAxes);
    
    measLogicalIndex = [hasAz hasEl hasRange];
      
    if ~hasRangeRate
        if hasZStates
            jacobian(:,[1 3 6]) = A(measLogicalIndex,1:3);
        else
            jacobian(:,[1 3]) = A(measLogicalIndex,1:2);
        end
    else
        rrjac = matlabshared.tracking.internal.fusion.rangeratejac...
				(state,sensorpos, sensorvel, true); % call with flag = 1, CT model
        if hasZStates
            jacobian(1:end-1,[1 3 6]) = A(measLogicalIndex,1:3);
            jacobian(end,[1:4,6:7]) = rrjac;
        else
            jacobian(1:end-1,[1 3]) = A(measLogicalIndex,1:2);
            jacobian(end,1:4) = rrjac;
        end
    end
end