% César Sousa
% 9-10-2015

% Script para analisar o mov. am CUBO controlo de VELOCIDADE

clc; clear all; close all;

format long % to print all digits of the number in the command window

%load the info from the files
    Positions = csvread('data/find_arrow_position.csv',1,0);
    Visp = csvread('data/find_arrow_state.csv',1,0);
    Fanuc = csvread('data/positions.txt');
    Servos_position = csvread('data/servos_position.txt');
    Servos_velocity = csvread('data/servos_velocity.txt');

% delete fanuc configuration values
    Fanuc = Fanuc(1:2:end,:); 
   
% configure the time lime to be equal to the othes
    Positions(:,1) = Positions(:,1) / 1000000000; 
    Visp(:,1) = Visp(:,1) / 1000000000; 

% get the starting time of the experience
    start_time = max([Servos_velocity(1,1) Servos_position(1,1) Fanuc(1,1) Visp(1,1) Positions(1,1)]);
    
% get the ending time of the experience
    end_time = min([Fanuc(end,1) Visp(end,1) Positions(end,1)]);

    if end_time < start_time
        warning('qqer coisa esta mal nos tempos!!');
    end
    
% set the time to start at second zero
    Positions(:,1) = set_time_line(Positions(:,1),start_time);
    Visp(:,1) = set_time_line(Visp(:,1),start_time);
    Fanuc(:,1) = set_time_line(Fanuc(:,1),start_time);
    Servos_position(:,1) = set_time_line(Servos_position(:,1),start_time);
    Servos_velocity(:,1) = set_time_line(Servos_velocity(:,1),start_time);

    %Converte pos Servos para (Deg)
    Servos_position = [Servos_position(:,1) Servos_position(:,2:3)*0.1];
     
% get the ending time of the experience
    end_time = min([Fanuc(end,1) Visp(end,1) Positions(end,1)]);

%calculate the area of the curve
    %Tilt
    areaSetpointTilt = trapz(abs(Visp(:,1)),abs(Visp(:,2)))
    %Pan
    areaSetpointPan = trapz(abs(Visp(:,1)),abs(Visp(:,3)))
    
    
%______________________________________________________________
%______________________________________________________________
%  PLOT
%______________________________________________________________
%______________________________________________________________

xLimit = [10 end_time];         %# Range of x values
    
Positions(:,1) = Positions(:,1) - xLimit(1);
Visp(:,1) = Visp(:,1) - xLimit(1);
Fanuc(:,1) = Fanuc(:,1) - xLimit(1);
Servos_position(:,1) = Servos_position(:,1) - xLimit(1);
Servos_velocity(:,1) = Servos_velocity(:,1) - xLimit(1);
xLimit = xLimit - xLimit(1);
    
exp_seperator = [28.5 53.77 72.29 87.49 100.1 110.8 123.8];
exp_seperator = exp_seperator - exp_seperator(1);

FontSize = 16;


%chose wich grathics to plot
showFreqData = 1;
if showFreqData==1
    Plot_indicator = [1 %   Pan graphic
        1               %   Tilt graphic
        1               %   Visp FREQ
        1               %   Servos pos FREQ
        1               %   Servos vel FREQ
        1               %   OpenCV FREQ
        1               %   Fanuc FREQ
        1];             %   Fanuc position
else
    Plot_indicator = [1 %   Pan graphic
        1               %   Tilt graphic
        0               %   Visp FREQ
        0               %   Servos pos FREQ
        0               %   Servos vel FREQ
        0               %   OpenCV FREQ
        0               %   Fanuc FREQ
        1];             %   Fanuc position
end
if Plot_indicator(8) == 1
    figure('Name','FANUC POSITION','NumberTitle','off','Color',[1 1 1])
    plot(Fanuc(2:end,1),Fanuc(2:end,2:end));
    hold on;
    title('Fanuc position over time','FontSize',FontSize);
    xlabel('Time (s)','FontSize',FontSize);
    ylabel('Position (mm)','FontSize',FontSize);
    legend('XX','YY','ZZ','P','R','W');
    set(gca,'FontSize',FontSize),...
    draw_cube_movementes_separator_cont_VEL
    xlim(xLimit)
    hold off;
end
%______________________________________________________________
%   PAN
%______________________________________________________________
%______________________________________________________________
%   Servo Position
%______________________________________________________________
if Plot_indicator(1) == 1
%     figure('Name','PAN','NumberTitle','off','Color',[1 1 1])
% 
%     subplot(3,2,[1 3]),...
%     plot(Servos_position(:,1),Servos_position(:,3));
%     hold on;
%     xlabel('Time (s)'),... 
%     xlim(xLimit),... 
%     ylabel('angle (deg)'),... 
%     title('Position of the Pan servo'),...
%     legend('Position of the Pan servo'),...
%     yL = get(gca,'YLim'); 
%     line([[exp_seperator]' [exp_seperator]'],yL,'Color','r');
%     hold off;
%     %______________________________________________________________
%     %   Servo Velocity
%     %______________________________________________________________
%     
%     subplot(3,2,[2 4]),...
%     plot(Servos_velocity(:,1),Servos_velocity(:,3));
% 
%     xlabel('Time (s)'),... 
%     ylabel('Servos velocity'),... 
%     xlim(xLimit),... 
%     title('Velocity of the Pan servo'),...
% 
%     legend('Velocity of the Pan servo'),...
% 
%     yL = get(gca,'YLim'); 
%     % draw line to split the several phases of the experiment
%     line([exp_seperator' exp_seperator'],yL,'Color','r');
% %     XL = XLIM
% %     XLIM([0 XL(2)])
% 
%     %______________________________________________________________
%     %   Position of the arrow (setPoint)
%     %______________________________________________________________
%     figure('Name','PAN ERROR','NumberTitle','off','Color',[1 1 1])
%     
%     [ax, h1, h2] = plotyy(Fanuc(:,1),Fanuc(:,3)-Fanuc(1,3),...
%         Visp(:,1),Visp(:,3)),...
%     linkaxes(ax,'x'),...x axis is the same for both grafics
%     xlabel('Time (s)','FontSize',FontSize),... 
%     ylabel(ax(1),'Distance (mm)','FontSize',FontSize),... 
%     ylabel(ax(2),'Distance to Setpoin (pixels)','FontSize',FontSize),... 
%     xlim(xLimit),... 
%     title('Distance of the arrow to the center on image Vs Fanuc''s position','FontSize',FontSize),...
%     legend('Fanuc''s position -y axis',...
%             'Position of the arrow (setPoint)');
% 	hold on;
%     set(h2,'marker','*','MarkerSize',4)
% 	set(gca,'FontSize',FontSize),...
% 
%             
%     grid on;
    
%______________________________________________________________
%   Graficos juntos! 
%______________________________________________________________
    
    %Figure
    figure('Name','Vel e pos PAN servo + Fanuc YY','NumberTitle','off','Color',[1 1 1])
    %Plot
    %plot(Fanuc(:,1),Fanuc(:,3)-Fanuc(1,3));
    plot(Servos_position(:,1),Servos_position(:,3)-Servos_position(340,3));
    ylim([-50 50]);
    set(gca,'YColor','b');
    % Add Data
        % Coloca a fontsize a 16 por defeito
    %addaxis(Servos_position(:,1),Servos_position(:,3)-Servos_position(340,3)); 
    addaxis(Servos_velocity(:,1),Servos_velocity(:,3)-Servos_velocity(1,3)...
        ,'color','m');
    addaxis2([Visp(:,1) Visp(:,1)],[Visp(:,3) zeros(size(Visp(:,3),1),1)],'color','r');
    % XX axis
	xlim(xLimit)
    xlabel('Time (s)','FontSize',FontSize),... 
    % YY axis
        %1
    %hPlot=ylabel('Distance to the start position (mm)');
    hPlot=ylabel('Pan angle (deg)');
    set(hPlot, 'FontSize', FontSize)
        %2
    %addaxislabel(2,'Pan angle (deg/10)','FontSize', FontSize);
    %addaxisset([-500 500],2);
        %3
	addaxislabel(2,'Pan servo velocity');
    addaxisset([0 14],2);
        %4
	addaxislabel(3,'Difference to the center of image (pixels)');
    addaxisset([-250 150],3);
    % Configs
    set(gca,'FontSize',FontSize),...
    legend(...'Fanuc''s position (YY axis)',...
           'Position of the Pan servo',...
           'Velocity of the Pan servo',...
           'Position of the arrow in the image (YY image axis - Set point)');
    draw_cube_movementes_separator_cont_VEL
    grid off;

% % % 	[ax] = plot(Visp(:,1),Visp(:,3));
% % % 
% % %     xlabel('Time (s)'),... 
% % %     ylabel('Error (pixels)'),... 
% % % 
% % %     title('distance the arrow to the center of the image'),...
% % % 
% % % 
% % %     legend('Position of the arrow (setPoint)');
% % %     yL = get(gca,'YLim'); 
% % %     % draw line to split the several phases of the experiment
% % %     line([exp_seperator' exp_seperator'],yL,'Color','r');
% % % %     XL = XLIM
% % % %     XLIM([0 XL(2)])

end
%______________________________________________________________
%   TILT
%______________________________________________________________

%______________________________________________________________
%   Servo Position
%______________________________________________________________
if Plot_indicator(2) == 1
%     figure('Name','TILT','NumberTitle','off','Color',[1 1 1])
% 
%     plot(Servos_position(:,1),Servos_position(:,2));
%     hold on;
%     xlabel('Time (s)'),... 
%     ylabel('XX-err (m)'),... 
%     title('Position of the TILT servo'),...
%     legend('Position of the Pan servo');
%     yL = get(gca,'YLim'); 
%     % draw line to split the several phases of the experiment
%     line([exp_seperator' exp_seperator'],yL,'Color','r');
% %     XL = XLIM;
% %     XLIM([0 XL(2)]);
%     hold off
%     
%     %______________________________________________________________
%     % Servo Velocity
%     %______________________________________________________________
% 
%     figure('Name','TILT velocity','NumberTitle','off','Color',[1 1 1])
% 
%     plot(Servos_velocity(:,1),Servos_velocity(:,2));
%     hold on;
%     xlabel('Time (s)'),... 
%     ylabel('Servos_velocity'),... 
%     title('Velocity of the TILT servo'),...
%     legend('Velocity of the Pan servo'),...
% 	yL = get(gca,'YLim'); 
%     % draw line to split the several phases of the experiment
%     line([exp_seperator' exp_seperator'],yL,'Color','r');
% %     XL = XLIM;
% %     XLIM([0 XL(2)]);
%     hold off;
%     %______________________________________________________________
%     % Position of the arrow (setPoint)
%     %______________________________________________________________
% 
%     figure('Name','TILT ERROR','NumberTitle','off','Color',[1 1 1])
%     plot(Visp(:,1),Visp(:,2));
%     hold on;
%     xlabel('Time (s)'),... 
%     ylabel('Error (pixels)'),... 
%     title('Position of the arrow'),...
%     legend('Position of the arrow (setPoint)/ distance to the center of the image'),...
%     yL = get(gca,'YLim'); 
%     % draw line to split the several phases of the experiment
%     line([exp_seperator' exp_seperator'],yL,'Color','r');
%     XL = XLIM;
%     XLIM([0 XL(2)]);
%     hold off;
        
%______________________________________________________________
%   Graficos juntos! 
%______________________________________________________________

     %Figure
    figure('Name','Vel e pos TILT servo + Fanuc YY','NumberTitle','off','Color',[1 1 1])
    %Plot
    %plot(Fanuc(:,1),Fanuc(:,3)-Fanuc(1,3));
    plot(Servos_position(:,1),...
        Servos_position(:,2)-Servos_position(1,2));
    ylim([-50 50]);
    set(gca,'YColor','b');
    % Add Data
        % Coloca a fontsize a 16 por defeito
    %addaxis(Servos_position(:,1),Servos_position(:,2)-Servos_position(1,2)); 
    addaxis(Servos_velocity(:,1),Servos_velocity(:,2)-Servos_velocity(1,2)...
        ,'color','m');
    addaxis2([Visp(:,1) Visp(:,1)],[Visp(:,2) zeros(size(Visp(:,2),1),1)],'color','r');
    % XX axis
	xlim(xLimit)
    xlabel('Time (s)','FontSize',FontSize),... 
    % YY axis
        %1
    %hPlot=ylabel('Distance to the start position (mm)');
    hPlot=ylabel('Tilt angle (deg)');
    set(hPlot, 'FontSize', FontSize)
        %2
    %addaxislabel(1,'Tilt angle (deg/10)','FontSize', FontSize);
    %addaxisset([-500 500],2);
        %3
	addaxislabel(2,'Tilt servo velocity');
    addaxisset([0 14],2);
        %4
	addaxislabel2(3,'Difference to the center of image (pixels)');
    addaxisset([-250 150],3);
%     htxt = ylabel('Difference to the center of image (pixels)');
%     set(htxt,'color','r');

    % Configs
    set(gca,'FontSize',FontSize),...
    legend(...%'Fanuc''s position (YY axis)',...
           'Position of the Tilt servo',...
           'Velocity of the Tilt servo',...
           'Position of the arrow in the image (YY image axis - Set point)');
    draw_cube_movementes_separator_cont_VEL
    grid off;
end

%______________________________________________________________
%   Frequency
%______________________________________________________________

%______________________________________________________________
%   Visp frequency
%______________________________________________________________
if Plot_indicator(3) == 1
    average_time = mean(diff(Visp(:,1)))
	figure('Name','Visp Freq.','NumberTitle','off','Color',[1 1 1])
    plot(1:size(Visp,1)-1,diff(Visp(:,1)),'.b',...
        1:size(Visp,1)-1,average_time,'-r');
    hold on;
    title('Visp dela time');
    xlabel('Sample');
    ylabel('Time (s)');
    legend('Time (s)',['Average time: ' ...
        num2str(average_time) ' sec  - Frequency: ' ...
        sprintf('%.1f',(1/average_time)) ' Hz']);
end
%______________________________________________________________
%   Servos Position
%______________________________________________________________
if Plot_indicator(4) == 1   
    average_time = mean(diff(Servos_position(:,1)));
    figure('Name','Servo Position','NumberTitle','off','Color',[1 1 1])
    plot(1:size(Servos_position,1)-1,diff(Servos_position(:,1)),'.b',...
        1:size(Servos_position,1)-1,average_time,'-r')
    hold on;
    title('Servos position dela time betwen experiences');
    xlabel('Sample');
    ylabel('Time (s)');
    legend('Time (s)',['Average time: ' ...
        num2str(average_time) ' (sec)  - Frequency: ' ...
        sprintf('%.1f',(1/average_time)) ' (Hz)']);
end
%______________________________________________________________
%   Servos Velocity
%______________________________________________________________
if Plot_indicator(5) == 1   
    %calculate the diference between two consecutive samples
    average_time = mean(diff(Servos_velocity(:,1)));
    
    figure('Name','Servo Velocity','NumberTitle','off','Color',[1 1 1])
	plot(1:size(Servos_velocity,1)-1,diff(Servos_velocity(:,1)),'.b',...
        1:size(Servos_velocity,1)-1,average_time,'-r')
    hold on;
    title('Servos velocity dela time between experiences');
    xlabel('Sample');
    ylabel('Time (s)');
    legend('Time (s)',['Average time: ' ...
        num2str(average_time) ' (sec)  - Frequency: ' ...
        sprintf('%.1f',(1/average_time)) ' (Hz)']);
end  
%______________________________________________________________
%   Find arrow (opencv)
%______________________________________________________________
if Plot_indicator(6) == 1  
    average_time = mean(diff(Positions(:,1)))
    figure('Name','Find arrow algorithm (OPENCV)','NumberTitle','off','Color',[1 1 1])
    plot(1:size(Positions,1)-1,diff(Positions(:,1)),'.b',...
        1:size(Positions,1)-1,average_time,'-r')
    hold on;
    title('find arrow algorithm, time between experiences');
    xlabel('Sample');
    ylabel('Time (s)');
    legend('Time (s)',['Average time: ' ...
        num2str(average_time) ' (sec)  - Frequency: ' ...
        sprintf('%.1f',(1/average_time)) ' (Hz)']);
end
%______________________________________________________________
%   get data from Fanuc
%______________________________________________________________
if Plot_indicator(7) == 1  
    Fanuc = Fanuc(200:end,:);
    average_time = mean(diff(Fanuc(:,1)))
    figure('Name','Fanuc get data frequancy','NumberTitle','off','Color',[1 1 1])
    plot(1:size(Fanuc,1)-1,diff(Fanuc(:,1)),'.b',...
        1:size(Fanuc,1)-1,average_time,'-r')
    hold on;
    title('Fanuc data, time between experiences');
    xlabel('Sample');
    ylabel('Time (s)');
    legend('Time (s)',['Average time: ' ...
        num2str(average_time) ' (sec)  - Frequency: ' ...
        sprintf('%.1f',(1/average_time)) ' (Hz)']);
end    

break;
%______________________________________________________________
%   calculate MSE
%______________________________________________________________
validXDataFanuc = [11.69 140.2]; %Fanuc movement
validXDataVisp  = [11.69 141.3];  %Visp valid data (diff(targetPos - imgCenter))
Visp = Visp(892:3017,:);    % Get the valid data from the set position

timeExe = (validXDataVisp(end)-validXDataVisp(1));

panMseVisp = sum((Visp(:,3).^2))/timeExe% MSE of the Pan Movement
tiltMseVisp = sum((Visp(:,2).^2))/timeExe% MSE of the Tilt Movement


 

