function [q,qd,qdd,t,solninfo] = contopptraj(waypoints,vellim,accellim,varargin)
%CONTOPPTRAJ Create constrained time-optimal path parametrized trajectory
%   [Q,QD,QDD,T] = contopptraj(WAYPOINTS,VELLIM,ACCELLIM) fits a path to a
%   set of waypoints WAYPOINTS and finds a time-optimal trajectory along
%   the path subject to the specified velocity limits, VELLIM, and the
%   acceleration limits, ACCELLIM. WAYPOINTS is specified as an m-by-n
%   array where m is the number of elements in the state space, and n is
%   the number of distinct waypoints. VELLIM and ACCELLIM are m-by-2 arrays
%   specifying the minimum and maximum velocity and acceleration for each
%   state space element. To specify no limits on either velocity or
%   acceleration, pass an empty array. The outputs Q, QD, QDD, are
%   position, velocity, and acceleration samples respectively along the
%   trajectory. The output t is the time at which the samples occur.
%
%   [Q,QD,QDD,T] = contopptraj(POLYPATH,VELLIM,ACCELLIM) finds a
%   time-optimal trajectory along the specified polynomial path, POLYPATH,
%   subject to the specified velocity limits, VELLIM, and the acceleration
%   limits ACCELLIM. Here POLYPATH is specified as a scalar piecewise
%   polynomial struct such as the output returned from mkpp or spline.
%
%   [_,SOLNINFO] = contopptraj(_) outputs a solution-info struct with
%   diagnostic information associated with the output trajectory. The
%   struct contains these fields:
%      ExitFlag:    Number indicating whether a solution was found
%                      1 - 'Success'
%                      2 - 'Infeasible'
%
%   [ __ ] = contopptraj( __ ,NumSamples=N) specifies the number of samples
%   N to use when generating the trajectory. By default, the number of
%   samples is 100.
%
%   Example 1 : Generate Trajectory for Set of Waypoints
%   ----------------------------------------------------
%      % Create waypoints, velocity and acceleration limits
%      waypoints = [0 5 10 -10; 0 8 -10 5];
%      vellim = [-1,1;-2,2];
%      accellim = [-1,1;-2,2];
%
%      % Generate the trajectory
%      [q,qd,qdd,t] = contopptraj(waypoints,vellim,accellim);
%
%      % Plot positions, velocities, and accelerations against sample time
%      figure
%      subplot(1,3,1);
%      plot(t,q);
%      title("Position")
%      subplot(1,3,2);
%      plot(t,qd);
%      title("Velocity")
%      subplot(1,3,3);
%      plot(t,qdd);
%      title("Acceleration")
%      legend(["X","Y"])
%      sgtitle("Sample Time vs Positions/Velocities/Accelerations")
%
%   Example 2 : Generate Trajectory for Piecewise Polynomial
%   --------------------------------------------------------
%      wpts = [2 5 8 4];
%      tpts = [1 2 3 5];
%      numsamples = 50;
%
%      % Compute a minimum snap polynomial trajectory using default
%      % boundary conditions
%      [~,~,~,~,~,pp,~,~] = minsnappolytraj(wpts, tpts, numsamples);
%      vellim = [-10,15];
%      accellim = [-6,6];
%
%      % Generate the trajectory for the piecewise polynomial
%      [q,qd,qdd,t] = contopptraj(pp,vellim,accellim,NumSamples=1000);
%
%      % Plot position, velocity, and acceleration against sample time
%      figure
%      subplot(1,3,1);
%      plot(t,q);
%      title("Position")
%      subplot(1,3,2);
%      plot(t,qd);
%      title("Velocity")
%      subplot(1,3,3);
%      plot(t,qdd);
%      title("Acceleration")
%      sgtitle("Sample Time vs Position/Velocity/Acceleration")
%
%   References:
%      [1] Pham, Hung & Pham, Quang Cuong, "A New Approach to Time-Optimal
%      Path Parameterization Based on Reachability Analysis." IEEE
%      Transactions on Robotics. vol. 34, no. 3, pp. 645-659, June 2018
%
%   See also minjerkpolytraj, cubicpolytraj, trapveltraj.

%   Copyright 2021-2022 The MathWorks, Inc.

%#codegen

% Define the path to use for the optimization
    if isstruct(waypoints)
        pp=waypoints;

        % Use "unmkpp" to check pp form structure
        unmkpp(pp);
        pathPolynomial = waypoints;
    else
        validateattributes(waypoints,{'numeric'},{'2d','nonempty','real','finite'},'contopptraj','waypoints');
        numWaypoints=size(waypoints,2);
        if(numWaypoints<2)
            robotics.manip.internal.error('contopptraj:WaypointsTooFew')
        end
        pathPolynomial = fitPolynomialPath(waypoints);
    end
    pathObj = robotics.internal.toppra.paths.ParameterizedPolynomialPath(pathPolynomial);
    stateDimension = pathObj.Dimension;

    % Define the active constraints to use for the optimization
    hasVelLimits = ~isempty(vellim);
    hasAccelLimits = ~isempty(accellim);
    if (~hasVelLimits && ~hasAccelLimits)
        robotics.manip.internal.error('contopptraj:NoLimitsSpecified')
    end

    % Validate limits and create constraints. Validate "size" and "finite"
    % attributes first. Validate "increasing" second because it operates as
    % a row-wise validation along a column.
    if hasVelLimits
        validateattributes(vellim,{'numeric'},{'size',[stateDimension,2],'finite'},...
            'contopptraj','vellim');
        validateattributes(vellim',{'numeric'},{'increasing'},'contopptraj','vellim');
    end
    if hasAccelLimits
        validateattributes(accellim,{'numeric'},{'size',[stateDimension,2],'finite'},...
            'contopptraj','accellim');
        validateattributes(accellim',{'numeric'},{'increasing'},'contopptraj','accellim');
    end
    pathConstraints=...
        robotics.internal.toppra.utils.createPathConstraints(...
        hasVelLimits,vellim,hasAccelLimits,accellim);

    % Parse for Name-Value pairs
    params.NumSamples=100;
    s=coder.internal.nvparse(params,varargin{:});
    numSamples=robotics.internal.validation.validatePositiveIntegerScalar(s.NumSamples,...
                'contopptraj','NumSamples');

    % Configure the solver and solve for the optimal trajectory
    solver = robotics.internal.toppra.TOPPRASolver(pathObj, pathConstraints, numSamples);
    [q, qd, qdd, t, solninfo] = solver.solve();
end

function pathPolynomial = fitPolynomialPath(waypoints)
%fitPolynomialPath fits a polynomial path to a set of waypoints
%   This function is used as the default fitting mechanism when the user
%   provides numeric waypoints as an input.

    numWaypoints = size(waypoints, 2);

    % Assume it takes one second to reach each waypoint
    t = 0:numWaypoints-1;

    % Leverage minjerkpolytraj to fit the polynomial path
    numSamples = numWaypoints;
    [~, ~, ~, ~, pathPolynomial] = minjerkpolytraj(waypoints, t, numSamples);
end
