%% Linear 1 (Abordagem1 )
clc
clear all
load('linear_1_ab1')
%close all

figure('units','normalized','outerposition',[0 0 1 1])
axis equal, axis(1*[0 3 0 3 -0.1 1.5])
hold on, grid on
xlabel('x'), ylabel('y'), zlabel('z');

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);


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

%view(-142,22)
sd=0.01; %tempo
view(-45,22) %foto
view(-8,19) %video
view(0,90)
%while 1
pause(1)
AnimateRobot(H,AAA_jinv,P,h,sd,Matriz_platform_3d_jinv);
%end

for angle=1:1:360
    view(angle,16)
    pause(0.001)
end
%%

pause(1)
figure(3)
axis([0 n+5, 0 1.4])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade')
plot(manipulabilidade(1,:),manipulabilidade(2,:),'b*')

figure(4)
axis([0 n+5, 0 0.02])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade do braço')
plot(manipulabilidade_manipulador(1,:),manipulabilidade_manipulador(2,:),'b*')


%% Linear 1 (Abordagem2 )
clc
clear all
close all
load('linear_1_ab2')



figure('units','normalized','outerposition',[0 0 1 1])
axis equal, axis(1*[0 3 0 3 -0.1 1.5])
hold on, grid on
xlabel('x'), ylabel('y'), zlabel('z');


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


Org=LinkOrigins(AA);
[P,F]=seixos3(0.1);

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



MDH=GenerateMultiDH(DH,MQ,t);
AAA=CalculateRobotMotion(MDH);

% qff=MQ(:,end);
% jacobiano_qf=Jacobian_robonuc_aprox2(qff);
% jacobiano_qf_t=jacobiano_qf.';
% Manipulabilidade=sqrt(det(jacobiano_qf*jacobiano_qf_t))
sd=0.01; %tempo
view(-45,22) %foto
view(-8,19) %video
view(0,90)
%while 1
%view(-135,8)
pause(1)
AnimateRobot(H,AAA,P,h,sd); 
%end

for angle=1:1:360
    view(angle,16)
    pause(0.001)
end

%%
figure(4)
axis([0 n+5, 0 1.4])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade')
plot(manipulabilidade(1,:),manipulabilidade(2,:),'b*')


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

figure(6)
axis([0 n+5, 0 0.02])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade do braço')
plot(manipulabilidade_manipulador(1,:),manipulabilidade_manipulador(2,:),'b*')
