function distanceHeuristic = exampleHelperCalculateDistanceMetric(truckParams, controlParams)
%exampleHelperCalculateDistanceMetric Generates lookup table used for distance estimation
%%
% The cost of a trajectory is defined as follows:
% 
% $$\Gamma_{\textrm{rev}} =\sum_{i\;=\;1} 10{\left(x_2 \left\lbrack i\right\rbrack 
% -x_2 \left\lbrack i-1\right\rbrack \right)}^2 +10{\left(y_2 \left\lbrack i\right\rbrack 
% -y_2 \left\lbrack i-1\right\rbrack \right)}^2 +{10}^4 {\left(\beta \left\lbrack 
% i\right\rbrack -\beta \left\lbrack i-1\right\rbrack \right)}^2$$
% 
% $$\Gamma_{\textrm{fwd}} =\sum_{i\;=\;1} 10{\left(x_1 \left\lbrack i\right\rbrack 
% -x_1 \left\lbrack i-1\right\rbrack \right)}^2 +10{\left(y_1 \left\lbrack i\right\rbrack 
% -y_1 \left\lbrack i-1\right\rbrack \right)}^2 \;$$
% 
% We also define a function, <./exampleHelperTargetReachedFunction.m |exampleHelperTargetReachedFcn|>, 
% which tests whether a configuration is within an acceptable region surrounding 
% a goal state, $\left(\Delta r,\Delta \theta_2 ,\Delta \beta \right)=\left(0\ldotp 
% 9,\;0\ldotp 1,\;0\ldotp 1\right)$:
% 
% $$||{\textrm{xy}}_2 \left(t_f \right)\;-{\textrm{xy}}_{\textrm{goal}} ||\le 
% {\Delta r}^2$$
% 
% $$|\theta_2 \left(t_f \right)-\theta_{\textrm{goal}} |\le \Delta \theta_2$$
% 
% $$|\beta \left(t_f \right)-\beta_{\textrm{goal}} |\le \Delta \beta$$
% 
% To evaluate our cost function, we sample a grid of (x,y) points relative to 
% the rear-axle of the frame being controlled. The vehicle starts in a default 
% configuration, $\alpha =0,\;\beta =0$, and is propagated towards the target 
% until the time limit has been reached. For the sake of the distance 
% metric, we relax the criteria such that the target is "reached" if the 
% vehicle comes within 0.9m of the target-state at any point in the 
% trajectory. In this event, the cost of the trajectory is evaluated using 
% <./exampleHelperComputeTrajectoryCost.m |exampleHelperComputeTrajectoryCost|>, 
% otherwise a worst-case cost is assigned:
%
% Copyright 2021 The MathWorks Inc.

    % Define the reachability grid
    xRange = -90:5:90;
    yRange = -90:5:90;
    [XX,YY] = meshgrid(xRange,yRange);
    
    % Allocate cost matrix
    cost = repmat({nan(size(XX))},2,1);
    
    % Define initial conditions for forward/reverse motion
    lookaheadDist = [controlParams.ForwardLookahead,controlParams.ReverseLookahead];
    Q0 = zeros(2,6);
    Q0(2,:) = [0 0 0 0 0 0]; % Reverse state is trailer rear-axle
    Q0(1,1) = -(truckParams.L2+truckParams.M); % Initial state shifted
    
    % Initialize target state
    qTgt = zeros(1,6);
    betaExpect = 0;
    
    % For the offline evaluation of our distance matrix we will use a much higher control
    % horizon, which will improve the chances that our forward simulation
    % reaches the target.
    maxOfflineSteps = 1000;
    tgtPose = zeros(numel(XX),6,2);
    qFinal  = zeros(numel(XX),6,2);
    f = zeros(2,1);
    R = [controlParams.ForwardLookahead; controlParams.ReverseLookahead];
    t = {'Forward','Reverse'};
    for DIR = 1:2
        % Define initial state and reference control, [xInit, yInit, maxLookaheadDist]
        q0 = Q0(DIR,:);
        uRef = [0 0 controlParams.MaxVelocity*(-1)^(DIR+1) lookaheadDist(DIR)];
        qTgt(4:5) = [betaExpect (-1)^(DIR+1)];
        
        XData = [];
        YData = [];
        ZData = [];
        
        for i = 1:numel(XX)
            % Define target for grid configuration
            th = atan2(YY(i),XX(i));
            if DIR == 2
                th = th-sign(th)*pi;
            end
            
            qTgt(1:3) = [[XX(i) YY(i)] th];
            tgtPose(i,:,DIR) = qTgt;
            
            if abs(th) > 3*pi/4 || norm(q0(1:2) - qTgt(1:2),2) < 20
                % Skip states that lie too far in the opposite direction.
                continue;
            end
            
            % Propagate system
            [Q,~,~] = exampleHelperPropagateTruck(truckParams, controlParams, controlParams.StepSize, q0, uRef, qTgt, maxOfflineSteps);
            
            % Check if trajectory reached the goal region
            gReached = sum((Q(:,1:2)-qTgt(:,1:2)).^2,2) < 0.9^2;
            if any(gReached)
                idx = find(gReached,1,'first');
                traj = Q(1:idx,:);
                % Calculate trajectory cost
                [cost{DIR}(i), accumulatedCost] = exampleHelperComputeTrajectoryCost(traj);
                accumulatedCost = accumulatedCost(1:size(traj,1),:);
            else
                % Cost is zero for visualization purposes only
                traj = Q;
                accumulatedCost = zeros(size(traj,1),1);
            end
            % Store trajectories for visualization
            XData = [XData(:); nan; traj(:,1)];
            YData = [YData(:); nan; traj(:,2)];
            ZData = [ZData(:); nan; accumulatedCost];
            qFinal(i,:,DIR) = traj(end,:);
        end
        % Display trajectories
        figure;
        
        subplot(1,2,1);
        hold on;
        title([t{DIR} ' Simulations']);
        xlabel(['x' num2str(DIR)]);
        ylabel(['y' num2str(DIR)]);
        zlabel('Cost');
        plot3(XData,YData,ZData,'b');
        xError = [qFinal(:,1,DIR) tgtPose(:,1,DIR) nan(numel(XX),1)]';
        yError = [qFinal(:,2,DIR) tgtPose(:,2,DIR) nan(numel(XX),1)]';
        plot(xError(:),yError(:),'r');
        legend({'Trajectories','XY Error'});
        axis('square')
        
        subplot(1,2,2);
        a = imagesc(cost{DIR},'XData',xRange([1 end]),'YData',yRange([1 end]));
        axis equal;
        a.CDataMapping = 'scaled';
        title([t{DIR} ' Cost']);
        xlabel(['x' num2str(DIR)]);
        ylabel(['y' num2str(DIR)]);
        colorbar();
        drawnow;
    end
    
    % Store results as lookup table in control parameter struct
    distanceHeuristic.XPoints = xRange;
    distanceHeuristic.YPoints = yRange;
    distanceHeuristic.ForwardCost = cost{1};
    distanceHeuristic.ReverseCost = cost{2};
end