%função que vai desenhar o robo

%% Objectos para representar sistemas de coordenadas
%% de forma não ambígua. Possibilidade de usar um 
%% objecto único para os 3 eixos.  V. Santos, Oct 2007.

close all;
clear all;
clc;

%% Definição parcial de cada sub-objecto (eixo)
Xpart = [
 0  0.05   0.5   0.4  0.5  0.6   0.7  0.8  0.7   1     1    1.5 1   1     0.7  0.8  0.7  0.6  0.5  0.4  0.5  0.05
 0 -0.05  -0.05 -0.2 -0.2 -0.05 -0.2 -0.2 -0.05 -0.05 -0.2  0   0.2 0.05  0.05 0.2  0.2  0.05 0.2  0.2  0.05 0.05
 0  0      0     0    0    0     0     0   0     0      0   0   0   0     0      0    0    0   0    0    0    0
 ]*20;

Ypart = [
0 0.05 0.05 0.2 0.2 0.05 0.05 0.2 0   -0.2 -0.05 -0.05 -0.2 -0.2 -0.05 -0.05
0 0.05 0.6  0.7 0.8 0.7  1    1   1.5 1     1     0.7   0.8  0.7  0.6   0.05
0 0    0    0   0   0    0    0   0   0     0     0     0    0    0     0 
 ]*20;

Zpart = [
     0   0     0      0     0    0    0     0     0     0   0   0   0    0    0   0    0    0    0     0    0
     0  -0.05 -0.05  -0.2  -0.2 -0.1 -0.1  -0.05 -0.05 -0.2 0   0.2 0.05 0.05 0.2 0.2  0.1  0.1  0.05  0.05 0.05
     0   0.05  0.55   0.45  0.8  0.8  0.62  0.66  1     1   1.5 1   1    0.7  0.8 0.45 0.45 0.62 0.58  0.5  0.05
]*20; 
 
%% Concatenação dos três objectos num só.
X = [ Xpart(1,:) Ypart(1,:) Zpart(1,:) ];
Y = [ Xpart(2,:) Ypart(2,:) Zpart(2,:) ];
Z = [ Xpart(3,:) Ypart(3,:) Zpart(3,:) ];


%% Ilustração do desenho do objecto
fill3(X,Y,Z, 'b');
xlabel('X'); ylabel('Y'); zlabel('Z');
grid on; hold on;


%% Transformações:
P0 = [X;Y;Z; ones(1,size(X,2))]; %ones(1,size(X,2)) - matriz de 1's com tantos quanto numeros existem numa linha

TabelaI=[%%  t   l   d   a
    0 0 45 90
    0 139 0 0
    0 142 0 0
    ];

TabelaF=[%%  t   l   d   a
    0 0 45 90
    0 139 0 0
    0 142 0 0
    ];

K=100;
%%%%%%%%%%%%%%%%%%%%%%%%%
Pi=[0 0 200]'
Pf=[0 0 250]'
r1=1;
r3=0;
DH=TabelaI;
%%%%%%%%%%%%%%%%%%%%%%%%%
%plot3(Pf(1,1),Pf(2,1),Pf(3,1)+3,'*b');
%%%%%%%%%%%%%%%%%%%%%%%%%
AngsI=CIRRRantropomorfico(DH,Pi,r1,r3)
for ii=2:1:size(TabelaI,2)
    for i=1:1:size(TabelaI,1)
        DHI(i,1)=AngsI(i,1);
        DHI(i,ii)=TabelaI(i,ii);
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%
AngsF=CIRRRantropomorfico(DH,Pf,r1,r3)
for ff=2:1:size(TabelaF,2)
    for f=1:1:size(TabelaF,1)
        DHF(f,1)=AngsF(f,1);
        DHF(f,ff)=TabelaF(f,ff);
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%
TabelaI=DHI
TabelaF=DHF
%%%%%%%%%%%%%%%%%%%%%%%%%
for t=1:size(TabelaI,1)
    Qi(t,1)=TabelaI(t,1);
    Qf(t,1)=TabelaF(t,1);
end

MQ=VectLinSpace(Qi,Qf,K);
DH=TabelaI;
MDH=GeraMultiDH(DH,MQ);
SHM=CalculaMovimentoRobo(MDH);
%%%%%%%%%%%%
T=SHM(:,:,1,1);
P = T * P0;
h(1,1)=fill3(P(1,:),P(2,:),P(3,:), 'g');
for i=2:1:size(SHM,4)
   Tr=SHM(:,:,1,i);
   T = T * Tr;
   P = T * P0;
   h(1,i)=fill3(P(1,:),P(2,:),P(3,:), 'g');
   axis equal;
   %axis([0 10 0 10 0 500])
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   pause(1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=2:1:size(SHM,3)
    T=SHM(:,:,j,1);
    P = T * P0;
    set(h(1,1),'XData',P(1,:))
    set(h(1,1),'YData',P(2,:))
    set(h(1,1),'ZData',P(3,:))
    for i=2:1:size(SHM,4)
        Tr=SHM(:,:,j,i);
        T = T * Tr;
        P = T * P0;
        set(h(1,i),'XData',P(1,:))
        set(h(1,i),'YData',P(2,:))
        set(h(1,i),'ZData',P(3,:))
        axis equal;
        %axis([0 10 0 10 0 500])
    end
    pause(0.001)
end
%%%%%%%%%%%%%

%Zeros=PZeros(SHM);
%line( [ 0 Zeros(1,1)], [0 Zeros(1,2)], [0 Zeros(1,3)]);

%for i=1:1:size(Zeros,1)-1
%    plot3(Zeros(i,1),Zeros(i,2),Zeros(i,3),'*r');
%    line( [ Zeros(i,1) Zeros(i+1,1)], [Zeros(i,2) Zeros(i+1,2)], [Zeros(i,3) Zeros(i+1,3)]);
%end
%plot3(Zeros(i+1,1),Zeros(i+1,2),Zeros(i+1,3),'*r');
%plot3(0,0,0,'*r');


%%M=HipM(:,:,size(HipM,3));
%%AE=AngEuler(M);
%%CD=AE'
