%#########################################################
% This codes compute the center of mass of a man and ZMP trajectory.
% Human_cm has data of center of mass.
%#########################################################
clear all
close
clc
FullFileName = '07_01.c3d';
asf_file = '07.asf';
T1=0;%850; %no of frames if T1=0 then all else T1
% ------ Checks -------
 show_nodes = 0;
 show_cm = 0;
%-----------------------

% -------- Mass -----------
%% Head
Head_M = 7.5;
Neck_M = 1.0;
%% Arms
R_Uparm_M = 2.0;
R_Foarm_M = 2.0;
L_Uparm_M = 2.0
L_Foarm_M = 2.0;
%% Body
Body_M = 18.5;
Waist_M = 10.0;
%% Legs
R_Thigh_M = 5.5;
R_Shin_M = 5.5;
R_Foot_M = 3.5;
L_Thigh_M = 3.5;
L_Shin_M = 1.0;
L_Foot_M = 1.0;

Human_M = Head_M + Neck_M + R_Uparm_M + R_Foarm_M + L_Uparm_M + L_Foarm_M ...
        + Body_M + Waist_M + R_Thigh_M + R_Shin_M + R_Foot_M + L_Thigh_M ...
        + L_Shin_M + L_Foot_M;
%---------------------------

%----- Data aquisition from c3d file ----

[Markers,VideoFrameRate,AnalogSignals,AnalogFrameRate,Event,ParameterGroup,CameraInfo,ResidualError]=readC3D(FullFileName);
fd=fopen(FullFileName,'r');
data=fread(fd);
fclose(fd);
data=char(data);
k2=find(data==':');
marknames=[data(k2(2:1+41)+1) data(k2(2:1+41)+2) data(k2(2:1+41)+3) data(k2(2:1+41)+4)];


%% mm --> m
Markers = Markers./1000;
%----------------------------------------

%---- Check nodes---------
if show_nodes == 1 
    datax = Markers(1,:,1);
    datay = Markers(1,:,2);
    dataz = Markers(1,:,3);
    plot3(datax,datay,dataz,'.')
    hold on
    axis equal
    for i = 1:length(datax)
        i
        plot3(datax(i),datay(i),dataz(i),'r.')
        pause
        plot3(datax(i),datay(i),dataz(i),'b.')
    end
    hold off
end

% ----- Assigne Markers data to each part------

for j=1:41
    st=[marknames(j,:) '=' '(Markers(:,' num2str(j) ',:));'];
    eval(st);
end

%---------------------------------------

%----- Compute center of mass-----------------
% Head
Head_cm = squeeze(mean([RFHD,LFHD,RBHD,LBHD],2));
Neck_cm = (Head_cm - squeeze(mean([CLAV,C7],2)))./4 + squeeze(mean([CLAV,C7],2));
% Arms
R_Uparm_cm = squeeze(mean([RSHO,RELB],2));
R_Foarm_cm = squeeze(mean([RFIN,RELB],2));
L_Uparm_cm = squeeze(mean([LSHO,LELB],2));
L_Foarm_cm = squeeze(mean([LFIN,LELB],2));
% Body
Body_cm = squeeze(mean([T10,STRN],2));
Body_cm(:,3) = squeeze(mean([CLAV(:,:,3),STRN(:,:,3)],2));
Waist_cm = squeeze(mean([RFWT,LFWT,LBWT,RBWT],2));
% Legs
R_Thigh_cm = squeeze(mean([RKNE,mean([RBWT,RFWT],2)],2));
R_Shin_cm = squeeze(mean([RKNE,RANK],2));
R_Foot_cm = squeeze(mean([RTOE,RHEE,RMT5],2));
L_Thigh_cm = squeeze(mean([LKNE,mean([LBWT,LFWT],2)],2));
L_Shin_cm = squeeze(mean([LKNE,LANK],2));
L_Foot_cm = squeeze(mean([LTOE,LHEE,LMT5],2));

UpperBody_cm = ( Head_cm*Head_M + Neck_cm*Neck_M ...
          + R_Uparm_cm*R_Uparm_M + R_Foarm_cm*R_Foarm_M ...
          + L_Uparm_cm*L_Uparm_M + L_Foarm_cm*L_Foarm_M ...
          + Body_cm*Body_M + Waist_cm*Waist_M)/(Head_M + Neck_M + R_Uparm_M + R_Foarm_M + L_Uparm_M + L_Foarm_M + Body_M + Waist_M);
UpperBody_M = Head_M + Neck_M + R_Uparm_M + R_Foarm_M...
                + L_Uparm_M + L_Foarm_M + Body_M + Waist_M;

Human_cm = ( Head_cm*Head_M + Neck_cm*Neck_M ...
          + R_Uparm_cm*R_Uparm_M + R_Foarm_cm*R_Foarm_M ...
          + L_Uparm_cm*L_Uparm_M + L_Foarm_cm*L_Foarm_M ...
          + Body_cm*Body_M + Waist_cm*Waist_M ...
          + R_Thigh_cm*R_Thigh_M + R_Shin_cm*R_Shin_M + R_Foot_cm*R_Foot_M ...
          + L_Thigh_cm*L_Thigh_M + L_Shin_cm*L_Shin_M + L_Foot_cm*L_Foot_M) ...
          /Human_M;
      
Human_cm = squeeze(Human_cm);
%---------------------------------------------
%##################################################
% compute  zmp
%##################################################

%--------------------
N = 7; %number_of_links
T = length(Human_cm);
TS = 1/120;%0.01%? time_stsp;
G = 9.81;
g = [0;0;-G];
%--------------------

%---- Moment of Inertia ---
I = readasf(asf_file);

%---- Vector r, acceleration--------
clear r

m(1) = UpperBody_M;
m(2) = R_Thigh_M;
m(3) = L_Thigh_M;
m(4) = R_Shin_M;
m(5) = L_Shin_M;
m(6) = R_Foot_M;
m(7) = L_Foot_M ;

r(:,:,1) = UpperBody_cm;
r(:,:,2) = R_Thigh_cm;
r(:,:,3) = L_Thigh_cm;
r(:,:,4) = R_Shin_cm;
r(:,:,5) = L_Shin_cm;
r(:,:,6) = R_Foot_cm;
r(:,:,7) = L_Foot_cm;

clear r_acc
for i = 1:7
    r_vel(:,:,i) = (diff(r(:,:,i),1,1)/TS);%,3,'moving');
    r_acc(:,:,i) = (diff(r_vel(:,:,i),1,1)/TS);%,3,'moving');
end
%-----------------------------------
%-- Get zmp dot dot ---------------
ini_pos = zeros(1,1,3);
% Human_cm_vel = diff((Human_cm),2,1)/dt;
Human_cm_acc = diff(Human_cm,2,1)/(TS*TS);
Zcm = Human_cm_acc(:,3);

%------------Angular dispacements--------------
for t=1:length(Markers)
    J_ang_Lthigh(t) = vectorangle(L_Thigh_cm(t,:),Waist_cm(t,:),UpperBody_cm(t,:));
    J_ang_Rthigh(t) = vectorangle(R_Thigh_cm(t,:),Waist_cm(t,:),UpperBody_cm(t,:));
    J_ang_Lshin(t) = vectorangle(L_Shin_cm(t,:),(squeeze(LKNE(t,:,:)))',L_Thigh_cm(t,:));
    J_ang_Rshin(t) = vectorangle(R_Shin_cm(t,:),(squeeze(RKNE(t,:,:)))',R_Thigh_cm(t,:));
    J_ang_Lank(t) = vectorangle(L_Foot_cm(t,:),(squeeze(LANK(t,:,:)))',L_Shin_cm(t,:));
    J_ang_Rank(t) = vectorangle(R_Foot_cm(t,:),(squeeze(RANK(t,:,:)))',R_Shin_cm(t,:));
end
ang_upperbody = zeros(size(J_ang_Lthigh));
ang_Lthigh = J_ang_Lthigh;
ang_Rthigh = J_ang_Rthigh;
ang_Lshin = J_ang_Lthigh + J_ang_Lshin;
ang_Rshin = J_ang_Rthigh + J_ang_Rshin;
ang_Lfoot = J_ang_Lthigh + J_ang_Lshin + J_ang_Lank;
ang_Rfoot = J_ang_Rthigh + J_ang_Rshin + J_ang_Rank;

TS = 1/120;
SPAN = 3;
angvel_upperbody = smooth(diff(ang_upperbody)/TS,SPAN,'moving');
angvel_Lthigh = smooth(diff(ang_Lthigh)/TS,SPAN,'moving');
angvel_Rthigh = smooth(diff(ang_Rthigh)/TS,SPAN,'moving');
angvel_Lshin = smooth(diff(ang_Lshin)/TS,SPAN,'moving');
angvel_Rshin = smooth(diff(ang_Rshin)/TS,SPAN,'moving');
angvel_Lfoot = smooth(diff(ang_Lfoot)/TS,SPAN,'moving');
angvel_Rfoot = smooth(diff(ang_Rfoot)/TS,SPAN,'moving');

clear w
w(:,1) = diff(angvel_upperbody)/TS;
w(:,2) = diff(angvel_Lthigh)/TS;
w(:,3) = diff(angvel_Rthigh)/TS;
w(:,4) = diff(angvel_Lshin)/TS;
w(:,5) = diff(angvel_Rshin)/TS;
w(:,6) = diff(angvel_Lfoot)/TS;
w(:,7) = diff(angvel_Rfoot)/TS;
for i = 1:N
    ww = [repmat([0],length(w(:,i)),1) w(:,i) repmat([0],length(w(:,i)),1)];
    ang_acc(:,:,i) = ww;
end


%---------------------------------------------
sum_denomi = 0;
ZMP0 = [];
for t = 1:T-2
    for i = 1:N
        sum_denomi = sum_denomi + cross(r(t,:,i)',(m(i)* (r_acc(t,:,i)'-g))) ...
                                + I(:,:,i)*ang_acc(t,:,i)';
    end
    ZMP0 = [ZMP0 sum_denomi/(Human_M*(Zcm(t)+ G))];
    sum_denomi = 0;
end
ZMP(1,:) = -ZMP0(2,:);
ZMP(2,:) = ZMP0(1,:);
ZMP(3,:) = ZMP0(3,:);

if T1==0
    x_zmp = ZMP(1,:);
    y_zmp = ZMP(2,:);
    x_cm = Human_cm(:,1);
    y_cm = Human_cm(:,2);
else
    x_zmp = ZMP(1,1:T1);
    y_zmp = ZMP(2,1:T1);
    x_cm = Human_cm(1:T1,1);
    y_cm = Human_cm(1:T1,2);
end

% ----Show Results of center of mass-----

if show_cm == 1
    for i = 1:length(Markers)
                plot3(ZMP(1,i),ZMP(2,i),ZMP(3,i),'k*','Markersize',10); 
        hold on
        axis equal
        grid on
        plot3(Markers(i,:,1),Markers(i,:,2),Markers(i,:,3),'.');
      
        plot3(Head_cm(i,1),Head_cm(i,2),Head_cm(i,3),'r.');
        plot3(Neck_cm(i,1),Neck_cm(i,2),Neck_cm(i,3),'r.');
        plot3(R_Uparm_cm(i,1),R_Uparm_cm(i,2),R_Uparm_cm(i,3),'r.');
        plot3(R_Foarm_cm(i,1),R_Foarm_cm(i,2),R_Foarm_cm(i,3),'r.');
        plot3(L_Uparm_cm(i,1),L_Uparm_cm(i,2),L_Uparm_cm(i,3),'r.');
        plot3(L_Foarm_cm(i,1),L_Foarm_cm(i,2),L_Foarm_cm(i,3),'r.');
        plot3(Body_cm(i,1),Body_cm(i,2),Body_cm(i,3),'r.');
        plot3(UpperBody_cm(i,1),UpperBody_cm(i,2),UpperBody_cm(i,3),'r*');
        plot3(Waist_cm(i,1),Waist_cm(i,2),Waist_cm(i,3),'r.');
        plot3(R_Thigh_cm(i,1),R_Thigh_cm(i,2),R_Thigh_cm(i,3),'r.');
        plot3(R_Shin_cm(i,1),R_Shin_cm(i,2),R_Shin_cm(i,3),'r.');
        plot3(R_Foot_cm(i,1),R_Foot_cm(i,2),R_Foot_cm(i,3),'r.');
        plot3(L_Thigh_cm(i,1),L_Thigh_cm(i,2),L_Thigh_cm(i,3),'r.');
        plot3(L_Shin_cm(i,1),L_Shin_cm(i,2),L_Shin_cm(i,3),'r.');
        plot3(L_Foot_cm(i,1),L_Foot_cm(i,2),L_Foot_cm(i,3),'r.'); 
        plot3(Human_cm(i,1),Human_cm(i,2),Human_cm(i,3),'g.','Markersize',20)
        hold off
        pause
    end
    hold off
end
figure(1);
plot(x_zmp,y_zmp,'b.',x_cm,y_cm,'r.'); axis equal;

%-------------computing torques---------------------
J_angvel_Lthigh = smooth(diff(J_ang_Lthigh)/TS,SPAN,'moving');
J_angvel_Rthigh = smooth(diff(J_ang_Rthigh)/TS,SPAN,'moving');
J_angvel_Lshin = smooth(diff(J_ang_Lshin)/TS,SPAN,'moving');
J_angvel_Rshin = smooth(diff(J_ang_Rshin)/TS,SPAN,'moving');
J_angvel_Lank = smooth(diff(J_ang_Lank)/TS,SPAN,'moving');
J_angvel_Rank = smooth(diff(J_ang_Rank)/TS,SPAN,'moving');

J_angacc_Lthigh = smooth(diff(J_angvel_Lthigh)/TS,SPAN,'moving');
J_angacc_Rthigh = smooth(diff(J_angvel_Rthigh)/TS,SPAN,'moving');
J_angacc_Lshin = smooth(diff(J_angvel_Lshin)/TS,SPAN,'moving');
J_angacc_Rshin = smooth(diff(J_angvel_Rshin)/TS,SPAN,'moving');
J_angacc_Lank = smooth(diff(J_angvel_Lank)/TS,SPAN,'moving');
J_angacc_Rank = smooth(diff(J_angvel_Rank)/TS,SPAN,'moving');

for t = 11:56
    stateVec = [J_ang_Rank(t) J_ang_Rshin(t) J_ang_Rthigh(t) J_ang_Lthigh(t) J_ang_Lshin(t)  J_ang_Lank(t) ...
    J_angvel_Rank(t) J_angvel_Rshin(t) J_angvel_Rthigh(t) J_angvel_Lthigh(t) J_angvel_Lshin(t)  J_angvel_Lank(t) ...
    J_angacc_Rank(t) J_angacc_Rshin(t) J_angacc_Rthigh(t) J_angacc_Lthigh(t) J_angacc_Lshin(t)  J_angacc_Lank(t)];
    taus(:,t-10) = calcTauFourLinkBody(stateVec);
end
t=linspace(11*TS,56*TS,size(taus,2));
figure(gcf+1);
plot(t,(taus')/10); xlabel('time (sec)'); ylabel('Torque (N-m)');
legend('\tau_{Rankle}','\tau_{Rknee}','\tau_{Rhip}','\tau_{Lhip}','\tau_{Lknee}');
title('Computed Torques using Inverse Dynamics for a segment of single support gait');