%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                           %
%        Testar Metodos Matemáticos         %
%                                           %
%    Gon�alo Alberto Ramos Carpinteiro      %
%                N� 49642                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Live test

close all; clc


% Start communication with fanuc

try
    fclose(fanuc); 
    delete(fanuc);
    clear fanuc
catch
end

fanuc = tcpip('192.168.0.230', 4900); 
set(fanuc, 'InputBufferSize', 30000); 

fopen(fanuc);

% Start communication with arduino

try
    fclose(arduino); 
    delete(arduino);
    clear arduino
catch
end

arduino = serial('/dev/ttyUSB0', 'BaudRate', 9600);
fopen(arduino);

% Run a program

fprintf(fanuc, 'RUNTPP\nCARPSIMU\n1\n');
% Data for confirmation.
[msg, bytesread]=fgetl(fanuc);


% Create a figure

Fig = figure('position',[100 50 600 600],'color',[1 1 1]);

% Get position and plot

run seixos; 

CleanBuffer(fanuc)

width=1000;
cx = -80;
cy = -880;
cz = -400;

sensors(1,:) = [0 -350 350 0];
sensors(2,:) = [350 0 0 -350];
sensors(3,:) = [40 70 70 40];
sensors(4,:) = [1 1 1 1];


Zmed=[];
pit=[];
rol=[];

pit0=mean(pith(:,2));
rol0=mean(rolh(:,2));

while(1)
    %
    %Get fanuc position
    pos = getcrcpos(fanuc);
    
    transform = trans3(pos(1),pos(2),pos(3))*RPY([pos(6) pos(5) pos(4)]);
    
    %Get sensor values
    
    values = read_distances(arduino);
    distances = calibrate_distances(values)*10;
    
    %Draw sensors origins
    Org=sensors;
    Orgf=transform*Org;
    plot3(Orgf(1,:),Orgf(2,:),Orgf(3,:),'g*');
    
    hold on
    
    %Draw sensors position
    Pnts=sensors;
    Pnts(3,:)=distances;
    Pts=transform*Pnts;
    plot3(Pts(1,:),Pts(2,:),Pts(3,:),'r*');
    
    %Draw line
    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)])
    
     %Get plan
    
    Pp=get_plan_meth_2(sensors(1,:),sensors(2,:),[0.1 0.1 0.1 0.1]);
    a=Pp(1); b=Pp(2); c=Pp(3); d=Pp(4);
    [PX,PY,PZ]=get_plan(a,b,c,d);
    
    %Draw plan
    To=surf2patch(PX,PY,PZ);
    % Obtenção das faces e dos vertices
    Pf=To.faces;
    Pv=To.vertices;
    
    % Vertices em homogeneo para usar transformações
    Pv=Pv';
    Pvh=[Pv ; ones(1,size(Pv,2))];

    % Transformação inicial
    Pvf=transform*Pvh;
    Pvf=Pvf';
    Pvf=Pvf(:,1:3);
    
        patch('Vertices',Pvf,'Faces',Pf,'FaceColor',[1 1 0],'EdgeColor',[0.8 0.8 0]);
    
    %Get plan
    
    Pp=get_plan_meth_2(sensors(1,:),sensors(2,:),distances);
    a=Pp(1); b=Pp(2); c=Pp(3); d=Pp(4);
    [PX,PY,PZ]=get_plan(a,b,c,d);
    %Draw plan
    To=surf2patch(PX,PY,PZ);
    % Obtenção das faces e dos vertices
    Pf=To.faces;
    Pv=To.vertices;
    
    % Vertices em homogeneo para usar transformações
    Pv=Pv';
    Pvh=[Pv ; ones(1,size(Pv,2))];

    % Transformação inicial
    Pvf=transform*Pvh;
    Pvf=Pvf';
    Pvf=Pvf(:,1:3);
    
        patch('Vertices',Pvf,'Faces',Pf,'FaceColor',[0 1 0],'EdgeColor',[0 0.8 0]);
    
    
    
    % angle

    lx=d*c/a;
    ly=d*c/b;

    pit=[pit rad2deg(atan(d/lx))-pith];
    rol=[rol rad2deg(atan(d/ly))-rolh];
    
    
    object=transform*P;

    fill3(object(1,:),object(2,:),object(3,:),'b');

    axis([ cx-width/2 cx+width/2 cy-width/2 cy+width/2 cz-width/2 cz+width/2])
    grid on
     view(20,30)
    xlabel('x')
    ylabel('y')
    zlabel('z')
    
     title(sprintf('Simulation  Z med:%3.2d Pit: %3.2d Rol: %3.2d',mean(Planf(3,:)),rad2deg(atan(d/lx))-pit0,rad2deg(atan(d/ly))-rol0))
    
    Zmed=[Zmed mean(Planf(3,:))];
%
    pause(0.1)
    hold off
end
 
 %% Export data



%% Off-line test

% Clear all memory and close all figures.
close all; clc

% Abrir ficheiros.

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

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



file_s(:,2:end)=file_s(:,2:end)*10;

% Zona de calibração
rol0=2.9948; % met2
% pit0=2.9948; % met3

pit0=0.2176; % met2
% rol0=0.2176; % met3
 figure('position',[100 50 600 600],'color',[1 1 1])
  
  plot(file_f(:,1),file_f(:,4))
  hold on
  des=424+267/2;
  plot(file_s(:,1),file_s(:,2)-des,'r')
  plot(file_s(:,1),file_s(:,3)-des,'g')
  plot(file_s(:,1),file_s(:,4)-des,'k')
  plot(file_s(:,1),file_s(:,5)-des,'y')
  grid on
  title('Altura lida do fanuc no seu referencial global e sensores.')
 xlabel('Tempo decorrido')
 ylabel('Altura')
 %axis([1 4.5 -420 -270])
 legend('Fanuc','A0','A1','A2','A3');

 export_fig('Dados_teste_dinamico.pdf','-pdf')
 
 
 % Correr os eixos
 run seixos;
 
 % Coordenadas dos sensores
 
 X = [0 -350 350 0];
 Y = [350 0 0 -350];
 Z0 = [40 70 70 40];
  
 % Interpolar para os sensores
 file_sf(:,1)=file_f(:,1);
 file_sf(:,2)=interp1(file_s(:,1),file_s(:,2),file_f(:,1));
 file_sf(end,2)=file_sf(end-1,2);
 file_sf(:,3)=interp1(file_s(:,1),file_s(:,3),file_f(:,1));
 file_sf(end,3)= file_sf(end-1,3);
 file_sf(:,4)=interp1(file_s(:,1),file_s(:,4),file_f(:,1));
 file_sf(end,4)= file_sf(end-1,4);
 file_sf(:,5)=interp1(file_s(:,1),file_s(:,5),file_f(:,1));
 file_sf(end,5)= file_sf(end-1,5);
 %
%   figure('position',[100 50 600 600],'color',[1 1 1])
 
width=1000;
cx = -80;
cy = -880;
cz = -400;
Zmed =[];
pit=[];
rol=[];

%  
%  
%  xlabel('x')
%  ylabel('y')
%  zlabel('z')
%  axis([ cx-width/2 cx+width/2 cy-width/2 cy+width/2 cz-width/2 cz+width/2])
%  
%  hold on
%  grid on
%  view(3)
 
 for n=1:size(file_sf,1)

%      cla
transform = trans3(file_f(n,2),file_f(n,3),file_f(n,4))*RPY([degtorad(file_f(n,7)) degtorad(file_f(n,6)) degtorad(file_f(n,5))]);

A0p=file_sf(n,2);
A1p=file_sf(n,3);
A2p=file_sf(n,4);
A3p=file_sf(n,5);
  
%  subplot(1,2,2)
 
 %Draw axis
 object=transform*P;
%  fill3(object(1,:),object(2,:),object(3,:),'b');
 Z=[A0p A1p A2p A3p];
 
%  hold on
 
 
 %Draw sensors origins
 Org=[X;Y;Z0;[1 1 1 1]];
 Orgf=transform*Org;
%  plot3(Orgf(1,:),Orgf(2,:),Orgf(3,:),'g*');


%Draw sensors position
Pnts=[X;Y;Z;[1 1 1 1]];
Pts=transform*Pnts;
% plot3(Pts(1,:),Pts(2,:),Pts(3,:),'r*');

%Draw line
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)])


     %Get plan
    
    Pp=get_plan_meth_3(X,Y,[0.1 0.1 0.1 0.1]);
    a=Pp(1); b=Pp(2); c=Pp(3); d=Pp(4);
    [PX,PY,PZ]=get_plan(a,b,c,d);
    
    %Draw plan
    To=surf2patch(PX,PY,PZ);
    % Obtenção das faces e dos vertices
    Pf=To.faces;
    Pv=To.vertices;
    
    % Vertices em homogeneo para usar transformações
    Pv=Pv';
    Pvh=[Pv ; ones(1,size(Pv,2))];

    % Transformação inicial
    Pvf=transform*Pvh;
    Pvf=Pvf';
    Pvf=Pvf(:,1:3);
    
%         patch('Vertices',Pvf,'Faces',Pf,'FaceColor',[1 1 0],'EdgeColor',[0.8 0.8 0]);

% Plano horizontal
 Ph2=get_plan_meth_3(X,Y,Z);
 [PX,PY,PZ]=get_plan(Ph2(1),Ph2(2),Ph2(3),Ph2(4));
    %Draw plan
    To=surf2patch(PX,PY,PZ);
    % Obtenção das faces e dos vertices
    Pf=To.faces;
    Pv=To.vertices;
    
    % Vertices em homogeneo para usar transformações
    Pv=Pv';
    Pvh=[Pv ; ones(1,size(Pv,2))];

    % Transformação inicial
    Pvf=transform*Pvh;
    Pvf=Pvf';
    Pvf=Pvf(:,1:3);
    
%         patch('Vertices',Pvf,'Faces',Pf,'FaceColor',[0 1 0],'EdgeColor',[0 0.8 0]);
 
%Get plan
% plot3(Plan2(1,:),Plan2(2,:),Plan2(3,:),'-y')

% Alutra media

Zmed=[Zmed mean(mean(PZ))];

% Angulo

a=Ph2(1);b=Ph2(2);c=Ph2(3);d=Ph2(4);

 lx=d*c/a;
 ly=d*c/b;
        
 pit=[pit; file_sf(n,1) rad2deg(atan(d/lx))-pit0];
 rol=[rol; file_sf(n,1) rad2deg(atan(d/ly))-rol0];
 
% title(sprintf('Horizontal Metodo 2 Zmed: %3.2d pit: %3.2d rol: %3.2d ',mean(mean(PZ)),rad2deg(atan(d/lx))-pit0,rad2deg(atan(d/ly))-rol0));
 
 
%  pause(0.005)
 
 end

 %
%

 
  
 % Calibrar

% pit(:,2)=pit(:,2)-rol0;
% 
% rol(:,2)=rol(:,2)-pit0;
 
 % Corrigir pitch
 for i=1:size(file_f,1)
    
   if file_f(i,5)<0
       file_f(i,5)=(file_f(i,5)+180);
   elseif file_f(i,5)>0
       file_f(i,5)=(file_f(i,5)-180);
   end
    
 end
 

 figure('position',[100 50 600 600],'color',[1 1 1])
  
   plot(file_f(:,6))
   hold on
  plot(pit(:,2),'r')
  hold on
  grid on
  title('Angulo Pitch')
 xlabel('Medicoes')
 ylabel('Angulo (o)')
 %axis([1 4.5 -420 -270])

 export_fig('Pitch_teste_dinamico.pdf','-pdf')
 
 
  figure('position',[100 50 600 600],'color',[1 1 1])
  
   plot(file_f(:,5))
   hold on
  plot(rol(:,2),'r')
  hold on
  grid on
  title('Angulo Roll')
 xlabel('Medicoes')
 ylabel('Angulo (o)' )
 %axis([1 4.5 -420 -270])

 export_fig('Roll_teste_dinamico.pdf','-pdf')
 


