gpopsPropagate(solver,dynamics,solution,phase,options)

function [prop,prop_error] = gpopsPropagate(solver,dynamics,solution,phase,options)
%------------------------------------------------------------------%
%          Propagate Trajectory with Specified Controls            %
%------------------------------------------------------------------%

%------------------------------------------------------------------%
%--------%
% Inputs:
%--------%
%    solver:    string containing name of MATLAB ODE solver
%    dynamics:  dynamics function to be integrated
%    solution:  solution from GPOPS
%    phase:     phase number in which to perform propagation
%    options:   options from the MATLAB command ODESET
%--------%
% Outputs:
%--------%
%    prop:          Propagated solution from ODE solver
%    prop_error:    Error between GPOPS and propagted trajectory
%--------------------%
% Calling Sequence 1:
%--------------------%
%  [PROP,PROP_ERROR] = gpopsPropagate(SOLVER,DYNAMICS,SOLUTION,OPTIONS)
%      Will propagate solution from initial condition in every phase
%      of the optimal control problem
% --------------------%
% Calling Sequence 2:
% --------------------%
%  [PROP,PROP_ERROR] = gpopsPropagate(SOLVER,DYNAMICS,SOLUTION,PHASE,OPTIONS)
%      Will propagate solution from initial condition in every phase
%      of the optimal control problem using the options given in
%      the fifth argument OPTIONS.
%------------------------------------------------------------------%
% GPOPS Copyright (c) Anil V. Rao, Geoffrey T. Huntington, David   %
% Benson, Michael Patterson, Christopher Darby, & Camila Francolin %
%------------------------------------------------------------------%

% ------------------------------ %
% Check Number of Input Arguments
% ------------------------------ %
if nargin == 4
    options = [];
elseif nargin < 4
    error('Not Enough Input Arguments');
end

%----------------------%
% Get Number of Phases:
%----------------------%
n_phases = size(solution(:,1),1) - 1;

if ~isempty(phase) && isnumeric(phase) && phase <= n_phases
    iphase = phase;
    fphase = phase;
elseif ~isempty(phase) && isnumeric(phase) && phase > n_phases
    str = 'Number of phases to integrate exceeds number of phases in problem';
    error(str);
else
    iphase = 1;
    fphase = n_phases;
end
prop  = cell(n_phases+1,2);
prop_error = cell(n_phases+1,2);

%----------------------------------%
% Propagate Results for Each Phase %
%----------------------------------%
for i = iphase:fphase
    
    itime = solution{i,1};
    icond = solution{i,2};
    icond = icond(1,:);
    icontrols = solution{i,3};
    ncontrols = size(solution{i,3},2);
    nstates   = size(solution{i,2},2);
    
    switch solver    
     case 'ode45'
      [tout,yout] = ODE45(@gpopsDynamics,itime,icond,options,icontrols,i,itime,icond,dynamics,ncontrols,nstates);
     case 'ode113'
      [tout,yout] = ODE113(@gpopsDynamics,itime,icond,options,icontrols,i,itime,icond,dynamics,ncontrols,nstates);
     case 'ode15s'
      [tout,yout] = ODE15s(@gpopsDynamics,itime,icond,options,icontrols,i,itime,icond,dynamics,ncontrols,nstates);
     otherwise
      str = strcat(['ODE solver',solver,' does not exist']);
      error(str);
    end
    prop{i,1}      = tout;
    prop{i,2}      = yout;
    prop_error{i,1}     = prop{i,1} - solution{i,1};
    prop_error{i,2}     = prop{i,2} - solution{i,2};
end

prop{n_phases+1,1}      = 'time';
prop{n_phases+1,2}      = 'states';
prop_error{n_phases+1,1}     = 'time';
prop_error{n_phases+1,2}     = 'states';
end


function xdot = gpopsDynamics(t,x,icontrol,i,itime,icond,dynamics,ncontrols,nstates)

%------------------------------------------------------------------%
% GPOPSDYNAMICS:  Dummy ODE function for use with GPOPS            %
%------------------------------------------------------------------%

%------------------------------------------------------------------%
% GPOPS Copyright (c) Anil V. Rao, Geoffrey T. Huntington, David   %
% Benson, Michael Patterson, and Christopher Darby.                %
%------------------------------------------------------------------%

soldae{1} = t;
soldae{2} = x.';
for j = 1:ncontrols
    soldae{3}(j) = interp1(itime,icontrol(:,j),t,'spline');
end
soldae{4} = [];


dae = feval(dynamics,soldae,i);
xdot = dae(1:nstates);
xdot = xdot.';
    
end