%---------------cinematica diferencial integrada ROBONUC-----------------
%Tiago De Almeida Tavares 77001
%com graus de liberdade em x y e teta + manipulador (Aprox1)
addpath ./lib
addpath ./urdf
clc
clear all
close all
% posição incial e final de juntas
   %x y t 1 2 3 4 5 6
Qi=[0 0 0 0 0 0 pi/4 0 0]';
Qf=[1 1 pi/2 0 0 0 0 pi/2 0]';
N=400;

%----------------
Qi_fanuc=Qi(4:9);
Qf_fanuc=Qf(4:9);
MQ_manipulator=LinspaceVect(Qi_fanuc, Qf_fanuc, N); %pontos de passagem das juntas

Qi_p=Qi(1:3);
Qf_p=Qf(1:3);
MQ_platform=LinspaceVect(Qi_p,Qf_p,N);


%----------Matriz DH Manipulador FANUC--------------------
teta1=0;
teta2=0;
teta3=0;
teta4=0;
teta5=0;
teta6=0;
%distancia á origem = 0.330;
a1=0.050;
a2=0.330;
a3=0.035;
d4=0.335;
d6=0.080;

z_platform= 0; %altura da platform

%matriz DH
DH=[teta1 a1 0 pi/2
    teta2+pi/2 a2 0 0
    teta3 a3 0 pi/2
    teta4 0 d4 -pi/2
    teta5 0 0 pi/2
    teta6 0 d6 0];


%posição inicial do ee
MDH=GenerateMultiDH(DH,MQ_manipulator);
AAA=CalculateRobotMotion(MDH);
AA=AAA(:,:,:,1);
%AA = Tlinks(DH);
T_ee=eye(4);
for i=1:size(AA,3)
    T_ee=T_ee*AA(:,:,i);
end
X_ee_i=T_ee(1,4);
Y_ee_i=T_ee(2,4);
Z_ee_i=T_ee(3,4);
[phi_ee_i,teta_ee_i,psi_ee_i]=invRPY(T_ee);

%----- Representação do sistema----
% robot_fanuc=importrobot('lrmate200id_macro.xacro');
% axes = show(robot_fanuc); hold on
% axes.CameraPositionMode= 'auto';

MDH=GenerateMultiDH(DH,MQ_manipulator);
AAA=CalculateRobotMotion(MDH);

% [Ps,Fs]=seixos3(0.3);
% h1=patch('Vertices',Ps(1:3,:)','Faces',Fs,'FaceColor','k'); %%desenha origem
axis equal, axis(0.8*[-1 4 -1 4 -1 2])
hold on, grid on
xlabel('x'), ylabel('y'), zlabel('z');

%desenhar o robot na primeira posicao
x_p_i=MQ_platform(1,1);
y_p_i=MQ_platform(2,1);
teta_p_i=MQ_platform(3,1);
w_T_basePlat=[cos(teta_p_i) -sin(teta_p_i) 0  x_p_i
              sin(teta_p_i) cos(teta_p_i)  0  y_p_i
                    0            0         1  z_platform

                    0            0         0    1];
AA=AAA(:,:,:,1);

for ww=1%:size(AAA,3)
    AA(:,:,ww)=w_T_basePlat*AA(:,:,ww);
end


Org=LinkOrigins(AA);
%Org(:,1)=[x_p_i y_p_i z_platform]';

[P,F]=seixos3(0.1);

h=DrawLinks(Org);
c=['r' 'g' 'b' 'y' 'm' 'r' 'g' 'b'];
H=DrawFrames(AA,P,F,c);

%%  Move parametrizado
%posicao incial e final do ee
Pos_ee_i=[X_ee_i Y_ee_i Z_ee_i phi_ee_i teta_ee_i psi_ee_i]';

%incemento usado
dt=5/N;


Q=Qi;
MQ=zeros(9,N);

for n=1:N
    manipulabilidade_var=Manipulabilidade(2,Q);
    manipulabilidade(:,n)=[n;manipulabilidade_var];
    
    if imag(manipulabilidade_var)~= 0
        manipulabilidade_var=0;
    end
    
    manipulabilidade_withoutCOmplex(:,n)=[n;manipulabilidade_var];
    
    MQ(:,n)=Q;
    Jinv=JacobianInv_robonuc_aprox1(Q);
    
        %Posição: dx dy dz
    dx=0;
    dy=1*dt;
    dz=(-(32*sin(16*n*dt))/25)*dt;
    
    %Orientação: da, db, dc
    da=0;
    db=0;
    dc=0;
    
    
    dr=[dx,dy,dz,da,db,dc]';
    
    dq=Jinv*dr;
    Q=Q+dq;
    AllDq(:,n)=dq; 
    
end

MQ_platform_jinv=MQ(1:3,:);
MQ_manipulator_jinv=MQ(4:9,:);

MDH_jinv=GenerateMultiDH(DH,MQ_manipulator_jinv);
AAA_jinv=CalculateRobotMotion(MDH_jinv);

%posições da base_platform ao longo do tempo 
for pos=1:N
    x_p=MQ_platform_jinv(1,pos);
    y_p=MQ_platform_jinv(2,pos);
    teta_p=MQ_platform_jinv(3,pos);
    Matriz_platform_3d_jinv(:,:,pos)=[cos(teta_p) -sin(teta_p) 0 x_p
                                 sin(teta_p) cos(teta_p) 0 y_p
                                    0            0      1  z_platform
                                    0            0      0  1];
end


sd=0.1; %tempo
%while 1
AnimateRobot(H,AAA_jinv,P,h,sd,Matriz_platform_3d_jinv);
%end
figure(3)
axis([0 n+5, 0 1.5])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade')
plot(manipulabilidade(1,:),manipulabilidade(2,:),'*')


figure(6)
axis([0 n+5, 0 1.6])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade')
plot(manipulabilidade_withoutCOmplex(1,:),manipulabilidade_withoutCOmplex(2,:),'*')


save('sinosoide_1_ab1')
%% Qi e Qf obtido atraves do Jinv (MOVE LINEAR)
%posicao incial e final do ee
Pos_ee_i=[X_ee_i Y_ee_i Z_ee_i phi_ee_i teta_ee_i psi_ee_i]';
Pos_ee_f=[X_ee_i+2 Y_ee_i+2 Z_ee_i+0.3 phi_ee_i teta_ee_i psi_ee_i]';

%incemento usado
dr=(Pos_ee_f-Pos_ee_i)/N;


Q=Qi;
MQ=zeros(9,N);

for n=1:N
    manipulabilidade(:,n)=[n;Manipulabilidade(1,Q)];
    
    q_manipulador=Q(4:end);
    manipulabilidade_manipulador(:,n)=[n;Manipulabilidade_manipulador(1,q_manipulador)];
    
    MQ(:,n)=Q;
    Jinv=JacobianInv_robonuc_aprox1(Q);
    dq=Jinv*dr;
    Q=Q+dq;
    AllDq(:,n)=dq; 
    
end

MQ_platform_jinv=MQ(1:3,:);
MQ_manipulator_jinv=MQ(4:9,:);

MDH_jinv=GenerateMultiDH(DH,MQ_manipulator_jinv);
AAA_jinv=CalculateRobotMotion(MDH_jinv);

%posições da base_platform ao longo do tempo 
for pos=1:N
    x_p=MQ_platform_jinv(1,pos);
    y_p=MQ_platform_jinv(2,pos);
    teta_p=MQ_platform_jinv(3,pos);
    Matriz_platform_3d_jinv(:,:,pos)=[cos(teta_p) -sin(teta_p) 0 x_p
                                 sin(teta_p) cos(teta_p) 0 y_p
                                    0            0      1  z_platform
                                    0            0      0  1];
end


sd=0.1; %tempo
%while 1
AnimateRobot(H,AAA_jinv,P,h,sd,Matriz_platform_3d_jinv);
%end
figure(3)
axis([0 n+5, 0 1.5])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade')
plot(manipulabilidade(1,:),manipulabilidade(2,:),'*')


figure(4)
axis([0 n+5, 0 0.1])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade_manipulador')
plot(manipulabilidade_manipulador(1,:),manipulabilidade_manipulador(2,:),'*')

save('linear_1_ab1')
%% Teste do jacobiano
dq=(Qf-Qi)/N;
Dr=zeros(6,N);
Dr(1,1)=(X_ee_i);
Dr(2,1)=(Y_ee_i);
Dr(3,1)=(z_platform+Z_ee_i);

Q=Qi; %instante incial
for n=2:N
    J=Jacobian_robonuc_aprox1(Q);
    dr=J*dq;
    Q=Q+dq;
    Dr(:,n)=Dr(:,n-1) + dr; %incromentar
end
%plot dos pontos calculados pelos jacobiano
plot3(Dr(1,:),Dr(2,:),Dr(3,:),'*g'), hold on

%% Animação do conjunto

%posições da base_platform ao longo do tempo 
for pos=1:N
    x_p=MQ_platform(1,pos);
    y_p=MQ_platform(2,pos);
    teta_p=MQ_platform(3,pos);
    Matriz_platform_3d(:,:,pos)=[cos(teta_p) -sin(teta_p) 0 x_p
                                 sin(teta_p) cos(teta_p) 0 y_p
                                    0            0      1  z_platform
                                    0            0      0  1];
end

sd=0.1; %tempo
%while 1
AnimateRobot(H,AAA,P,h,sd,Matriz_platform_3d); 
%end