%% Limpar dados existentes
clc;
clear all;
close all;

% Iniciar ficheiro

file_f = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Planos/Fanuc.txt');
file_s = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Planos/Sensor.txt');

% Retirar a diferença de alturas de 15 mm para trabalhar no mesmo plano.
file_s(:,2)=file_s(:,2)-1.5;
file_s(:,5)=file_s(:,5)-1.5;

% Colocar intervalo de tempo correcto em relação ao instante inicial!
file_f(:,1)=file_f(:,1)-file_f(1,1);
file_s(:,1)=file_s(:,1)-file_s(1,1);


% Obter ângulos segundo método plano médio, mais robusto e nemos tempo
% computação


Pitch=[];
Roll=[];

for i=1:size(file_s,1)
    %
    Z=file_s(i,2:end);
    
    cc = combnk(1:4,3);                                 
    X=[35 0 -35 0];
    Y=[0 35 0 -35];

    Pa=[];
    Pb=[];
    Pc=[];
    Pd=[];

       
    for n=1:size(cc,1)

         pos=cc(n,:);
         x1= X(pos(1)); y1= Y(pos(1));
         x2= X(pos(2)); y2= Y(pos(2));
         x3= X(pos(3)); y3= Y(pos(3));

         % Os Z

         z1= Z(pos(1)); 
         z2= Z(pos(2));
         z3= Z(pos(3));
         
         xf=[x1 x2 x3];
         yf=[y1 y2 y3];
         zf=[z1 z2 z3];

                  
         % Plano

         A=  (y1*z2 - y2*z1 - y1*z3 + y3*z1 + y2*z3 - y3*z2)/(x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1);
         B= -(x1*z2 - x2*z1 - x1*z3 + x3*z1 + x2*z3 - x3*z2)/(x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1);
         C=  (x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2)/(x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1);
         
         
         % D calculator
         % a/d x + b/d y + c/d z == 1
         % A x + B y + C z == 1
         
         Zpt=A/C*x3+B/C*y3;
         D=z3+Zpt;
         
         a=A*D; b=B*D; c=C*D; d=D;

        % Acumular A B C e D
        Pa=[Pa a]; Pb=[Pb b]; Pc=[Pc c]; Pd=[Pd d];

    end
    %
    
    aa=mean(Pa); bb=mean(Pb); cc=mean(Pc); dd=mean(Pd);
    
    % Desenhar pontos e plano
     figure('position',[100 50 600 600],'color',[1 1 1])
     hp=plot3(X(1),Y(1),Z(1),'r*',X(2),Y(2),Z(2),'*r',X(3),Y(3),Z(3),'r*',X(4),Y(4),Z(4),'r*');
     hold on
%          %fill3(xf,yf,zf,'r')
          title('Plano medio ')
          grid on
%           % Desenhar o plano
%  
          x=-45:15:45;
        [Xp,Yp] = meshgrid(x);
%         
         Zp=-aa/cc*Xp-bb/cc*Yp+dd/cc;
%         
         [u,v,w]=surfnorm(Xp,Yp,Zp);
         hs=surf(Xp,Yp,Zp);
         colormap([1,1,0])
         quiver3(Xp,Yp,Zp,u,v,w)
%           line([0 a],[0 b],[0 c])
         xlabel('x'); ylabel('y'); zlabel('z')
%         view(-20,6)
%         
%         export_fig('Plano_med.pdf','-pdf')
    
    % Angulo
    n=[aa bb cc];
    
        lx=d*c/a;
        ly=d*c/b;
        
        pit=rad2deg(atan(d/lx));
        rol=rad2deg(atan(d/ly));
    
        Pitch=[Pitch ; file_s(i,1) pit];
        Roll=[Roll ; file_s(i,1) rol];
end



figure('position',[100 50 600 600],'color',[1 1 1])
plot(file_f(:,1),file_f(:,2),'b');
hold on
plot(Pitch(:,1),Pitch(:,2),'r');
grid on
% axis square
xlabel('Tempo (s)');
ylabel('Angulo (o)');
title('Valores do Pitch Fanuc vs Sensores com planos');
export_fig('Pitch_plano_med.pdf','-pdf')
    
    
figure('position',[100 50 600 600],'color',[1 1 1])
plot(file_f(:,1),file_f(:,3),'b');
hold on
plot(Roll(:,1),Roll(:,2),'r');
grid on
% axis square
xlabel('Tempo (s)');
ylabel('Angulo (o)');
title('Valores do Roll Fanuc vs Sensores com planos');
export_fig('Roll_plano_med.pdf','-pdf')