% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% 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_setAngle2(angle1, angle2, type, angleTol, RefAngle)
        % First we calculate the angle difference in each point
        solution(1) = RefAngle;
        t = 1;
        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
            if(abs(angle(i) - RefAngle) <= angleTol)
               t = t + 1;
               solution(t) = angle(i);
            end
        end
end