% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% This function will help understand what is happening inside the script,  %
% using plots and text to do do. Essentially we can divide this debug in   %
% four levels:                                                             %
%   % Level 00: No Debug     - As if this function was never called        %
%   % Level 01: Basic Debug  - This will show two consequent frames,       %
%   %                          give them titles, that are associated with  %
%   %                          the iteration that is happening, the number %
%   %                          of valid points (if the distance method is  %
%   %                          beeing used) amongst the obtained matched   %
%   %                          points, as well has drawing the Centroid    %
%   %                          (if that option is true - see CONFIG_DATA)  %
%   % Level 02: Medium Debug - This level is somewhat similar to the last  %
%   %                          one in what concerns the titles, however    %
%   %                          there is a big change in this level. The    %
%   %                          matched points will be presented one by one %
%   %                          to see if they really are a match           %
%   % Level 03: High Debug   - This level is similar to the last one,      %
%   %                          however in this level there will appear a   %
%   %                          text next to each matched point, saying in  %
%   %                          what coordinates it is located in, as well  %
%   %                          the angle that it makes with the reference  %
%   %                          point. In this level we will also be able   %
%   %                          to see both the average and median          %
%   %                          reference angle, so we can better           %
%   %                          understand the results we are getting.      %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% In the CONFIG_DATA is also possible to change the appeareance of some    %
% plot definition, for example the type and color of the centroid marker   %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %

function function_Debug(Level, Points1, Points2, indexPairs, Centroid, showCentroid, pauseTime, pauseFrame, centroidStr, pointsStr, colorStr, i, function_initPic)

    imageT1 = rgb2gray(imread(sprintf('rosbag/frame/frame%04d.jpg',i-1)));
    imageT2 = rgb2gray(imread(sprintf('rosbag/frame/frame%04d.jpg',i)));
    
    ANG1         = function_calculateAngle(Centroid(1,:), Points1);
    ANG2         = function_calculateAngle(Centroid(2,:), Points2);
    angleAverage = function_setAngle(ANG1, ANG2, true);
    angleMedian  = function_setAngle(ANG1, ANG2, false);

    if (Level > 0)
    % LEVEL 01
    %======================================================================
    % This Debug Shows the image and corresponding matched points all in
    % one image (all the points in one image)
    %======================================================================
        % Level 1
        % Draws the image in both subplots and adds the titles and labels
        figure(1)
        subplot(1,2,1)
        imshow(imageT1); hold on; grid on;
        title(sprintf('Iteration %04d', i-function_initPic))
        xlabel(sprintf('Nbr. Of Valid Points is: %03d in %03d\nAverage Angle is %03.2fº', size(Points1,1), size(indexPairs,1), angleAverage))
        if(showCentroid); plot(Centroid(1,1), Centroid(1,2), centroidStr); end

        subplot(1,2,2)
        imshow(imageT2); hold on; grid on;
        title(sprintf('Iteration %04d', i-function_initPic+1))
        xlabel(sprintf('Nbr. Of Valid Points is: %03d in %03d\nMedian Angle is %03.2fº', size(Points2,1), size(indexPairs,1), angleMedian))
        if(showCentroid); plot(Centroid(2,1), Centroid(2,2), centroidStr); end

        
        if (Level > 1)
        % LEVEL 02
        %======================================================================
        % This Debug Shows the image and corresponding matched points one by
        % one
        %======================================================================
            for i = 1:size(Points1,1)
                % Level 2
                % Draws each single points (in i)
                subplot(1,2,1)
                imshow(imageT1); hold on; grid on;
                plot(Points1(i,1), Points1(i,2), pointsStr);
                if(showCentroid); plot(Centroid(1,1), Centroid(1,2), centroidStr); end
                
                subplot(1,2,2)
                imshow(imageT2); hold on; grid on;
                plot(Points2(i,1), Points2(i,2), pointsStr);
                if(showCentroid); plot(Centroid(2,1), Centroid(2,2), centroidStr); end
                
                if (Level > 2)
                % LEVEL 03
                %======================================================================
                % This Debug Shows the image and corresponding matched points one by
                % one as well as the angle made between them and corresponding
                % coordinates for each point
                %======================================================================
                    % Prevents the calculation of angle for equivalent
                    % points
                    if ((Centroid(1,1) ~= Points1(i,1)) || (Centroid(1,2) ~= Points1(i,2))) 
                        angle1 = function_calculateAngle(Centroid(1,:), Points1(i,:));
                        angle2 = function_calculateAngle(Centroid(2,:), Points2(i,:));
                        angle = angle2 - angle1;
                        if(abs(angle > 180)); angle = angle - 360*sign(angle); end
                        
                        subplot(1,2,1)
                        title(sprintf('Average Angle is %03.2fº', angleAverage))
                        text(Points1(i,1), Points1(i,2), sprintf('< = %03.2fº\n(%03.0f,%03.0f)', angle1, Points1(i,1), Points1(i,2)), 'Color', colorStr, 'FontWeight', 'bold');
                        if(showCentroid); text(Centroid(1,1), Centroid(1,2), sprintf('(%03.0f,%03.0f)', Centroid(1,1), Centroid(1,2)), 'Color', colorStr, 'FontWeight', 'bold'); end
                        xlabel(sprintf('Angle Difference is %03.2fº', angle));
                        
                        subplot(1,2,2)
                        title(sprintf('Median Angle is %03.2fº', angleMedian))
                        text(Points2(i,1), Points2(i,2), sprintf('< = %03.2fº\n(%03.0f,%03.0f)', angle2, Points2(i,1), Points2(i,2)), 'Color', colorStr, 'FontWeight', 'bold');
                        if(showCentroid); text(Centroid(2,1), Centroid(2,2), sprintf('(%03.0f,%03.0f)', Centroid(2,1), Centroid(2,2)), 'Color', colorStr, 'FontWeight', 'bold'); end
                        xlabel(sprintf('Angle Difference is %03.2fº', angle));
                    end
                end
            % pause for level 2 and 3    
            pause(pauseTime)
            if(pauseFrame); pause; end
            clf    
            end
        end
        
        
        % Level 1
        % Draws all the points
        subplot(1,2,1)
        plot(Points1(:,1), Points1(:,2), pointsStr)
        if(showCentroid); plot(Centroid(1,1), Centroid(1,2), centroidStr); end
        subplot(1,2,2)
        plot(Points2(:,1), Points2(:,2), pointsStr)
        if(showCentroid); plot(Centroid(2,1), Centroid(2,2), centroidStr); end

        pause(pauseTime)
        if(pauseFrame); pause; end
        clf
    end
end