function [Q,U,uInternal] = exampleHelperPropagateTruck(truckParams, controlParams, dt, q0, uRef, qTgt, maxNumStep)
%exampleHelperPropagateTruck Utility used to propagate the system
%
%   This helper function is used to propagate the truck-trailer system. It
%   integrates the system at a fixed time-step, dt, a maxNumStep number of
%   times. The function returns the current state, Q, the top-level control
%   command for the NEXT timestep, U, and the most recently applied
%   low-level control command, uInternal. Each row of Q, U, uInternal
%   contains the information at the given timestep.

% Copyright 2021 The MathWorks Inc.

    if all(q0(1:2) == qTgt(1:2))
        % No propagation needed
        Q = q0;
        U = uRef;
        uInternal = [0 0 0];
    else
        % Preallocate outputs for state and low-level control commands
        Q = nan(maxNumStep,size(q0,2));
        U = repmat(uRef,maxNumStep,1);
        uInternal = zeros(maxNumStep,3);

        % Create lookahead primative
        cmdVel = uRef(3)*controlParams.MaxVelocity;
        dir = cmdVel >= 0;
        
        % Create waypoints for the correct motion (forward/reverse)
        if dir
            % Reference path is for truck's rear-axle
            qTgtFull = exampleHelperExpandToFullState(qTgt, truckParams.L2, truckParams.M);
            waypoints = [uRef(1:2); qTgtFull(1:2)]';
        else
            % Reference path is for trailer's rear-axle
            waypoints = [uRef(1:2); qTgt(1:2)]';
        end

        % Retrieve current state/command
        q = q0;
        q(5) = qTgt(5);
        u = uRef;
        i = 0;

        % Drive system 
        while i < maxNumStep
            i = i+1;

            % Calculate steering angle
            [steer, refDist, pastTarget] = exampleHelperPurePursuitGetSteering(q, truckParams, controlParams, u(end), waypoints, dir);
            
            % Update control command
            v = cmdVel;
            u = [v steer refDist];

            % Calculate derivative
            dq = exampleHelperStateDerivative(truckParams.L1,truckParams.L2,truckParams.M,q,u);

            % Update state using simple Euler integration
            q = q+dt*dq;

            Q(i,:) = q;
            uInternal(i,:) = u;
            U(i,end) = refDist;

            if pastTarget
                % Exit early once we've reached or overshot the target
                Q((i+1):end,:) = [];
                U((i+1):end,:) = [];
                uInternal((i+1):end,:) = [];
                return;
            end
        end
    end
end