function state = constvel(state, varargin)
% CONSTVEL      A constant velocity (CV) motion model in state-space.
%   Both trackingEKF and trackingUKF require a definition of a
%   StateTransitionFcn property. CONSTVEL provides a constant velocity
%   state transition function.
%
%   state = CONSTVEL(state) calculates the state at the next time-step
%   based on the current state, assuming dt = 1 second.
%
%   state = CONSTVEL(state, dt) calculates the state at the next time-step
%   based on the current state and the time interval, dt. 
%
%   state = CONSTVEL(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 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]. 
%       In a 2-D case, state = [x; vx; y; vy].
%       In a 3-D case, state = [x; vx; y; vy; 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-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
%
%   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 velocity state with various time steps
%   % --------------------------------------------------------------------
%   % Define a state vector for 3-D constant velocity motion
%   state = [1;1;0;2;5;-2];
%
%   % Predict the state assuming dt = 1 second
%   state = CONSTVEL(state) 
%
%   % Predict the state given dt = 0.1 second
%   state = CONSTVEL(state, 0.1) 
%
%   % Example 2: predict multiple constant velocity states
%   % ----------------------------------------------------
%   % Define a state matrix for 3-D constant velocity motion
%   states = [1 2 2.5;1 2.5 3;0 -1 2;2 3 -1;5 0 3;-2 4 2];
%   
%   % Predict the state assuming dt = 1 second
%   states = CONSTVEL(states)
%   
%   % Predict the state given dt = 0.1 second
%   states = CONSTVEL(states, 0.1)
%
%   % Example 3: predict and measure the position of a constant velocity model
%   % ------------------------------------------------------------------------
%   % Define a state vector for a 2-D constant velocity motion
%   state = [10;-1;0;1];
%   dt = 0.2; % time step in seconds
%   
%   % Use CONSTVEL to create a trajectory and measure the positions with cvmeas
%   position = zeros(2, 100); % Pre-allocate memory
%   measurement = zeros(3, 100); % Pre-allocate memory
%   for i = 1:1:100
%       state = CONSTVEL(state, dt);
%       position(:,i) = [state(1); state(3)];
%       measurement(:,i) = cvmeas(state);
%   end
%   plot(position(1,:), position(2,:))
%   hold on
%   plot(measurement(1,:), measurement(2,:), '.')
%   title('Constant Velocity Model'); xlabel('X[m]'); ylabel('Y[m]')
%   legend('Trajectory', 'Measurements'); hold off;
%
%   See also trackingEKF, trackingUKF, initcvekf, initcvukf, cvmeas,
%   constacc, constturn, constveljac
   
%   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-2018 The MathWorks, Inc.

%#codegen

narginchk(1,3)

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

% Define constants and integer variables
if coder.target('MATLAB')
    TWO = 2;
    FOUR = 4;
    SIX = 6;
    numDims = 0;
    numCols = 0;
    numStates = 0;
else
    TWO = coder.internal.indexInt(2);
    FOUR = coder.internal.indexInt(4);
    SIX = coder.internal.indexInt(6);
    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 == TWO) || (colSize == FOUR) || (colSize == SIX);
isColOrient = (rowSize == TWO) || (rowSize == FOUR) || (rowSize == SIX);

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

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 2, this is safe to cast to an
% integer.
numDims(1) = numStates/TWO;

% 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'}, 'constvel', 'dt', nargin);
    dt = varargin{end};
end

for i = 1:numDims
    stateCol(2*i-1,:) = stateCol(2*i-1,:) + bsxfun(@times, stateCol(2*i,:),dt) + bsxfun(@times,(0.5*dt^2),wcol(i,:)); %Position update rows
    stateCol(2*i,:) = stateCol(2*i,:) + bsxfun(@times,wcol(i,:),dt); %Velocity update rows
end

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