% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% function_GetMatchedPoints will return the correspondig feature points    %
% between two pictures. There is essentially two ways to do it, every      %
% point matched is a valid point (the inputed distance is < zero) or every %
% point must have a distance between any other point in both frames with a %
% difference lesser or equal to disntance.                                 %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %

%  INPUTS:   Name           | Type    | Dimensions | Description
%            angle          | nangles | nx2        | Points detected in image
%            indexPairs     | table   | jx2        | Table of points matched
%            distance       | int     | 1x1        | max difference in distance between two points in two frames
%            n              | int     | 1x2        | nbr of minimum needed points
%            t              | int     | 1x1        | current time frame
%
% OTUPUTS:   Name           | Type    | Dimensions | Description
%            Points1        | nPoints | jx2        | features in t-1
%            Points2        | nPoints | jx2        | features in t

function [Points1, Points2] = function_GetMatchedPoints(angle, indexPairs, distance, n, t)
    NOK = true;
    if(distance > 0)
        % Will test each point (until it finds) a point j that has a minimal
        % number of elements. Those elements are earned by meeting some
        % dimension restrictions. If the distance in both frames is more or
        % less the same, then the points is added. Notice that if we try to do
        % this test in a displaced point, then it will almost everytime fail in
        % the requirement.
        j = 1;
        % This cycle will run while a certain condition is not met. The
        % condition is that Points1 must have at least 3 points (excluding
        % the point that is beeing tested)
        while(NOK) 
            z = 1;
            for k = 1:size(indexPairs,1)
                if(j ~= k)
                    % This is the point that is beeing checked for
                    % validation. Therefore, this point (j) will be
                    % compared with every other point matched previously to
                    % check its validation.
                    PT1 = [angle(indexPairs(j,1), 2:3, t-1)
                           angle(indexPairs(j,2), 2:3,   t)];
                    % Every other points (than j) that will try to measure
                    % distance with j in order to test that same point.
                    PT2 = [angle(indexPairs(k,1), 2:3, t-1)
                           angle(indexPairs(k,2), 2:3,   t)];
                    % If the distance between those two points is less than
                    % a threshold, then they both must be valid feature
                    % points inboth frames, therefore we should save them
                    % into an array
                    if(function_CompareDistance(PT1, PT2, distance))
                        Points1(z,1) = angle(indexPairs(k,1), 2, t-1);
                        Points1(z,2) = angle(indexPairs(k,1), 3, t-1);
                        Points2(z,1) = angle(indexPairs(k,2), 2,   t);
                        Points2(z,2) = angle(indexPairs(k,2), 3,   t);

                        z = z + 1;
                    end
                end
            end
            % This will eliminate any possible rows that may have been
            % miscalculated. If a row has only zeros in it, it will be
            % deleted. This error can ocurr in cases that the assignment of
            % the row was not made correctly.
            Points1( ~any(Points1,2), : ) = [];
            Points2( ~any(Points2,2), : ) = [];
            % If the number of valid points (excluding the point that is
            % beeing tested is superior to n, then it is possible to
            % consider that this particular points is a valid feature
            % representation in both frames, therefor we can add it to the
            % point list and end the cycle.
            if(size(Points1,1) >= n)
                Points1(z,1) = angle(indexPairs(j,1), 2, t-1);
                Points1(z,2) = angle(indexPairs(j,1), 3, t-1);
                Points2(z,1) = angle(indexPairs(j,2), 2,   t);
                Points2(z,2) = angle(indexPairs(j,2), 3,   t);
                NOK = false;
            else
            % If the condition is not met then we need to continue the
            % cycle. For that we will increment j by one. However if we get
            % to a point where no j will met the condition specified above,
            % we need to change some of its condition and restart. In this
            % case the amount of distance difference between frames will
            % increment by 0.5;
                j = j + 1;
                if(j > size(indexPairs,1))
                    j = 1;
                    distance = distance + 0.5;
                end
            end
        end
    else
        % In this case, since the distance is less than zero, the script
        % assumes that the user does not mean to use the method described
        % earlier but a simpler one. Essentially, all the points that were
        % matched will be used in this case.
        for j = 1:size(indexPairs,1)
            Points1(j,:) = angle(indexPairs(j,1), 2:3, t-1);
            Points2(j,:) = angle(indexPairs(j,2), 2:3,   t);
        end
    end
end