%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                           %
%       Simular Movimento do Fanuc          %
%                                           %
%    Gon�alo Alberto Ramos Carpinteiro      %
%                N� 49642                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%
% Limpar Memorias %
%%%%%%%%%%%%%%%%%%%
clear all; close all; clc

% %% Criação objecto Vídeo
% 
% Fig = figure('position',[100 50 600 600],'color',[1 1 1]); 
% aviobj= VideoWriter('49642.avi');
% aviobj.FrameRate=50;
% aviobj.Quality = 100;
% a = axes('Parent',Fig);
% axis(a,'tight');
% set(a,'nextplot','replacechildren');
% opengl('software');
% open(aviobj);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Leitura dos dados e tratamento!!!  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

file_f = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Simulador/Fanuc.txt');
file_s = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Simulador/Sensor.txt');
% file_s(:,2)=file_s(:,2)-1.5;
% file_s(:,5)=file_s(:,5)-1.5;

file_f(:,1)=file_f(:,1)-file_f(1,1);
file_s(:,1)=file_s(:,1)-file_s(1,1);

% file_s(:,2:end)=file_s(:,2:end)-mean(file_s(:,2:end),2)*[1 1 1 1];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Eixos e Variaveis Iniciais %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Eixos -> P
run seixos;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Desenhar o plano metedo 2 para cada ponto %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% figure('position',[100 50 600 600],'color',[1 1 1]); 
grid on

for i=1:size(file_s,1)

    %%%%%%%%%%%%%%%%%%
    % Eixo na origem %
    %%%%%%%%%%%%%%%%%%
    fill3(P(1,:),P(2,:),P(3,:),'y');
    hold on
    
    %%%%%%%%%%%%%%%%%%
    % Valores altura %
    %%%%%%%%%%%%%%%%%%
    Z=file_s(i,2:end);
    
    
    %%%%%%%%%%%%%%%%%%%%%%%
    % Valor fixo de x e y %
    %%%%%%%%%%%%%%%%%%%%%%%
    X=[0 -35 35 0];
    Y=[35 0 0 -35];
    
    %%%%%%%%%%%%%%%%%%%%%%%%
    % Combinações metodo 2 %
    %%%%%%%%%%%%%%%%%%%%%%%%
    cc = combnk(1:4,3);                                 

    %%%%%%%%%%%%%%%%%%%%%%%%%%
    % Variaveis acumulativas %
    %%%%%%%%%%%%%%%%%%%%%%%%%%
    Pa=[];
    Pb=[];
    Pc=[];
    Pd=[];


    % Ciclo
    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
    
    % Valores do plano e desenho do mesmo    
    aa=mean(Pa); bb=mean(Pb); cc=mean(Pc); dd=mean(Pd);
    
    % Valores de x para criar o meshgrid
    x=-45:15:45;
    [Xp,Yp] = meshgrid(x);
        
    % Equação do plano para tirar valores de Z
    Zp=-aa/cc*Xp-bb/cc*Yp+dd/cc;
    
    % Transformações 2->x  3->y  4->z ;  5->x  6->y  7->z
    T0=Trans(file_f(i,2),file_f(i,3),file_f(i,4));
    T1=Rot('x',file_f(i,5));
    T2=Rot('y',file_f(i,6));
    T3=Rot('z',file_f(i,7));

    % Trasnformação final
    Tf=T0*T1*T2*T3;
    
    % Transformar o eixo e desenho
    P1=Tf*P;
    fill3(P1(1,:),P1(2,:),P1(3,:),'b');
    
    % Transformação e desenho dos pontos
    Pnts=[X;Y;Z;ones(1,4)];
    Pts=Tf*Pnts;
    plot3(Pts(1,:),Pts(2,:),Pts(3,:),'r*');
    
    % Origens dos sensores
    Zo=[0 0 0 0];
    Org=[X;Y;Zo;ones(1,4)];
    Orgf=Tf*Org;
    plot3(Orgf(1,:),Orgf(2,:),Orgf(3,:),'*g');
    
    % Linha que una as origens aos sensores
    line([Orgf(1,1) Pts(1,1)],[Orgf(2,1) Pts(2,1)],[Orgf(3,1) Pts(3,1)])
    line([Orgf(1,2) Pts(1,2)],[Orgf(2,2) Pts(2,2)],[Orgf(3,2) Pts(3,2)])
    line([Orgf(1,3) Pts(1,3)],[Orgf(2,3) Pts(2,3)],[Orgf(3,3) Pts(3,3)])
    line([Orgf(1,4) Pts(1,4)],[Orgf(2,4) Pts(2,4)],[Orgf(3,4) Pts(3,4)])
    
    % Plano chão
    Xgg=[-150 10 10 -150];
    Ygg=[-700 -700 -1000 -1000];
    Zgg=[-420 -420 -420 -420];
    
%     fill3(Xgg,Ygg,Zgg,'g');
    
    % Desenhar e transformar plano
    Plane=[Xp(:)';Yp(:)';Zp(:)';ones(1,size(Zp,2)*size(Zp,1))];
    
    Planef=Tf*Plane;
    
%     plot3(Planef(1,:),Planef(2,:),Planef(3,:),'-k')

    title('Simulacao')
    xlabel('x')
    ylabel('y')
    zlabel('z')
    grid on
%     view(-50,20)
    axis equal
    S=200;
    axis([-150 (-150+S) -1000 (-1000+S) -450 (-450+S)])
%     axis square
%     view(-90,0)
%             F = getframe(Fig);
%             writeVideo(aviobj,F);    
    pause(0.001)
    
    hold off

end


% % Fechar video
%         for k=1:400
%             F = getframe(Fig);
%             writeVideo(aviobj,F);
%         end
%         close(aviobj);



















