% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% function_SetAngle calculates the difference between this instant and the %
% previous one in angle values, so we can identify the rotation of the     %
% image this past instant.                                                 %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %

%  INPUTS:   Name           | Type    | Dimensions | Description
%            angle1         | nAngles | 1xn        | Angle in t-1
%            angle2         | nAngles | 1xn        | Angle in t
%
% OTUPUTS:   Name           | Type    | Dimensions | Description
%            angle          | Angle   | 1x1        | Angle difference between frames

function solution = function_setAngle(angle1, angle2, type, angleTol, RefAngle)
    solution = -60140;
    while(solution == -60140)
        % First we calculate the angle difference in each point
        for i = 1:size(angle1,2)        
            % This will give the angle difference for each point. If the
            % |angle| is greater than 180 we need to add or subtract 360
            % degrees (depending on the sign of the calculated angle)
            angle(i) = angle2(i) - angle1(i);
            if(abs(angle(i)) > 180); angle(i) = angle(i) - 360*sign(angle(i)); end
        end
        % If tolerance is negative then the reference will be the output,
        % otherwise there will be a verification for each angle to understand
        % if the reference and the calculated angle are a match. If they are,
        % then the value will be saved. This will allows multiple angle
        % difference which will reflect in a better kalman filter aproximation
        if(angleTol < 0)
            solution = RefAngle;
        else
            z = 0;
            for i = 1:size(angle,2)
                if(abs(angle(i) - RefAngle) <= angleTol)
                   z = z + 1;
                   solution(z) = angle(i);
                end
            end
        end
        angleTol = angleTol + 0.25;
    end
end