clc,close all,clear all

% DH parameters
%
% L(1) = Link('revolute','d',0,'a',0,'alpha',pi/2,'m',0);
% L(2) = Link('revolute','d',0,'a',5,'alpha',0,'m',2,'I',diag([0.1 0.1 0.1]));
% L(3) = Link('revolute','d',0,'a',5,'alpha',0,'m',1,'I',diag([0.01 0.01 0.01]));
%
% Robot = SerialLink(L,'name','Anthr');
%
%
% Robot.plot([0 0 0],'workspace',[-20 20 -20 20 -20 20]);
% Robot.teach

%
% q_config = [0 pi/10 pi/2];
% q_config_d = [0 0 0];
%
% % Robot.plot(q_config);
%
% T = Robot.fkine(q_config);
%
% q = Robot.ikine(T,'mask',[1 1 1 0 0 0]);
%
% B = Robot.inertia(q_config);
% C = Robot.coriolis([q_config q_config_d]);
% g_vec = Robot.gravload(q_config);
%
% tic
% [t,q,qd] = Robot.fdyn(10, [], q_config);
% toc
%
% hold on
%
% t = [0:.05:1];
%
% q_0 = [0 0 0];
% q_r1 = [0 -pi/10 0]
% % Robot.plot(q_r1);
% t = [0:.05:1];
% q_r2 = [pi -pi/10 pi/3];
% % Robot.plot(q_r2);
%
% [q,qd,qdd] = jtraj(q_0, q_r2, t); % compute joint coordinate trajectory
% plot(t, qdd)
% tau = Robot.rne(q, qd, qdd); % compute inverse dynamics

% B(q)qdd+C(q,qd)qd+g(q) = tao
%
% qdd = inv(B)(tao-g(q)-C(q,qd)qd)



%spherical arm

% L(1) = Link('revolute','d',0,'a',0,'alpha',-pi/2);
% L(2) = Link('revolute','d',5,'a',0,'alpha',pi/2);
% L(3) = Link('prismatic','theta',0,'a',0,'alpha',0);
%
% L(2).qlim = [-pi/100 pi/2];
% L(3).qlim = [0.1 2]; %limit for prismaic joint
%
% Robot = SerialLink(L,'name','Spher');
%
%
% Robot.plot([0 0 0]);
% Robot.teach

L(1) = Link('prismatic','theta',0,'a',0,'alpha',pi/2,'m',0); %only to set first prismatic joint to Y direction
L(2) = Link('prismatic','theta',0,'a',0,'alpha',-pi/2,'m',0);
L(3) = Link('revolute','d',0,'a',0,'alpha',pi/2,'m',2);
L(4) = Link('revolute','d',0,'a',5,'alpha',0,'m',2);
L(5) = Link('revolute','d',0,'a',0,'alpha',pi/2,'m',1);
L(6) = Link('revolute','d',3,'a',0,'alpha',-pi/2,'m',3);
L(7) = Link('revolute','d',0,'a',0,'alpha',pi/2,'m',2);
L(8) = Link('revolute','d',3,'a',0,'alpha',0,'m',1);
L(1).qlim = [0.0 0.0];
L(2).qlim = [0.0 20.0];
Robot = SerialLink(L,'name','Suturing');

q0 = [0 1 0 0 0 0 0 2];
qe = [0 5 -pi/2 0 0 pi/2 pi/4 2];

% L(1) = Link('revolute','d',0,'a',0,'alpha',pi/2,'m',0);
% L(2) = Link('revolute','d',0,'a',5,'alpha',0,'m',2);
% L(3) = Link('revolute','d',0,'a',0,'alpha',pi/2,'m',1);
% L(4) = Link('revolute','d',3,'a',0,'alpha',-pi/2,'m',3);
% L(5) = Link('revolute','d',0,'a',0,'alpha',pi/2,'m',2);
% L(6) = Link('revolute','d',3,'a',0,'alpha',0,'m',1);
% Robot = SerialLink(L,'name','Suturing');
%
% q0 = [0 0 0 0 0 2];
% qe = [-pi/2 0 0 pi/2 pi/4 2];

T0 = Robot.fkine(q0);
Te = Robot.fkine(qe);

t = [0:.056:2];     % generate a time vector
q = jtraj(q0, qe, t); %joint trajectory /// cartesian traj ctraj



T = Robot.fkine(q);


%plots position, velocities, accelerations XYZ TODO dynamics
f1 = figure('Name','p,v,a XYZ','NumberTitle','off');
p = T.transl;
subplot(3,1,1)
plot(t, p(:,1))
xlabel('Time (s)');
ylabel('X (m)')
subplot(3,1,2)
plot(t, p(:,2))
xlabel('Time (s)');
ylabel('Y (m)')
subplot(3,1,3)
plot(t, p(:,3))
xlabel('Time (s)');
ylabel('Z (m)')

%Plots position X relative to Z
f2 = figure('Name','X relative to Z','NumberTitle','off');
subplot(1,1,1)
plot(p(:,1), p(:,3));
xlabel('X (m)')
ylabel('Z (m)')
grid

%plot interactive bot
f3 = figure('Name','Interactive plot','NumberTitle','off');
Robot.teach

f4 = figure('Name','init pos','NumberTitle','off');
init = SerialLink(Robot, 'name', 'init');
init.plot(q0);

f5 = figure('Name','final position plot','NumberTitle','off');
final = SerialLink(Robot, 'name', 'final');
final.plot(qe);

% f6 = figure('Name','movement loop plot','NumberTitle','off');
% loop = SerialLink(Robot, 'name', 'loop');
% loop.plot(q, 'loop');




%trajectory planning
T1 = transl(5.0, -5.0,-6.0);
T2 = transl(5.0, -8.0,-6.0);

T = ctraj(T1, T2, 2);
q = Robot.ikine(T, 'pinv', 'tol', 1e-3);

% q0 = [0 0 0 0 0 2];
% qe = [-pi/2 0 0 pi/2 pi/4 2];
% T0 = Robot.fkine(q0);
% Te = Robot.fkine(qe);
% T = ctraj(T0, Te, 50);
% q = Robot.ikine(T, 'pinv');


f7 = figure('Name','trajectory TEST loop plot','NumberTitle','off');
trajBot = SerialLink(Robot, 'name', 'trajBot');
trajBot.plot(q, 'loop');

%%%%%note: can use STL files with plot3D !!!!!!!!!!!

%%%how to know if a point is reachable ??? No way to know except computing
%%%but basic rules would be: point < radius sum(Ai)