function [aLimitUsed, betaLimit, alphaDesiredEq, alphaGain] = exampleHelperCalculateLQGains(truckParams,Q,R)
%exampleHelperCalculateLQGains Generates lookup table for beta-stabilizing LQ controller
%%
% Begin by linearizing the nonlinear equation of motion for the interior angle 
% about an equilibrium point, $\left(\beta_e \;\alpha_e \right)$. The general 
% formula used to linearize nonlinear models is the following:
% 
% $$\dot{p} =\left(\frac{\delta A\left(p\right)}{\delta p}|\left(p_e \right)+\frac{\delta 
% B\left(p,u\right)}{\delta p}|\left(p_e \;,u_e \right)\;\right)\left(p-p_e \right)+\left(\frac{\delta 
% B\left(p,u\right)}{\delta u}|\left(p_e ,u_e \right)\right)\left(u-u_e \right)$$ 
% 
% and when applied to the equation of motion for the interior angle, we get 
% the following:
% 
% $$\dot{\beta} =\;v\left(-\frac{\cos \left(\beta_e \right)}{L_2 }-\frac{M}{L_1 
% L_2 }\sin \left(\beta_e \right)\tan \left(\alpha_e \right)\right)\left(\beta 
% -\beta_e \right)+v\left(\sec^2 \left(\alpha_e \right)\left(\frac{1}{L_1 }+\frac{M}{L_1 
% L_2 }\cos \left(\beta_e \right)\right)\right)\left(\alpha -\alpha_e \right)$$
% 
% Given a constant steering angle, $\alpha_e$, there exists a constant interior 
% angle, $\beta_e$, such that the centers of curvature for the truck's front/rear 
% axle and trailer rear-axle all lie at the same point. This is defined by the 
% following geometric relationship:
% 
% $$\beta_e =\textrm{sign}\left(\alpha_e \right)\left(\tan^{-1} \left(\frac{M}{R_1 
% }\right)+\tan^{-1} \left(\frac{L_2 }{R_2 }\right)\right)$$
% 
% $$R_1 =\frac{L_1 }{|\tan \left(\alpha_e \right)|}\;,R_2 =\textrm{sqrt}\left(R_1^2 
% +M^2 -L_2^2 \right)$$
% 
% Inserting the equilibrium point back into the linearized system results in 
% $\dot{\beta_e } =0$, and leaves us with the following equations describing the 
% dynamics of the truck/trailer's interior angle:
% 
% $$A\;=-\frac{\cos \left(\beta_e \right)}{L_2 }-\frac{M}{L_1 L_2 }\sin \left(\beta_e 
% \right)\tan \left(\alpha_e \right)$$
% 
% $$B=\sec^2 \left(\alpha_e \right)\left(\frac{1}{L_1 }+\frac{M}{L_1 L_2 }\cos 
% \left(\beta_e \right)\right)$$
% 
% Before defining our control law, we should first define the reasonable limits 
% of our physical system. There are several ways to define these limits:
%% 
% * Define a vehicle-specific $\alpha_{\max }$ and calculate the corresponding 
% $\beta_{\max }$
% * Define a vehicle-specific $\beta_{\max }$ and calculate the corresponding 
% $\alpha_{\max }$
% * Determine the $\beta_{\max }$ based on a constraint in the control law
%% 
% To make things interesting, let's derive our $\beta_{\max }$ such that any 
% reverse motion applied at the steering frame results in a stable interior angle 
% and a non-positive velocity in the trailer frame:
% 
% $$\textrm{Find}\;\beta_{\max } \;|\;\forall v_{\textrm{truck}} <0\to v_{\textrm{trailer}} 
% \le 0\;\textrm{and}\;\dot{\beta} =0$$

% Copyright 2021 The MathWorks Inc. 

    % Extract vehicle geometry
    L1 = truckParams.L1;
    L2 = truckParams.L2;
    M  = truckParams.M;
    
    % Intuitively, for a given geometry, there should exist an interior angle such 
    % that the center of curvature for the truck's rear-axle lies at the center 
    % of the trailer's rear axle. This implies R2 is 0.
    R2Limit = 0;
    
    % Plugging this into equation ___ lets us calculate R1
    R1Limit = sqrt(R2Limit^2 + L2^2 - M^2);
    
    % Requiring that this be an equilibrium point provides alpha and beta
    alphaLimit = abs(atan(L1/R1Limit));
    betaLimit = sign(alphaLimit)*(atan(M/R1Limit)+atan(L2/R2Limit));
%% 
% Let's verify this limit for the given model:

    % Sample steering angles between 0 and 1.25x limit
    alphaDesiredEq = linspace(0,alphaLimit*1.25,1001);
    
    % Calculate steady-state radii
    R1 = L1./abs(tan(alphaDesiredEq));
    R2Squared = R1.^2 + M^2 - L2^2;
    R2 = sqrt(abs(R2Squared)).*sign(R2Squared);
    
    % Define some negative velocity that will be applied at the cabin
    vTruck = -3;
    
    % Calculate interior angle at steady state
    betaEq = sign(alphaDesiredEq).*(atan(M./R1)+atan(L2./R2));
    
    % Apply velocity at truck and plot velocity in trailer-frame
    vTrailer = vTruck*cos(betaEq).*(1+M/L1.*tan(betaEq).*tan(alphaDesiredEq));
    figure;
    plot(alphaDesiredEq,vTrailer,'b-.');
    hold on;
    plot(alphaLimit,vTruck*cos(betaLimit).*(1+M/L1.*tan(betaLimit).*tan(alphaLimit)),'rx');
    title('Trailer Axial Velocity VS Steering Angle');
    xlabel('Desired Steering Angle (rad)');
    ylabel('Axial Velocity (m/s)');
%% 
% With the operating region defined, let us now define our feedback controller. 
% This controller is given $\beta_{\textrm{ref}}$, i.e the virtual steering angle 
% returned by the PurePursuit controller. It then applies a proportional gain 
% to the interior angle which scales the error between the system's current vs 
% equilibrium angle:
% 
% $$\beta_{\textrm{ref}} =\textrm{purePursuit}\left(q_2 ,q_{\textrm{tgt}} \right)$$
% 
% $$\beta_{\textrm{des}} =\beta_{\textrm{ref}} \;+K_{\beta } *\left(\beta -\beta_{\textrm{ref}} 
% \right)$$
% 
% This desired interior angle is transformed to a desired steering angle using 
% the formulae described in the previous section. Lastly, a feedback gain is applied 
% which scales the input based on the distance between the system's current interior 
% angle and the equilibrium angle associated with the desired steering angle:
% 
% $$\alpha_{\textrm{des}} =L\left(\beta_{\textrm{des}} \right)$$
% 
% $$\alpha_{\textrm{cmd}} =\alpha_{\textrm{des}} \;+\alpha_{\textrm{feed}} \;=\alpha_{\textrm{des}} 
% -K\left(\beta -\beta_{\textrm{eq}} \left(\alpha_{\textrm{des}} \right)\right)$$
% 
% An LQR controller is used to select appropriate feedback gains. We first sample 
% the steering limits and then solve for the feedback gain that minimizes the 
% following objective function, using eq __ & __ for our A and B matrices:
% 
% $$\min_{\bar{u} } \;J=\int_0^{\infty } {\bar{p} }^T Q_1 \bar{p} +{\bar{u} 
% }^T Q_2 \bar{u} \;\textrm{dt}=\int_0^{\infty } {\left(\beta -\beta_{\textrm{eq}} 
% \right)}^2 Q+{\left(\alpha -\alpha_{\textrm{eq}} \right)}^2 \;\textrm{dt}$$

    % Sample the steering limits for desired steering angles
    aLimitUsed = alphaLimit*.99;
    alphaDesiredEq = linspace(-aLimitUsed,aLimitUsed,101);
    
    % Calculate the internal equilibrium angle for each desired steering angle
    R1 = L1./abs(tan(alphaDesiredEq));
    R2Squared = R1.^2 + M^2 - L2^2;
    R2 = sqrt(abs(R2Squared)).*sign(R2Squared);
    betaEq = sign(alphaDesiredEq).*(atan(M./R1)+atan(L2./R2));
    
    % Define the A and B matrices
    ABar = -1./L2.*(cos(betaEq) + M./L1.*sin(betaEq).*tan(alphaDesiredEq));
    BBar = 1./L1.*(1+M./L2.*cos(betaEq)).*(1+tan(alphaDesiredEq).^2);
    
    % Iterate through all state/steering pairs and find an appropriate gain for the linearized system
    alphaGain = nan(size(alphaDesiredEq));
    for i = 1:numel(alphaGain)
        alphaGain(i) = lqr(ABar(i),BBar(i),Q,R);
    end

    % Plot the relationship between the desired steering angle and the gain-scheduled feedback
    figure
    plot(alphaDesiredEq,alphaGain);
    title('LQ Gain vs Desired Steering Angle');
    xlabel('Desired Steering Angle (rad)');
    ylabel('LQR Gain');
    drawnow
end