%% R1000
N=9;
% Dynamics parameters
link_mass = robot.m;
com_pos = robot.com;
% link_inertia = robot.I;
link_inertia(:,:,1) = diag([1,1,1]);
link_inertia(:,:,2) = diag([1,1,1]);
link_inertia(:,:,3) = diag([1,1,1]);
link_inertia(:,:,4) = diag([1,1,1]);
link_inertia(:,:,5) = diag([1,1,1]);
link_inertia(:,:,6) = diag([1,1,1]);
link_inertia(:,:,7) = diag([1,1,1]);
link_inertia(:,:,8) = diag([1,1,1]);
link_inertia(:,:,9) = diag([1,1,1]);

q_sym = sym('q%d',[N,1],'real');
qd_sym = sym('qd%d',[N,1],'real');
q2d_sym = sym('qdd%d',[N,1],'real');
thetalist = qd_sym(1:N);
dthetalist = qd_sym(1:N);
ddthetalist = q2d_sym(1:N);

% 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 = [];
for i = 1:N
    M = robot.T(:,:,i)+transl(com_pos(i));
    Mlist = cat(3, Mlist, M);
end
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
Mlist = cat(3, Mlist, M);

%TODO: Get Slist form DH table method
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;cross(-[0;0;0],[0.2+0.5+0.45+0.12+0.28-0.3157;0;-0.4-0.126493])]];

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

[V1,Vd1,Adgab_mat,Fmat,tau_mat] ...
    = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
    [0;0;-9.8], exf, Mlist, Glist, Slist);
G = FKinSpaceExpand_Sym(Mlist, Slist, thetalist);

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

T=FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
F_Simpack = getSimpackF_Sym(G,T,Mlist,Fmat);

%Gen Files
matlabFunction(F_Simpack,'File',sprintf('autogen/standard_dynamics_%s',opt.robotName),...
    'Vars',{q_sym,qd_sym,q2d_sym});
standard_dynamics_func = sprintf('standard_dynamics_%s',opt.robotName);
% traj
time = 0:1: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);
F_Simpack_list = feval(standard_dynamics_func, q_J',qd_J',qdd_J');

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