% output .sd file, .mod file, .bio file _dyn.c
% input subject ID, subject mass (kg), footlength (m), gender (female/male), marker diameter (m), shoes mass (kg)
% read data from stationary .trc file

clear;

forceFreq = 1000; % Hz No need to resample
videoFreq = 240; % Hz
videoTime = 2; % s

path1 = 'C:\XuemeiSVNsimtk\MMS\mdl\legs3d\sub';  % Windows

lineNum = videoFreq*videoTime;

% Input for this subject
subID = 701; %input('Subject ID?\n');
subMass = 67.6; %input('subject mass (kg)\n'); without shoe
subHeight = 1.74;
shoeMass = 1.2; %input('shoes mass (kg)\n'); two shoes
footLength = 0.3065; %input('Right foot length (m)\n');
Rmalheel = 0.06; %input('Right lateral malleolus to the back of heel (m)\n');
Lmalheel = 0.06; %input('Left lateral malleolus to the back of heel (m)\n');
Dmarker = 0.016; %input('marker diameter (m)\n');
gender = 'M'; %input('subject gender: female(F) or male(M)\n', 's');

% load de Leva Table: de Leva, J. Biomech 1996, 29(9), 1223-1231, to calculate Mass, Inertia
constant = [0.4346  0.4486  0.372   0.347   0.191;      %1 trunk
            0.1416	0.4095	0.329	0.329	0.149;      %2 Rthigh
            0.0433	0.4395	0.251	0.246	0.102;      %3 Rshank
            0.0137	0.4415	0.257	0.245	0.124;      %4 Rfoot
            0.1416	0.4095	0.329	0.329	0.149;      %5 Lthigh
            0.0433	0.4395	0.251	0.246	0.102;      %6 Lshank
            0.0137	0.4415	0.257	0.245	0.124;];    %7 Lfoot
                
% read data from stationary .trc file -- sub***s.trc
path2 = 's';
path3 = [path1, num2str(subID), path2];
fileName1 = [path3 '.trc'];
fid = fopen(fileName1, 'r');
for i = 1:6
    fgetl(fid);
end
SmarkerData = fscanf(fid, '%g', [113 240]);
Smarker = SmarkerData'./1000;
fclose(fid);

% split and average data for each marker
Sternum = mean(Smarker(:,3:5));
Rshoulder = mean(Smarker(:,6:8));
Lshoulder = mean(Smarker(:,9:11));
Sacrum = mean(Smarker(:,12:14));
RasisB = mean(Smarker(:,15:17));
LasisB = mean(Smarker(:,18:20));
Rgretro = mean(Smarker(:,21:23));
Lgretro = mean(Smarker(:,24:26));
Rantfem = mean(Smarker(:,27:29));
Lantfem = mean(Smarker(:,30:32));
Rlatcond = mean(Smarker(:,33:35));
Llatcond = mean(Smarker(:,36:38));
Rtibtub = mean(Smarker(:,39:41));
Ltibtub = mean(Smarker(:,42:44));
Ranttib = mean(Smarker(:,45:47));
Lanttib = mean(Smarker(:,48:50));
Rlattib = mean(Smarker(:,51:53));
Llattib = mean(Smarker(:,54:56));
Rpostcalc = mean(Smarker(:,57:59));
Lpostcalc = mean(Smarker(:,60:62));
Rlatcalc = mean(Smarker(:,63:65));
Llatcalc = mean(Smarker(:,66:68));
RmetaV = mean(Smarker(:,69:71));
LmetaV = mean(Smarker(:,72:74));
Rtoe = mean(Smarker(:,75:77));
Ltoe = mean(Smarker(:,78:80));
Rsupfoot = mean(Smarker(:,81:83));
Lsupfoot = mean(Smarker(:,84:86));
Vertex = mean(Smarker(:,87:89));
Rmedcond = mean(Smarker(:,90:92));
Lmedcond = mean(Smarker(:,93:95));
Rlatmal = mean(Smarker(:,96:98));
Llatmal = mean(Smarker(:,99:101));
Rmedmal = mean(Smarker(:,102:104));
Lmedmal = mean(Smarker(:,105:107));
Rasis = mean(Smarker(:,108:110));
Lasis = mean(Smarker(:,111:113));

% joint center for .mod in Mocap Solver

Mshoulder = (Rshoulder+Lshoulder)/2.0;

% Bell et al., J. Biomech 1990, 23(6), 617-621
% Right Hip Joint Center - RHJC
% Left Hip Joint Center - LHJC
Dasis = norm(Rasis-Lasis);
RHJC = [(Rasis(1,1)-0.14*Dasis), Rgretro(1,2), (Rasis(1,3)-0.3*Dasis)];
LHJC = [(Lasis(1,1)+0.14*Dasis), Lgretro(1,2), (Lasis(1,3)-0.3*Dasis)];

% Right Knee Joint Center - RKJC
% Left Knee Joint Center - LKJC
% KJC - midpoint of latcond and medcond
RKJC = (Rlatcond+Rmedcond)/2.0;
LKJC = (Llatcond+Lmedcond)/2.0;

% Right Ankle Joint Center - RAJC
% Left Ankle Joint Center - LAJC
% AJC - midpoint of latmal and medmal
% J. Biomech. 32(1999), 877-889
RAJC = (Rlatmal+Rmedmal)/2.0;
LAJC = (Llatmal+Lmedmal)/2.0;

% de Leva
% Center Mass of trunk - CMT
MHJC = (LHJC+RHJC)/2.0;
CMT = [MHJC(1,1), MHJC(1,2), MHJC(1,3)+(1-0.5976)*(Sternum(1,3)-MHJC(1,3))];
% 0.5976 from head data on de Leva

% this vertex is not on top of head
% MHJC
Vertex = [MHJC(1,1), MHJC(1,2), subHeight];
%Vertex = [Vertex(1,1), Vertex(1,2), subHeight];

% Mass according to de Leva Table
% Mass according to de Leva Table: Rfemur, Rtibia, foot, Lfemur, Ltibia,
% UpperTrunk, MidTrunk, Pelvis6
mass(1) = subMass*constant(1,1); % trunk mass, only subMass*constant
mass(2) = subMass*constant(2,1); % right thigh
mass(3) = subMass*constant(3,1); % right shank
mass(4) = subMass*constant(4,1) + shoeMass*0.5; % right foot
mass(5) = subMass*constant(5,1); % left thigh
mass(6) = subMass*constant(6,1); % left shank
mass(7) = subMass*constant(7,1) + shoeMass*0.5; % left foot

RTalusMass = 0; 
LTalusMass = 0;

% segment length
seglen(1) = 0.5319; %m from de Leva Table
seglen(2) = norm(RHJC-RKJC); % RfemurLength
seglen(3) = norm(RKJC-RAJC); % RtibiaLength
seglen(4) = footLength;
seglen(5)= norm(LHJC-LKJC); % LfemurLength 
seglen(6) = norm(LKJC-LAJC); % LtibiaLength
seglen(7) = footLength;
Rtaluslength = 0.01; 
Ltaluslength = 0.01;

% Inertia according to de Leva Table
% for Inertia of Pelvis and Femur, mass is the total one, including WM
for i =1:7
    Ixx(i) = mass(i)*((seglen(i)*constant(i,4)).^2);
    Iyy(i) = mass(i)*((seglen(i)*constant(i,3)).^2);
    Izz(i) = mass(i)*((seglen(i)*constant(i,5)).^2);
end
% right foot (segment 4) is special, long axis is Y
Ixx(4) = mass(4)*((seglen(4)*constant(4,4)).^2);
Iyy(4) = mass(4)*((seglen(4)*constant(4,5)).^2);
Izz(4) = mass(4)*((seglen(4)*constant(4,3)).^2);
% left foot (segment 7) is special, long axis is Y
Ixx(4) = mass(7)*((seglen(7)*constant(7,4)).^2);
Iyy(4) = mass(7)*((seglen(7)*constant(7,5)).^2);
Izz(4) = mass(7)*((seglen(7)*constant(7,3)).^2);
for i = 1:7
    IMatrix(:,:,i) = [Ixx(i),    0,       0;
                      0,     Iyy(i),    0;
                      0,         0,   Izz(i)];
end

mass(1,1) = subMass*0.2*(1-2*constant(2,1)-2*constant(3,1)-2*constant(4,1));
% Pelvis, 20% of submass-thigh-shank-foot
PelvisWM = subMass*0.8*(1-2*constant(2,1)-2*constant(3,1)-2*constant(4,1));
% 80% of upper body

fid = fopen('sub701.mod', 'w');

fprintf(fid, 'ACCURACY\t\t0.000100\n');
fprintf(fid, 'MAX_ITERATIONS\t\t200\n');
fprintf(fid, 'DEBUG_LEVEL\t\t0\n');
fprintf(fid, 'OUTPUTFILES\t\tDOF RMS NOHTR\n');
fprintf(fid, 'INIT_POSE\t\tsub701s\n');
fprintf(fid, 'GRAVITY Z\n\n\n');

fprintf(fid, 'SEGMENT Pelvis1\n');
fprintf(fid, '                 PARENT        \tGLOBAL \n');
fprintf(fid, '                 JOINT_TYPE    \tSLIDER \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', CMT(1,1), CMT(1,2), CMT(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 1 0 0 \n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');

fprintf(fid, 'SEGMENT Pelvis2\n');
fprintf(fid, '                 PARENT        \tPelvis1\n');
fprintf(fid, '                 JOINT_TYPE    \tSLIDER \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', CMT(1,1), CMT(1,2), CMT(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 0 1 0 \n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');

fprintf(fid, 'SEGMENT Pelvis3\n');
fprintf(fid, '                 PARENT        \tPelvis2\n');
fprintf(fid, '                 JOINT_TYPE    \tSLIDER \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', CMT(1,1), CMT(1,2), CMT(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 0 0 1 \n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');

fprintf(fid, 'SEGMENT Pelvis4\n');
fprintf(fid, '                 PARENT        \tPelvis3\n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE  \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', CMT(1,1), CMT(1,2), CMT(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 0 0 1 \n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');

fprintf(fid, 'SEGMENT Pelvis5\n');
fprintf(fid, '                 PARENT        \tPelvis4\n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE  \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', CMT(1,1), CMT(1,2), CMT(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 1 0 0 \n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');

fprintf(fid, 'SEGMENT Pelvis6\n');
fprintf(fid, '                 PARENT        \tPelvis5\n');
fprintf(fid, '                 MARKER        \tLasisB \tWEIGHT 1.0 \n');
fprintf(fid, '                 MARKER        \tRasisB \tWEIGHT 1.0 \n');
fprintf(fid, '                 MARKER        \tSacrum   \tWEIGHT 1.0 \n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE  \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', CMT(1,1), CMT(1,2), CMT(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 0 1 0 \n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');
fprintf(fid, '                 MASS          \t %6.4f \n', mass(1,1));
fprintf(fid, '                 INERTIA       \n');
fprintf(fid, '                               \t%6.4f\t\t%6.4f\t\t%6.4f\n', IMatrix(:,:,1));

fprintf(fid, 'SEGMENT RHip1 \n');
fprintf(fid, '                 PARENT        \tPelvis6\n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE  \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', RHJC(1,1), RHJC(1,2), RHJC(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 1 0 0\n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');

fprintf(fid, 'SEGMENT RHip2 \n');
fprintf(fid, '                 PARENT        \tRHip1 \n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', RHJC(1,1), RHJC(1,2), RHJC(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 0 1 0 \n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');

fprintf(fid, 'SEGMENT RFemur\n');
fprintf(fid, '                 PARENT        \tRHip2 \n');
fprintf(fid, '                 MARKER        \tRgretro   \tWEIGHT 1.0 \n');
fprintf(fid, '                 MARKER        \tRantfem   \tWEIGHT 1.0 \n');
fprintf(fid, '                 MARKER        \tRlatcond  \tWEIGHT 1.0 \n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', RHJC(1,1), RHJC(1,2), RHJC(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 0 0 1 \n');
fprintf(fid, '                 CENTER_OF_MASS\t %6.4f \n', constant(2,2));
fprintf(fid, '                 MASS          \t %6.4f \n', mass(1,2));
fprintf(fid, '                 INERTIA       \n');
fprintf(fid, '                               \t%6.4f\t\t%6.4f\t\t%6.4f\n', IMatrix(:,:,2));

fprintf(fid, 'SEGMENT RTibia\n');
fprintf(fid, '                 PARENT        \tRFemur \n');
fprintf(fid, '                 MARKER        \tRtibtub  \tWEIGHT 1.0 \n');
fprintf(fid, '                 MARKER        \tRanttib  \tWEIGHT 1.0 \n');
fprintf(fid, '                 MARKER        \tRlattib  \tWEIGHT 1.0 \n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', RKJC(1,1), RKJC(1,2), RKJC(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 1 0 0 \n');
fprintf(fid, '                 CENTER_OF_MASS\t %6.4f \n', constant(3,2));
fprintf(fid, '                 MASS          \t %6.4f \n', mass(1,3));
fprintf(fid, '                 INERTIA       \n');
fprintf(fid, '                               \t%6.4f\t\t%6.4f\t\t%6.4f\n', IMatrix(:,:,3));

fprintf(fid, 'SEGMENT RTalus\n');
fprintf(fid, '                 PARENT        \tRTibia\n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE      \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', RAJC(1,1), RAJC(1,2), RAJC(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 1 0 0 \n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');

fprintf(fid, 'SEGMENT RFoot\n');
fprintf(fid, '                 PARENT        \tRTalus\n');
fprintf(fid, '                 MARKER        \tRpostcalc \tWEIGHT  1.0\n');
fprintf(fid, '                 MARKER        \tRlatcalc  \tWEIGHT  1.0\n');
fprintf(fid, '                 MARKER        \tRsupfoot  \tWEIGHT  1.0\n');
fprintf(fid, '                 MARKER        \tRmetaV    \tWEIGHT  1.0\n');
fprintf(fid, '                 MARKER        \tRtoe      \tWEIGHT  1.0\n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE      \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', RAJC(1,1), RAJC(1,2), RAJC(1,3)-0.01);
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t -0.29  0.69  0.66 \n');
fprintf(fid, '                 ORIGIN        \tGLOBAL \t %6.4f %6.4f %6.4f \n', Rpostcalc(1,1), RAJC(1,2)-Rmalheel, Rpostcalc(1,3));
fprintf(fid, '                 ENDPOINT      \tGLOBAL \t %6.4f %6.4f %6.4f \n', Rpostcalc(1,1), RAJC(1,2)-Rmalheel+footLength, Rpostcalc(1,3));
fprintf(fid, '                 CENTER_OF_MASS\t %6.4f \n', constant(4,2));
fprintf(fid, '                 MASS          \t %6.4f \n', mass(1,4));
fprintf(fid, '                 INERTIA       \n');
fprintf(fid, '                               \t%6.4f\t\t%6.4f\t\t%6.4f\n', IMatrix(:,:,4));

fprintf(fid, 'SEGMENT LHip1 \n');
fprintf(fid, '                 PARENT        \tPelvis6\n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE  \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', LHJC(1,1), LHJC(1,2), LHJC(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 1 0 0\n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');

fprintf(fid, 'SEGMENT LHip2 \n');
fprintf(fid, '                 PARENT        \tLHip1 \n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', LHJC(1,1), LHJC(1,2), LHJC(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 0 1 0 \n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');

fprintf(fid, 'SEGMENT LFemur\n');
fprintf(fid, '                 PARENT        \tLHip2 \n');
fprintf(fid, '                 MARKER        \tLgretro    \tWEIGHT 1.0 \n');
fprintf(fid, '                 MARKER        \tLantfem    \tWEIGHT 1.0 \n');
fprintf(fid, '                 MARKER        \tLlatcond   \tWEIGHT 1.0 \n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', LHJC(1,1), LHJC(1,2), LHJC(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 0 0 1 \n');
fprintf(fid, '                 CENTER_OF_MASS\t %6.4f \n', constant(5,2));
fprintf(fid, '                 MASS          \t %6.4f \n', mass(1,5));
fprintf(fid, '                 INERTIA       \n');
fprintf(fid, '                               \t%6.4f\t\t%6.4f\t\t%6.4f\n', IMatrix(:,:,5));

fprintf(fid, 'SEGMENT LTibia\n');
fprintf(fid, '                 PARENT        \tLFemur \n');
fprintf(fid, '                 MARKER        \tLtibtub  \tWEIGHT 1.0 \n');
fprintf(fid, '                 MARKER        \tLanttib  \tWEIGHT 1.0 \n');
fprintf(fid, '                 MARKER        \tLlattib  \tWEIGHT 1.0 \n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL \t %6.4f %6.4f %6.4f \n', LKJC(1,1), LKJC(1,2), LKJC(1,3));
fprintf(fid, '                 JOINT_AXIS    \tVECTOR \t 1 0 0 \n');
fprintf(fid, '                 CENTER_OF_MASS\t %6.4f \n', constant(6,2));
fprintf(fid, '                 MASS          \t %6.4f \n', mass(1,6));
fprintf(fid, '                 INERTIA       \n');
fprintf(fid, '                               \t%6.4f\t\t%6.4f\t\t%6.4f\n', IMatrix(:,:,6));

fprintf(fid, 'SEGMENT LTalus\n');
fprintf(fid, '                 PARENT        \tLTibia\n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE      \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL     \t %6.4f %6.4f %6.4f \n', LAJC(1,1), LAJC(1,2), LAJC(1,3));
fprintf(fid, '                 JOINT_AXIS    \t\tVECTOR     \t 1 0 0 \n');
fprintf(fid, '                 CENTER_OF_MASS\t 0 \n');

fprintf(fid, 'SEGMENT LFoot\n');
fprintf(fid, '                 PARENT        \tLTalus\n');
fprintf(fid, '                 MARKER        \tLpostcalc   \tWEIGHT  1.0\n');
fprintf(fid, '                 MARKER        \tLlatcalc    \tWEIGHT  1.0\n');
fprintf(fid, '                 MARKER        \tLsupfoot    \tWEIGHT  1.0\n');
fprintf(fid, '                 MARKER        \tLmetaV      \tWEIGHT  1.0\n');
fprintf(fid, '                 MARKER        \tLtoe        \tWEIGHT  1.0\n');
fprintf(fid, '                 JOINT_TYPE    \tHINGE      \n');
fprintf(fid, '                 JOINT_POSITION\tGLOBAL     \t %6.4f %6.4f %6.4f \n', LAJC(1,1), LAJC(1,2), LAJC(1,3)-0.01);
fprintf(fid, '                 JOINT_AXIS    \tVECTOR     \t 0.29  0.69  0.66 \n');
fprintf(fid, '                 ORIGIN        \tGLOBAL \t %6.4f %6.4f %6.4f \n', Lpostcalc(1,1), LAJC(1,2)-Lmalheel, Lpostcalc(1,3));
fprintf(fid, '                 ENDPOINT      \tGLOBAL \t %6.4f %6.4f %6.4f \n', Lpostcalc(1,1), LAJC(1,2)-Lmalheel+footLength, Lpostcalc(1,3));
fprintf(fid, '                 CENTER_OF_MASS\t %6.4f \n', constant(7,2));
fprintf(fid, '                 MASS          \t %6.4f \n', mass(1,7));
fprintf(fid, '                 INERTIA       \n');
fprintf(fid, '                               \t%6.4f\t\t%6.4f\t\t%6.4f\n', IMatrix(:,:,7));

fprintf(fid, 'FINISH\n');

fclose(fid);

system('C:\cygwin\cygwin < cygwin_solver.txt');
Mskel('sub701.vis');

