% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% In this function, the frames obtained from the camera will be analysed   %
% and compared in order to find features that exist in consecutive frames. % 
% Once this is accomplished, this script will calculate an angle between   %
% all the features in the previous frame and in the current one in order   %
% to find the difference in both frames, and thus, the rotation suffered   %
% in this instace. In the end, adding all this local rotations will give   %
% the orientation of the camera through time, rather than its rotation     %
% each instant.                                                            %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %

function [OneFeatureSolution] = function_CreateFeatureVisualData2(config, exp_nbr)
    % This will load the instant in nanoseconds of when we obtained the
    % frames. After dividing for 1e9 we obtain the absolute time (which we
    % will work with) in seconds. Since the frames numberations starts at 0
    % but after loading the time, it starts at 1, we have to add 1 to the
    % time,i.e, time(1) is refering to frame0000
    temp1 = csvread(sprintf('data/rosbag_Article%d/camera.csv', exp_nbr),1,0);
    time  = temp1(config(5,1)+1: config(6,1)+1,1)/1e9;
    % This variable is used to calculate the first angular velocity.
    previous_value_of_time = temp1(config(5,1),1);

    % This for cycle will run through all the frames that correspond to the
    % experiment. Note that i may begin at a different number than one.
    % Because of this there will be a different variable t, that will be
    % calculated in order that it always is one in the first frame, and
    % then is is incremented.
    t = 0;
    for i = config(5,1):config(6,1)-1  
        t = t+1;
        % Atribution of Time to current frame and initialization of the
        % variable (every time that the angle variation is equal to -60140
        % then this value will be useless and meaningless. (Is similar to
        % NaN, but this cant be used because NaN + int = NaN)
        angleDiff(t,1) = time(t,1);
        if(t==1); angleDiff(1,1000) = 0; end
        % Loading of the image and detection of points of interest (in this
        % case, SURF Features). All this points of interest will be added
        % to the variable angle, which will save all this points throuht
        % time, in order they can be acessed always that it's needed.
        image = imadjust(rgb2gray(imread(sprintf('data/rosbag_Article%d/frame/frame%04d.jpg',exp_nbr,i))));
        Points = detectSURFFeatures(image);
        for a = 1:size(Points.Location,1)
            for b = 1:size(Points.Location,2)
                angle(a,b+1,t) = Points.Location(a,b);
            end
        end


        % If we are looking at the first frame, then we just need to
        % extract the features, however, if that's not the case, we need in
        % adition to compare them, and calculate the angle rotation between
        % frames
        if (t == 1)
            [features1] = extractFeatures(image,Points);
        else
            [features2] = extractFeatures(image,Points);
            indexPairs  = matchFeatures(features1,features2);
            features1   = features2;
            
            
            PointsFeature1 = [];
            PointsFeature2 = [];
            % Will go to the variable angle and extract the matched points
            % presented in indexPairs (matchFeatures). Notice that
            % angle(a,b,c) means the following:
            %   a - nbr of features detected
            %   b - 1 is time
            %       2 is x
            %       3 is y
            %   c - is the number of the frame
            for j = 1:size(indexPairs,1)
                PointsFeature1(j,:) = angle(indexPairs(j,1), 2:3, t-1);
                PointsFeature2(j,:) = angle(indexPairs(j,2), 2:3,   t);
            end
            
            
            % This will extract the first matrix transformation. From this
            % matrix we can extract the rotation.
            tform     = estimateGeometricTransform(PointsFeature1, PointsFeature2, 'projective');
            angleDiff(t,2) = atan(tform.T(2,1)/tform.T(1,1)) * 180 / pi;
            %angleDiff(t,2) = asin(tform.T(2,1)) * 180 / pi;
            
            
            % This condtion will allow (or not) the increase of data to
            % the calculated transformations. 
            %      If false, the output will be of nx3, where n is the
            % number of frames, 1 is the time, 2 is the angular orientation
            % and 3 is the angular velocity. This comes from
            % estimateGeometricTransform.
            %      If true, the output will add to this values, the
            % measures obtained from each individual point
            if(config(14,1))
                % This function will calculate the centroid
                Centroid     = function_calculateCentroid(config(16,1), PointsFeature1, PointsFeature2);
                % This function will calculate the angle between two (or more)
                % points - beeing that the first point will always be the same
                FeatureAngle1 = function_calculateAngle(Centroid(1,:), PointsFeature1);
                FeatureAngle2 = function_calculateAngle(Centroid(2,:), PointsFeature2);
                % This function will calculate the difference between the
                % calculated angles using one of two methods (either using the
                % average or the median as a reference and then using a
                % tolerance to find out which of these angles are in the range
                % that the user thinks its admissable).
                temp = function_setAngle2(FeatureAngle1, FeatureAngle2, config(18,1), config(17,1), angleDiff(t,2));
                if(size(temp,2)+2 > size(angleDiff,2))
                    angleDiff(t, 3:end) = temp(1:size(angleDiff,2)-2);
                else
                    angleDiff(t, 3:size(temp,2)+2) = temp(:);
                    angleDiff(:,size(temp,2)+2:end) = [];
                end
            end
        end
    end
    
    
    reset = zeros(1,size(angleDiff,2));
    % Creation of Final Solution (size de <Diff*2 - 1)
    OneFeatureSolution = zeros(size(angleDiff,1),2*size(angleDiff,2)-1);
    % Assigning of the first values for every column
    OneFeatureSolution(1,1) = angleDiff(1,1);
    for a = 2:(size(angleDiff,2))
        OneFeatureSolution(1,(a-1)*2  ) = angleDiff(1,a);
        OneFeatureSolution(1,(a-1)*2+1) = angleDiff(1,a) / (OneFeatureSolution(1,1) - previous_value_of_time);
    end
    
    
    % Assigning of remaining values for every column
    for i = 2:size(angleDiff,1)
        for j = 2:(size(angleDiff,2))
            OneFeatureSolution(i,1) = angleDiff(i,1);
            OneFeatureSolution(i,(j-1)*2  ) = OneFeatureSolution(i-1,(j-1)*2) + angleDiff(i,j);
            OneFeatureSolution(i,(j-1)*2+1) = angleDiff(i,j) / (OneFeatureSolution(i,1) - OneFeatureSolution(i-1,1));

            if(abs(angleDiff(i,j)) < 0.5); reset(j) = reset(j) + 1; else reset(j) = 0; end
            if(reset(j) == 2); reset(j) = 0; OneFeatureSolution(i,(j-1)*2) = 0; end
        end
    end
    OneFeatureSolution( :, all( ~any( OneFeatureSolution ), 1 ) ) = []; % Clears Columns with all zeros
end