%% R1000
N=9;
% traj
time = 0:0.01:1;
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));
q_J = pi/4*ones(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_;q_J;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
%     link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i);
%     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_CG = [];
% 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_CG = cat(3, Mlist_CG, M);
% end
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
% Mlist_CG = cat(3, Mlist_CG, M);

% Get the com pos transformation in each joint reference frame
% FIXME: BUG here
% Mlist_CG=[];
% for i = 0:N-1
%     if i == 0
%         M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
%     else
%         rotation_i = diag([1,1,1]);
%         for j = 1:i
%             rotation_i = rotation_i*TransToRp(robot.T(:,:,i));
%             rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1));
%         end
%         M = TransInv(RpToTrans(rotation_i,rotation_i*com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,rotation_j*com_pos(:,i+1));
%     end
%     Mlist_CG = cat(3, Mlist_CG, M);
% end
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
% Mlist_CG = cat(3, Mlist_CG, M);

% ct=[];
% Mlist_CG=[];
% for i = 1:N
%     if i == 1
%         ct(:,i) = com_pos_R1(:,i);
%     elseif i< 9
%         ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i);
%     else
%         ct(:,i) = -com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
%     end
%     robot.Home.com(:,i) = ct(:,i);
%     M = RpToTrans(robot.T(1:3,1:3,i),robot.Home.R(:,:,i)*robot.Home.com(:,i));
%     Mlist_CG = cat(3, Mlist_CG, M);
% end
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
% Mlist_CG = cat(3, Mlist_CG, M);

% Mlist_CG=[];
% for i = 1:N
%         if i == 1
%         M = [diag([1,1,1]),com_pos_R1(:,i);zeros(1,3),1];
%     elseif i<=8
%         M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-com_pos_R2(:,i-1)+com_pos_R1(:,i)));
%     elseif i==9
%         M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-[0;0;0.05896]+com_pos_R1(:,i-1)+com_pos_R1(:,i)));
%         end
%         Mlist_CG = cat(3, Mlist_CG, M);
% end
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
% Mlist_CG = cat(3, Mlist_CG, M);

% get the CG at the world base frame
% com_pos_R1 = robot.com_pos_R1;
% com_pos_R2 = robot.com_pos_R2;
% ct=[];
% Mlist_CG_Base=[];
% for i = 1:N
%     if i == 1
%         ct(:,i) = com_pos_R1(:,i);
%     else
%         ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
% %     else
% %         ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
%     end
%     robot.Home.com(:,i) = ct(:,i);
%     M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
%     Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
% end
% % get the CG at the last GC frame
% Mlist_CG=[];
% for i = 1:N
%     if i == 1
%         Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
%     else
%         Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
%     end
% end
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
% Mlist_CG = cat(3, Mlist_CG, 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=robot.slist;

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

for i = 1:length(q_J)
    Js(:,:,i) = JacobianSpace(Slist, thetalist(i,:)');
    VV = Js(:,:,i)*dthetalist(i,:)';
%     [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
%         = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
%         [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
%     G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
%     T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
%     %Want to get the result from TC_delta, which means F at CG represent under frame at the last origin  
%     %why we need Mlist_ED
%     %please explain this more
%     F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
end
% plot Torque
% above 2020b 
% F_Simpack = pagetranspose(F_Simpack);
% below 2020b

% for i = 1:3
%     subplot(3,1,i);
%     hold on;
%     %added minus, so should be the same as simpack
%     plot(time,taumat(i+6,:))
%     xlabel('time(s)')
%     ylabel('Torque(Nm)')
% %     plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
% end
% 
% F_Simpack = permute(F_Simpack,[2 1 3]);
% figure(2)
% for i = 1:3
%     subplot(3,1,i);
%     hold on;
%     %added minus, so should be the same as simpack
%     plot(time,-reshape(F_Simpack(8,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);