function [alpha, refDist, pastTarget] = exampleHelperPurePursuitGetSteering(q, truckParams, controlParams, refDist, waypoints, driveDir)
%exampleHelperPurePursuitGetSteering Calculate steering angle using Pure Pursuit 

% Copyright 2021 The MathWorks Inc. 

    if driveDir == 1
        L = truckParams.L1; % Characteristic Length
    else
        L = truckParams.L2;    
    end
    
    if driveDir
        R = controlParams.ForwardLookahead;
        minRad = sqrt(truckParams.L2^2+truckParams.M^2);
        maxSteer = pi/4;
        qFull = exampleHelperExpandToFullState(q, truckParams.L2, truckParams.M);
        
        % Extract pose
        x = qFull(1);
        y = qFull(2);
        theta = qFull(3);
    else
        maxSteer = truckParams.BetaLimit;
        R = controlParams.ReverseLookahead;
        minRad = 0;
        x = q(1);
        y = q(2);
        theta = q(3);
    end

    vRef = diff(waypoints,[],2);
    p2pDist = norm(vRef);
    
    [P,refDist,projDist,intersectsFlag] = exampleHelperCalculateLookaheadPoint([x,y,theta],refDist,R,waypoints,minRad);
    
    vLook = [P(1)-x P(2)-y];
    
    thetaError = atan2(vLook(2),vLook(1)) - theta;
    if isnan(thetaError) || abs(abs(thetaError)-pi) < 1e-6 || (~driveDir && thetaError == 0 && refDist < p2pDist)
        steeringAngle = maxSteer;
    else
        steeringAngle = atan(2*L*sin(thetaError)/R);
    end
    
    pastTarget = abs(projDist) > p2pDist;
    
    if refDist > p2pDist && ~pastTarget
        % If the lookahead point lies beyond the goal, but the vehicle has
        % not yet passed by the goal, calculate the turn required to reach
        % the goal.
        P = waypoints(:,2)';
        vLook = [P(1)-x P(2)-y];
        thetaError = atan2(vLook(2),vLook(1)) - theta;
        steeringAngleGoal = atan(2*L*sin(thetaError)/R);
        
        if abs(steeringAngleGoal) < maxSteer
            % Goal point still lies outside the min-turn envelope, set goal
            % point as lookahead point.
            steeringAngle = steeringAngleGoal;
        end
    end
    
    if driveDir 
    % Forward motion
        alpha = min(maxSteer,abs(steeringAngle))*sign(steeringAngle);
    else
    % Reverse motion

        % Steering angle is a virtual target for the interior angle, betaDes.
        betaDes = sign(steeringAngle)*min(abs(steeringAngle),maxSteer);
        
        % Convert this to a desired angle at steering wheel
        alphaDes = virtualToActualSteering(betaDes, truckParams.L1, truckParams.L2, truckParams.M);
        
        % Keep within controllable limits
        alphaDes = min(abs(alphaDes),truckParams.AlphaLimit)*sign(alphaDes);
        
        % Calculate equilibrium angle corresponding to desired steering angle
        betaEq = computeEquilibriumInternalAngle(alphaDes,truckParams);

        % Calculate error between equilibrium/current interior angle
        betaError = betaEq-q(4);

        % Calculate and apply feedback gain to desired steering angle
        pGain = interp1(controlParams.AlphaPoints, controlParams.Gains, alphaDes);
        alpha = alphaDes - pGain*(betaError);
        
        % Keep within controllable limits
        alpha = min(abs(alpha),truckParams.AlphaLimit)*sign(alpha);
    end
end

function alphaDes = virtualToActualSteering(beta, L1, L2, M)
%virtualToActualSteering Convert trailer's virtual steer command to truck steering angle
%
%    This function was generated by the Symbolic Math Toolbox version 8.6.
%    13-Aug-2020 15:29:19

    t2 = sign(beta);
    t3 = L2.^2;
    t4 = beta.*t2;
    t5 = tan(t4);
    t6 = t5.^2;
    t7 = t6+1.0;
    alphaDes = t2.*atan(-(L1.*M.*t5-sqrt(t7).*abs(t5).*sqrt(L1.^2.*t3))./(t3.*t7-M.^2));
end

function betaEq = computeEquilibriumInternalAngle(alphaEq, params)
%computeEquilibriumInternalAngle Finds the equilibrium position for the internal
%angle beta as a function of the constant desired steering angle alpha_eq
%and the geometry of the system

    L1 = params.L1; % Truck axle to axle length
    M = params.M; %Truck rear axle to hitch point
    L2 = params.L2; % Hitch point to trailer axle

    R1 = L1./abs(tan(alphaEq));
    R2 = sqrt(R1.^2 + M.^2 - L2.^2);

    betaEq = sign(alphaEq).*(atan(M./R1) + atan(L2./R2));
end