% Load example data
load scans_2D iteration

% Get number of scans
numIterations = length(iteration);

% SLAMICP parameters
inlierTh = 0.3;             % Scalar. Max distance to consider a point an inlier (-1 = all).
initialPos = [0,0,0];       % Initial position guess vector. [x, y, yaw]
method = 'point_to_plane';  % String. Matching strategy String 'point_to_point' or 'point_to_plane'.
maxIter = 20;               % Scalar. Maximum number of iterations.  
Normals = [];               % Normals of M. Use [] if unknown.
outlierTh = 0.1;            % Threshold for detecting outliers of M in output.

% Initialize M
M = iteration{1}.scan;

% Initialize plots
figure();
map1Ax = axes;

% Initialize empty objects
mapObj = [];

% Main loop
for itID = 2:1:numIterations
    % Get a new Template scan
    T = iteration{itID}.scan;
    
    % Run SLAMICP
    [initialPos, ~, ~, ~, outliersIndx, ~, M, Normals] = SLAMICP(M, T, inlierTh, method, maxIter, initialPos, Normals, outlierTh);
    
    % Update map
    mapObj = updateMap(map1Ax, mapObj, M, outliersIndx);
    
    % Process GUI callbacks
    drawnow;
    
    % Show progress
    disp(['Iteration ',num2str(itID),'/',num2str(numIterations)]);
end


function mapObj = updateMap(mainAx, mapObj, points, ol)
    if isempty(mapObj)
        mapObj.map = plot(mainAx, points(:,1), points(:,2),'b.');
        hold(mainAx,'on');
        if isempty(ol)
            mapObj.ol = plot(mainAx, nan, nan,'r.');
        else
            mapObj.ol = plot(mainAx, ol(:,1), ol(:,2),'r.', 'MarkerSize',30);
        end
        hold(mainAx,'off');
        xlabel(mainAx,'x (m)');
        ylabel(mainAx,'y (m)');
        axis(mainAx, 'equal');
        grid(mainAx,'on')
        grid('minor')
    else
        mapObj.map.XData = points(:,1);
        mapObj.map.YData = points(:,2);
        
        if isempty(ol)    
            mapObj.ol.XData = nan;
            mapObj.ol.YData = nan;                
        else
            mapObj.ol.XData = ol(:,1);
            mapObj.ol.YData = ol(:,2);                
        end
    end
end