function state = constturn(state, varargin)
% CONSTTURN     Constant turn rate (CT) motion model
%   Both trackingEKF and trackingUKF require a definition of a
%   StateTransitionFcn property. CONSTTURN provides a constant turn rate
%   state transition function. A constant turn rate assumes that the motion
%   in the XY plane follows a constant angular velocity, omega. In
%   addition, the motion in the vertical Z plane is assumed to follow a
%   constant velocity model.
%
%   state = CONSTTURN(state) calculates the state at the next time-step
%   based on the current state, assuming dt = 1 second.
%
%   state = CONSTTURN(state, dt) calculates the state at the next
%   time-step based on the current state and the time interval, dt.
%
%   state = CONSTTURN(state, w, dt) calculates the state at the next
%   time-step based on the current state, the noise w, and the time
%   interval, dt.
%
%   state can be defined as a vector or a matrix for 2 or 3 dimensions.
%   If specified as a vector, the orientation of the state vector can be
%   either column or row vector, and the output will match the input's
%   orientation.
%       In a 2-D case, state = [x; vx; y; vy; omega]
%       In a 3-D case, state = [x; vx; y; vy; omega; z; vz]
%   If specified as a matrix, states must be concatenated along columns,
%   where each column represents a state following the convention specified
%   above.
%
%   If specified, w must be a scalar or a (D+1)-by-N matrix, where D is the
%   motion dimensionality (2-D or 3-D) and N is the number of state
%   columns. The rows of w correspond to [ax;ay;alpha;az], where ax, ay,
%   and az are the linear acceleration noise terms in the x-, y-, and
%   z-axes, resepctively, and alpha is the angular acceleration noise term.
%   If specified as a scalar, it will be expanded to a D-by-N matrix.
%
%   Positions are in meters; velocities are in meters/second; omega is the
%   turn rate in degrees/second.
%
%   Class support
%   -------------
%   state must be a finite real vector or matrix with the dimensions
%   specified above.
%   dt must be a real finite numeric scalar.
%
%   % Example 1: Predict a constant turn rate state with various time steps
%   % ---------------------------------------------------------------------
%   % Define a state vector for constant turn motion
%   state = [50; 0; 10; 10; rad2deg(0.2)];
%
%   % Predict the state assuming dt = 1 second
%   state = CONSTTURN(state) 
%
%   % Predict the state given dt = 0.1 second
%   state = CONSTTURN(state, 0.1) 
%
%   % Example 2: Predict a 3D constant turn rate state
%   % ---------------------------------------------------------------------
%   % Define a state vector for 3D constant turn motion
%   state = [50; 0; 10; 10; rad2deg(0.2); 10; 1];
%
%   % Predict the state assuming dt = 1 second
%   state = CONSTTURN(state) 
%
%   % Predict the state given dt = 0.1 second
%   state = CONSTTURN(state, 0.1) 
%
%   % Example 3: Predict multiple 3D constant turn rate states
%   % --------------------------------------------------------
%   % Define a state matrix for 3D constant turn motion
%   states = [50 100 20;0 10 2;10 15 23;10 12 15;rad2deg([0.2 0.4 0.8]);10 5 3;1 3 6];
%
%   % Predict the states assuming dt = 1 second
%   states = CONSTTURN(states)
%
%   % Predict the states given dt = 0.1 second
%   states = CONSTTURN(states, 0.1)
%
%   % Example 4: Predict and measure the position of a constant turn rate model
%   % -------------------------------------------------------------------------
%   % Define a state vector for a 2-D constant turn rate motion. 
%   % The trajectory describes a circle of radius 50 centered at (0,10) 
%   state = [50; 0; 10; 10; rad2deg(0.2)];
%   dt = 0.2; % time step in seconds
%   
%   % Use CONSTTURN to create a trajectory and measure the positions with ctmeas
%   position = zeros(2, 160); % Pre-allocate memory
%   measurement = zeros(3, 160); % Pre-allocate memory
%   for i = 1:1:160
%       state = CONSTTURN(state, dt);
%       position(:,i) = [state(1); state(3)];
%       measurement(:,i) = ctmeas(state);
%   end
%   plot(position(1,:), position(2,:))
%   hold on; axis equal; axis([-60 60 -50 70]); grid
%   plot(measurement(1,:), measurement(2,:), '.')
%   title('Constant Turn Rate Model'); xlabel('X[m]'); ylabel('Y[m]')
%   legend('Trajectory', 'Measurements'); hold off;
%
%   See also trackingEKF, trackingUKF, initctekf, initctukf, ctmeas,
%   constacc, constvel, constturnjac
   
%   References: 
%   [1] Samuel Blackman and Robert Popoli, "Design and Analysis of Modern
%       Tracking Systems", pages 205-207, Artech House, 1999.

%   Copyright 2016-2022 The MathWorks, Inc.

%#codegen

narginchk(1,3)

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

% Define constants and integer variables
if coder.target('MATLAB')
    ONE = 1;
    TWO = 2;
    FIVE = 5;
    SEVEN = 7;
    numDims = 0;
    numCols = 0;
    numStates = 0;
    szwcol = [0 0];
else
    ONE = coder.internal.indexInt(1);
    TWO = coder.internal.indexInt(2);
    FIVE = coder.internal.indexInt(5);
    SEVEN = coder.internal.indexInt(7);
    numDims = coder.internal.indexInt(0);
    numCols = coder.internal.indexInt(0);
    numStates = coder.internal.indexInt(0);
    szwcol = coder.internal.indexInt([0 0]);
end

% Check for row or column Orientation
[rowSize,colSize] = size(state);

isRowOrient = (colSize == FIVE) || (colSize == SEVEN);
isColOrient = (rowSize == FIVE) || (rowSize == SEVEN);

% 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(1) = size(stateCol,1);
numCols(1) = size(stateCol,2);
numDims(1) = (numStates+ONE)/TWO; % 5 states = 2D, need 3 noise terms, 7 states = 3D, need 4 noise terms

% If there are 3 input arguments, it means that the 2nd one (first
% varargin) is w
if nargin==3 || (nargin==2 && ~isscalar(varargin{1}))
    validateattributes(varargin{1},{'double','single'},...
        {'real','finite','nonsparse'}, mfilename, 'w', 2);
    w = cast(varargin{1},'like',state);
    if isscalar(w)
        wcol = w * ones(numDims,numCols,'like',state);
    else
        wcol = w;
    end
    szwcol(:) = size(wcol);
    coder.internal.assert(isequal(szwcol,[numDims,numCols]),'shared_tracking:motion:incorrectNoiseDim','w',numDims,numCols);
else
    wcol = zeros(numDims,numCols,'like',state);
end

% If dt is not given, assume dt = 1
if nargin==1 || (nargin==2 && ~isscalar(varargin{1}))
    dt = ones(1, 1, 'like', stateCol);
else
    validateattributes(varargin{end}, {'numeric'}, ...
        {'real', 'finite', 'scalar', 'nonsparse'}, 'constturn', 'dt', nargin);
    dt = varargin{end};
end

if dt < 0
    % Use inverse-dynamics model
    stateColT = matlabshared.tracking.internal.fusion.invconstturnImpl(stateCol, wcol, dt);
else
    % Use forward model
    stateColT = matlabshared.tracking.internal.fusion.constturnImpl(stateCol, wcol, dt);
end

% Return in same orientation 
if ~isColOrient
    state = stateColT';
else
    state = stateColT;
end
