function trailerTForm = exampleHelperPlotTruck(varargin)
%exampleHelperPlotTruck Animates a trajectory for the truck-trailer system

% Copyright 2021 The MathWorks Inc.

    if nargin == 1
        % Assume input is a path object, make a copy
        pathObject = copy(varargin{1});
        truckParams = pathObject.StatePropagator.StateSpace.TruckParams;
        controlParams = pathObject.StatePropagator.ControlParams;
        
        % Grab all states, controls, and targets
        dt = pathObject.StatePropagator.ControlStepSize;
        Q0 = pathObject.States;
        U0 = pathObject.Controls;
        QTGT = pathObject.TargetStates;
        N = round(pathObject.Durations/dt);
        % Get interpolated states and steering angles
        q = [];
        uInt = [];
        for i = 1:pathObject.NumSegments
            % Use the internal propagator to retrieve steering commands
            [Q,~,uInternal] = exampleHelperPropagateTruck(truckParams,controlParams,dt,Q0(i,:),U0(i,:),QTGT(i,:),N(i));
            q = [q; Q];
            uInt = [uInt; uInternal];
        end
        steerAngle = uInt(:,2);
    else
        % Assume we are only plotting a sequence of states
        truckParams = varargin{1};
        q = varargin{2};
        if nargin == 3
            steerAngle = varargin{3};
        else
            steerAngle = zeros(size(q,1),1);
        end
    end
    if isempty(q)
        return;
    end
    x2Sample = q(:,1);
    y2Sample = q(:,2);
    th2Sample = q(:,3);
    betaSample = q(:,4);
    W1 = truckParams.W1;
    L1 = truckParams.L1;
    T0Truck = [1 0 L1/2;
               0 1 0;
               0 0 1];
    truckPoints = transformPoints(getRectangleVertices(L1, W1), T0Truck);

    W2 = truckParams.W2;
    L2 = truckParams.L2;
    T0Trailer = [1 0 L2/2;
                 0 1 0;
                 0 0 1];
    trailerPoints = transformPoints(getRectangleVertices(L2, W2), T0Trailer);
    
    W3 = truckParams.Wwheel;
    L3 = truckParams.Lwheel;
    wheelPoints = getRectangleVertices(L3, W3);
    
    ax = gca;
    hold on;
    
    trailerTForm = hgtransform('Parent',ax);
    truckGroup = hggroup('Parent',trailerTForm);
    hTrailer = plot(trailerPoints(:,1), trailerPoints(:,2), 'b', 'LineWidth', 1, 'Parent',truckGroup);
    hWheelTrailerBL = fill(wheelPoints(:,1)+trailerPoints(3,1), wheelPoints(:,2)+trailerPoints(3,2), 'k', 'Parent',truckGroup);
    hWheelTrailerBR = fill(wheelPoints(:,1)+trailerPoints(4,1), wheelPoints(:,2)+trailerPoints(4,2), 'k', 'Parent',truckGroup);
    
    hingeTForm = hgtransform('Parent',truckGroup);
    hingeTForm.Matrix(1,end) = truckParams.L2;
    hingeGroup = hggroup('Parent',hingeTForm);
    hHinge = plot([0,truckParams.M],[0 0], 'b-', 'LineWidth', 2, 'Parent', hingeGroup);
    
    truckTForm = hgtransform('Parent',hingeTForm);
    truckTForm.Matrix(1,end) = truckParams.M;
    hTruck = plot(truckPoints(:,1), truckPoints(:,2), '-', 'LineWidth', 1, 'Parent', truckTForm);
    
    flTForm = hgtransform('Parent',truckTForm);
    frTForm = hgtransform('Parent',truckTForm);
    flTForm.Matrix(1:2,end) = truckPoints(1,:)';
    frTForm.Matrix(1:2,end) = truckPoints(2,:)';
    hWheelTruckFL = fill(wheelPoints(:,1), wheelPoints(:,2), 'k', 'Parent',flTForm);
    hWheelTruckFR = fill(wheelPoints(:,1), wheelPoints(:,2), 'k', 'Parent',frTForm);
    hWheelTruckBR = fill(wheelPoints(:,1)+truckPoints(3,1), wheelPoints(:,2)+truckPoints(3,2), 'k', 'Parent',truckTForm);
    hWheelTruckBL = fill(wheelPoints(:,1)+truckPoints(4,1), wheelPoints(:,2)+truckPoints(4,2), 'k', 'Parent',truckTForm);
    
    for i = 1:length(x2Sample)
        tic

        x2 = x2Sample(i); 
        y2 = y2Sample(i); 
        th2 = th2Sample(i);
        alpha = steerAngle(i);
        updateVehicle(trailerTForm, hingeTForm, flTForm, frTForm, x2, y2, th2, betaSample(i), alpha);
        
        % Pause to give the illusion of moving objects
    %     pause(dt - toc)
    end
    
end

function updateVehicle(backHandle, frontHandle, leftHandle, rightHandle, x, y, th, beta, alpha)
    backHandle.Matrix(1:2,:) = [cos(th) -sin(th) 0 x; sin(th) cos(th) 0 y];
    frontHandle.Matrix(1:2,1:2) = [cos(beta) -sin(beta); sin(beta) cos(beta)];
    rWheel = [cos(alpha) -sin(alpha); sin(alpha) cos(alpha)];
    leftHandle.Matrix(1:2,1:2) = rWheel;
    rightHandle.Matrix(1:2,1:2) = rWheel;
    drawnow;
end

function newPoints = transformPoints(points, T)
%TRANSFORMPOINTS

% Copyright 2020 The MathWorks Inc.

newPoints = T*[points';ones(1, size(points,1))];
newPoints = newPoints';
newPoints = newPoints(:, 1:2);

end

function points = getRectangleVertices(L, W) 
%getRectangleVertices L - along x, W - along y

% Copyright 2020 The MathWorks Inc.

points = [L/2, W/2;
          L/2, -W/2;
          -L/2, -W/2;
          -L/2, W/2;
          L/2, W/2];

end