function state = constacc(state, varargin)
% CONSTACC      A constant acceleration (CA) motion model in state-space.
%   Both trackingEKF and trackingUKF require a definition of a
%   StateTransitionFcn property. constacc provides a constant acceleration
%   state transition function.
%
%   state = constacc(state) calculates the state at the next time-step
%   based on the current state, assuming dt = 1 second.
%
%   state = CONSTACC(state, dt) calculates the state at the next time-step
%   based on the current state and the time interval, dt.
%
%   state = CONSTACC(state, w, dt) calculates the state at the next
%   time-step based on the current state, the noise w, and the time
%   interval, dt. The noise is treated as the acceleration increment of a
%   Wiener-sequence acceleration model.
%
%   state can be defined as a vector or matrix for 1, 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 1-D case, state = [x; vx; ax].
%       In a 2-D case, state = [x; vx; ax; y; vy; ay].
%       In a 3-D case, state = [x; vx; ax; y; vy; ay; z; vz; az].
%   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-by-N matrix, where D is the
%   motion dimensionality (1-D, 2-D or 3-D) and N is the number of state
%   columns. If specified as a scalar, it will be expanded to a D-by-N
%   matrix.
%
%   Positions are in meters; velocities are in meters/second; accelerations
%   are in meters/second^2.
%
%   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 acceleration state with various time steps
%   % ------------------------------------------------------------------------
%   % Define a state for 3-D constant acceleration motion
%   state = [1;1;1;2;1;0;2;-1;0.1]; 
%
%   % Predict the state assuming dt = 1 second
%   state = CONSTACC(state) 
%
%   % Predict the state given dt = 0.1 second
%   state = CONSTACC(state, 0.1) 
%   
%   % Example 2: predict multiple constact acceleration states
%   % --------------------------------------------------------
%   % Define a state matrix for 3-D constant accleration motion
%   states = [1 2 2.5;1 2 2.5;1 2 2.5;2 3 4;1 2 -1;0 -1 2;2 3 -1;-1 0 1;0.1 0.3 0.5];
%   
%   % Predict the states assuming dt = 1 second
%   states = CONSTACC(states)
%   
%   % Predict the states given dt = 0.1 second
%   states = CONSTACC(states, 0.1)
%
%   % Example 3: predict and measure the position of a constant acceleration model
%   % ----------------------------------------------------------------------------
%   % Define a state vector for a 2-D constant acceleration motion
%   state = [10;-1;0.1;0;1;-0.1];
%   dt = 0.2; % time step in seconds
%   
%   % Use CONSTACC to create a trajectory and measure the positions with cameas
%   position = zeros(2, 100); % Pre-allocate memory
%   measurement = zeros(3, 100); % Pre-allocate memory
%   for i = 1:1:100
%       state = CONSTACC(state, dt);
%       position(:,i) = [state(1); state(4)];
%       measurement(:,i) = cameas(state);
%   end
%   plot(position(1,:), position(2,:))
%   hold on
%   plot(measurement(1,:), measurement(2,:), '.')
%   title('Constant Acceleration Model'); xlabel('X[m]'); ylabel('Y[m]')
%   legend('Trajectory', 'Measurements'); hold off;
%
%   See also trackingEKF, trackingUKF, initcaekf, initcaukf, cameas,
%   constvel, constturn, constaccjac
   
%   References: 
%   [1] X. Rong Li and Vasselin P. Jilkov, "A Survey of Maneuvering Target
%       Tracking: Dynamic Models", Proceedings of the SPIE Conference on
%       Signal and Data Processing of Small Targets, 2000.

%   Copyright 2016-2022 The MathWorks, Inc.

%#codegen

narginchk(1,3)

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

% Define constants and integer variables
if coder.target('MATLAB')
    THREE = 3;
    SIX = 6;
    NINE = 9;
    numDims = 0;
    numCols = 0;
    numStates = 0;
else
    THREE = coder.internal.indexInt(3);
    SIX = coder.internal.indexInt(6);
    NINE = coder.internal.indexInt(9);
    numDims = coder.internal.indexInt(0);
    numCols = coder.internal.indexInt(0);
    numStates = coder.internal.indexInt(0);
end

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

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

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

numStates(1) = size(stateCol,1);
numCols(1) = size(stateCol,2);
% Already ensured numStates is a multiple of 3, safe to cast to integer
% here.
numDims(1) = numStates/THREE;

% 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
    coder.internal.assert(isequal(size(wcol),[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 % dt is always at the end of varargin
    validateattributes(varargin{end}, {'numeric'}, ...
        {'real', 'finite', 'scalar', 'nonsparse'}, 'constacc', 'dt', nargin);
    dt = varargin{end};
end

if dt < 0
    % Inverse model
    for i = 1:numDims
        % Position update
        stateCol(i*3-2,:) = stateCol(i*3-2,:) + bsxfun(@times,stateCol(i*3-1,:),dt) + bsxfun(@times,stateCol(i*3,:),((dt^2)/2)); 
        % Velocity update
        stateCol(i*3-1,:) = stateCol(i*3-1,:) + bsxfun(@times,stateCol(i*3,:),dt);
        % Acceleration update
        stateCol(i*3,:) = bsxfun(@minus,stateCol(i*3,:),wcol(i,:));
    end
else
    % Forward model
    for i = 1:numDims
        % Position update
        stateCol(i*3-2,:) = stateCol(i*3-2,:) + bsxfun(@times,stateCol(i*3-1,:),dt) + bsxfun(@times,stateCol(i*3,:),((dt^2)/2)) + bsxfun(@times,wcol(i,:), ((dt^2)/2)); 
        % Velocity update
        stateCol(i*3-1,:) = stateCol(i*3-1,:) + bsxfun(@times,stateCol(i*3,:),dt) + bsxfun(@times,wcol(i,:),dt);
        % Acceleration update
        stateCol(i*3,:) = bsxfun(@plus, stateCol(i*3,:),wcol(i,:));
    end
end

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