%%   MASTER THESIS   %%
%==================== %
%        JOÃO PEIXOTO %
%               60140 %
%  joao.peixoto@ua.pt %
%                2016 %
%==================== %


%% CHAPTER 4 - Visual Experiments
%     In this script, the main objective is to try to find blobs in a set
%     of frames, and try to extract the angular position of the camera at
%     each instant. Since this was one of the first experiments done, the
%     angular velocity calculation was not added.


%% 00 Clean Workspace and Global Variables
% To start, we clean the worskspace and define the global variables. In this
% case, the global variables are only meant to decide if something is shown
% or not.
for chapter = 0:0
    clc
    clear all
    close all
% ================================================= %
% =============== Program Variables =============== %
config       = CONFIG_DATA_1;                       % Choose the experiment configuration 
exp_nbr      =             1;                       % Choose the experiment number
aux          =             0;                       % Variable to help the indexing
show_results =          true;                       % To view results (true)
% ================================================= %
% ================================================= %
% Notice that if you try to run this script with the second set of data,
% nothing will happen, since there are no blobs to detect.
end
%% 01 Load Results
% Once the global variables are defined, we need to load and treat the data
% in order for us to study it
for chapter = 1:1
    [cart, filt]=function_inertialData(config, exp_nbr);
    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;
end
%% 02 Visual Data Extraction
% In this chapter we will load every frame, one by one and search it for
% blobs. If we find them, we will extract its orientation, centroid
% location and area. In the end, we organize the visual data, so that the
% biggest area blobs are the first to be displayed in the variable, and we
% will eliminate resutls of blobs that are not trustworthy.
for chapter = 2:2
    % Loads all pictures
    for k = config(5,1):config(6,1)
        aux = aux + 1;
        A = im2bw(rgb2gray(imread(sprintf('data/rosbag_Article%d/frame/frame%04d.jpg', exp_nbr, k))));
        s = regionprops(A,'centroid', 'Area', 'Orientation');
        centroids = cat(1, s.Centroid);
        isOk = false;        
        % In a frame, might be more than just one blob. This cycle will run
        % through all the blobs and add them to a variable., which will
        % record all the blobs detected, at what time, their area, centroid
        % location and orientation.
        % Notice that only blobs larger than a user defined (see
        % CONFIG_DATA_n), will be accepted.
        j = 0;
        for i = 1:size(s,1)
            if(s(i).Area) > config(7,:)
                j = j+1;
                results(j,1,aux) = time(aux);
                results(j,2,aux) = s(i).Area;
                results(j,3,aux) = s(i).Centroid(1);
                results(j,4,aux) = s(i).Centroid(2);
                results(j,5,aux) = s(i).Orientation;
                isOk = true;
            else
                continue
            end
        end
    end    
    if(isOk); results = function_organizeVisualData(results,config(8,:)); else;
              disp('No blobs detected');
    end
end
%% 03 Results
% In this chapter we find the plot of the extracted values of angular
% position of the camera
for chapter = 3:3
    blobData = [];
    if(show_results && isOk)
        tmp1 = []; tmp2 = [];
        figure; hold on; grid on;
        for i = 1:size(results,1)
            for a = 1:size(results,3)
                tmp1(a) = results(i,1,a);
                tmp2(a) = results(i,5,a);
            end
            res = function_compareResults(cart(:,[1 6]), [tmp1; tmp2]');
            plot(cart(:,1), cart(:,6))
            title(sprintf('Blob Calculated Orientation VS Ground Truth\nMean Difference of %1.2f', res(1)))
            xlabel('time(s)')
            ylabel('orientation (º)')
            axis([cart(1,1), cart(end,1) -120 120])
            plot(tmp1, tmp2, '*b')
        end
    end
end


blobData = [];
for i = 1:size(results,3)
    blobData(i,1) = results(1,1,i);
    blobData(i,2) = results(1,5,i);
end

blobData(:,1) = blobData(:,1) - blobData(1,1);
    cart(:,1) =     cart(:,1) -     cart(1,1);

A = []; A(:,1) = blobData(:,1); A(:,2) = blobData(:,2); save('../../../LaTeX/data/05/BlobData.txt', 'A', '-ascii','-double');
A = []; A(:,1) =     cart(:,1); A(:,2) =     cart(:,6); save('../../../LaTeX/data/05/GroundTruth.txt', 'A', '-ascii','-double');
