function exampleHelperShowLineFeaturesInScan(s1, s2, info, initGuess)
%EXAMPLEHELPERSHOWLINEFEATURESINSCAN Draw line features on top of scan pair plot
%   This example helper shows how to use the dubugInfo struct returned by
%   matchScansLine.

initGuess = initGuess(:);

s2t = transformScan(s2, initGuess);
EA = transformLineFeature(info.CurrentFeatures, initGuess);
scanSegMskB = info.CurrentScanMask;
FA = info.ReferenceFeatures;
scanSegMskA = info.ReferenceScanMask;

f1 = figure;
set(f1, 'color', [1 1 1]);
ax1 = subplot(1,2,1);
s1.plot
hold on
drawLinesLocalMsk(FA, s1, scanSegMskA, ax1, 1)
title('s1 Line Features')

ax2 = subplot(1,2,2);
s2t.plot
hold on
ax2.ColorOrderIndex = 2;
drawLinesLocalMsk(EA, s2t, scanSegMskB, ax2, 1)
title('s2 Line Features - Transformed')

end


function drawLinesLocalMsk(lineFeatures, scan, segMentMsk, ax, lineWidth)
%DRAWLINESLOCALMSK
hold on

if nargin < 7
    lw = 1;
else
    lw = lineWidth;
end

nF = size(lineFeatures,1);

points = scan.Cartesian;

for k = 1:nF
    rho = lineFeatures(k,1);
    alpha = lineFeatures(k,2);
    
    ids = find(segMentMsk(k,:));
    xLower = points(ids(1), 1);
    xUpper = points(ids(end), 1);
    yLower = points(ids(1), 2);
    yUpper = points(ids(end), 2);

    x = (rho - yUpper * sin(alpha))/cos(alpha);
    p1 = [x, yUpper];
    y = (rho - xUpper * cos(alpha))/sin(alpha);
    p2 = [xUpper, y];
    x = (rho - yLower * sin(alpha))/cos(alpha);
    p3 = [x, yLower];
    y = (rho - xLower * cos(alpha))/sin(alpha);
    p4 = [xLower, y];    
    %P = [p1;p2;p3;p4];
    if abs(alpha - pi/2) < pi/4 || abs(alpha + pi/2) < pi/4
        P = [p2;p4];
    else
        P = [p1;p3];
    end
    plot(ax, P(:,1), P(:,2), 'x-', 'LineWidth', lw);
    for i = 1:size(P,1)
        text(P(i,1), P(i,2), num2str(k), 'FontSize',14 );
    end
end
hold off


end

function EA = transformLineFeature(EB, odomAB)
%TRANSFORMLINEFEATURE

num = size(EB,1);
EA = zeros(size(EB));
for i = 1:num
    ln = EB(i,:);
    rho2 = ln(1);
    theta2 = ln(2);
    x1 = odomAB(1);
    y1 = odomAB(2);
    phi1 = odomAB(3);
    EA(i,:) = [x1*cos(theta2 + phi1) + y1*sin(phi1 + theta2) + rho2, ...
               robotics.internal.wrapToPi(theta2 + phi1)];
end

end