clear all
close all
clc

Qi_0=[0 0 0 0 0 0]';
Qf_0=[0 0 0 0 0 0]';
N_0=71;
Qi_1=[0 0 0 0 0 0]';
Qf_1=[0 0 pi/2 pi/2 0 0]';
N_1=182-72;
Qf_2=[0 0 0 0 0 0]';
Qi_2=[0 0 pi/2 pi/2 0 0]';
N_2=301-183;
Qf_3=[0 0 0 0 0 0]';
Qi_3=[0 0 0 0 0 0]';
N_3=702-302;

%ex2
MQ_0=Linespacevect(Qi_0,Qf_0,N_0);
MQ_1=Linespacevect(Qi_1,Qf_1,N_1);
MQ_2=Linespacevect(Qi_2,Qf_2,N_2);
MQ_3=Linespacevect(Qi_3,Qf_3,N_3);

MQ=[MQ_0 MQ_1 MQ_2 MQ_3];

l2=1;
l3=1;
l4=1;
l5=1;

DH=[pi/2    0       l2      pi/2
    pi/2    l3      0       0
    0       0       0       pi/2
    pi      0       l4      pi/2
    pi      0       0       pi/2
    0       0       l5      0];


%ex3
MDH=GenerateMultiDH(DH,MQ);

%ex5
AAA=CalculateRobotMotion(MDH);

ang_euler.fanuc.fi=[];
ang_euler.fanuc.teta=[];
ang_euler.fanuc.psi=[];

T=[0 0 1 0
   1 0 0 0
   0 1 0 0
   0 0 0 1];
for i=1:size(AAA,4)
    
    %Multiplicação de cada transformação para obter a transformação da
    %origem ao punho a cada instante
    A=eye(4);
    for n=(1:size(AAA,3))
        A=A*AAA(:,:,n,i);
    end
    A=A*T;
    
    %Calculo dos angulos de euler
    fi=atan2(A(2,1),A(1,1));
    ang_euler.fanuc.fi(end+1)=fi;

    teta=atan2(-A(3,1),A(1,1)*cos(fi)+A(2,1)*sin(fi));
    ang_euler.fanuc.teta(end+1)=teta;

    psi=atan2(A(3,2),A(3,3));
    ang_euler.fanuc.psi(end+1)=psi;

end