%% R1000
N=9;
% traj
time = 0:0.01:2;
f=1;
q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(2*pi*f*time);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
zero_ = zeros(1,length(q_J));
% Dynamics parameters
link_mass = robot.m;
com_pos = robot.com;
link_inertia = robot.I;

thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';

% Get general mass matrix
Glist=[];
for i = 1:N
    Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
    Glist = cat(3, Glist, Gb);
end

% Get the com pos transformation in each joint reference frame
Mlist_GC = [];
for i = 0:N-1
    if i == 0
        M = robot.T(:,:,i+1)*transl(com_pos(:,i+1));
    else
        M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
    end
    Mlist_GC = cat(3, Mlist_GC, M);
end
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
Mlist_GC = cat(3, Mlist_GC, M);

% Get the end efforce transformation in each joint reference frame
Mlist_ED = [];
for i = 1:N
    M = robot.T(:,:,i);
    Mlist_ED = cat(3, Mlist_ED, M);
end
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
Mlist_ED = cat(3, Mlist_ED, M);

%TODO: Get Slist form DH table method
% RRRRRRRRP
Slist=[[0;0;1;0;0;0],...
    [0;-1;0;cross(-[0;-1;0],[0.2;0;0])]...
    [0;-1;0;cross(-[0;-1;0],[0.2+0.5;0;0])]...
    [0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45;0;0])]...
    [0;0;1;cross(-[0;0;1],[0.2+0.5+0.45+0.12;0;0])]...
    [1;0;0;cross(-[1;0;0],[0.2+0.5+0.45+0.12+0.28;0;0])]...
    [0;0;-1;cross(-[0;0;-1],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
    [0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
    [0;0;0;1;0;0]];

Vlinear=sym(zeros(3,3));
J=sym(zeros(6,N));
exf=[0;0;0;0;0;0];

for i = 1:length(q_J)
    [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i)] ...
        = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
        [0;0;-9.8], exf, Mlist_GC, Glist, Slist);
    G(:,:,:,i) = FKinSpaceExpand(Mlist_GC, Slist, thetalist(i,:)');
    T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
    F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
end
% plot Torque
F_Simpack = pagetranspose(F_Simpack);
figure(1)
for i = 1:3
    subplot(3,1,i);
    hold on;
    plot(time,-reshape(F_Simpack(1,i,:),[1,length(F_Simpack)]))
%     plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
end

% Use Body Twist cal linear vel, but can't cal the end frame vel
% [V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
%     [0;0;0], exf, Mlist, Glist, Slist);
% j=1;
% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
% j=2;
% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23);