clear,clc
% This batch processing code targets to use OpenSim's built-in inverse
% dynamics function to get joint moment data for subject trials.

% Auther: Xiaowen Chen
% Plug-in Gait Optimization Lab
% Mechanical Engineering Department
% Pennsylvania State Universeity

% Part of the code is built from OpenSim official matlab scripts:
% https://simtk-confluence.stanford.edu:8443/display/OpenSim/Scripting+with+Matlab

% ################## Before you run the code ##############
% Setup OpenSim working environment:
% https://simtk-confluence.stanford.edu:8443/display/OpenSim/Scripting+with+Matlab
% Install Jonathan's MoCapTools and add the folder and subfolders to path
% https://github.com/JonathanCamargo/MoCapTools.git
% Install James Conder's Gaussian Filter Script and add to path
% https://www.mathworks.com/matlabcentral/fileexchange/43182-gaussfilt-t-z-sigma?s_tid=prof_contriblnk
% (if you don't want to use gaussian filter, please comment the gaussfilt()
% function and uncomment the filtfilt() function below it.

% Please make sure that the two .xml setup files named 'sidf.xml' and
% 'sidsetting.xml' are downloaded. Please finish the inverse kinematics of
% the trials if you have not done so and plot the hip, knee, and ankle
% joint angles and exported them. Also make sure that the .mot file that
% contains the ground reaction force is available before running this code.
% ## Please make sure that there are no more .mot files other than the grf
% data.
% ## Please also make sure that you name the Inverse Kinematics Trials data
% in format of t*ao, for example, t1ao, t2ao, ..., t10ao.
% ## Please make sure each .mot file corresponds to an inverse kinematics data file
% ##############################################################


subjectdir = uigetdir('testData', 'Select the folder that contains the current subject data');
% subject folder directory
ikdir=uigetdir(subjectdir, 'Select the folder that contains the IK data');
% inverse kinematics results file folder directory
grfdir=uigetdir(subjectdir, 'Select the folder that contains the .mot force plate data');
% ground reaction force
[fileorig1 origpath1 index]=uigetfile('*.xml','select the setup force file');
% inverse dynamics force setup file
[fileorig2 origpath2 index]=uigetfile('*.xml','select the IK setup file');
% inverse dynamics setup file
ik=dir(fullfile(ikdir, '*ao*'));
for i=1:length(ik)
    name=ik(i).name;
    aonum(i)=sscanf(name,'t%dao'); % scan for all the t*ao IK results files
end
[n,ind]=sort(aonum); % sort the IK results files to prevent mistakes such
% that t10ao rank higher than t1ao
grf=dir(fullfile(grfdir, '*.mot*')); % auto-find the ground reaction force files with .mot
% file extension

%

% This section do gaussian filtering of the ground reaction force signal
% and save it to a new .mot file under the IK results folder
if length(n)==length(grf)
    for i=n
        trial=[grfdir '\' grf(i).name];
        file1=Osim.readMOT(trial);
        data1=table2array(file1);
        tv=data1(:,1);
        tstart(i)=tv(1);
        tend(i)=tv(end);
        for j=2:size(data1,2)
            %data1(:,j)=gaussfilt(tv,data1(:,j),0.01);
            fc=5;
            fs=1/0.0017;
            wn=fc*2/fs;
            [b,a]=butter(1,wn);
            data1(:,j)=filtfilt(b,a,data1(:,j));
        end
        vvx1=data1(:,2);
        vvy1=data1(:,3);
        vvz1=data1(:,4);
        vpx1=data1(:,5);
        vpy1=data1(:,6);
        vpz1=data1(:,7);
        vtx1=data1(:,8);
        vty1=data1(:,9);
        vtz1=data1(:,10);
        vvx2=data1(:,11);
        vvy2=data1(:,12);
        vvz2=data1(:,13);
        vpx2=data1(:,14);
        vpy2=data1(:,15);
        vpz2=data1(:,16);
        vtx2=data1(:,17);
        vty2=data1(:,18);
        vtz2=data1(:,19);
        time=tv;
        ground_force1_vx=-vvz1;
        ground_force1_vy=vvy1;
        ground_force1_vz=vvx1;
        ground_force1_px=-vpz1;
        ground_force1_py=vpy1;
        ground_force1_pz=vpx1;
        ground_force2_vx=-vvz2;
        ground_force2_vy=vvy2;
        ground_force2_vz=vvx2;
        ground_force2_px=-vpz2;
        ground_force2_py=vpy2;
        ground_force2_pz=vpx2;
        ground_torque1_x=-vtz1;
        ground_torque1_y=vty1;
        ground_torque1_z=vtx1;
        ground_torque2_x=-vtz2;
        ground_torque2_y=vty2;
        ground_torque2_z=vtx2;
        file3=table(time,ground_force1_vx,ground_force1_vy, ...
            ground_force1_vz,ground_force1_px,ground_force1_py, ...
            ground_force1_pz,ground_torque1_x,ground_torque1_y, ...
            ground_torque1_z,ground_force2_vx,ground_force2_vy, ...
            ground_force2_vz,ground_force2_px,ground_force2_py, ...
            ground_force2_pz,ground_torque2_x,ground_torque2_y, ...
            ground_torque2_z);
        grffile=[ikdir '\trial_' num2str(i) '_grf.mot'];
        grfstr(i)=string(grffile);
        mot=Osim.writeMOT(file3,'FilePath',grffile);
    end
else
    disp('the files number do not match');
end

%

% This section changes the preset force file and copy the changed force
% setup file to the IK results directory
for i=n
    idffile(i)=string([ikdir '\' sprintf('idf%d.xml',i)]);
    copyfile([origpath1 '\' fileorig1],idffile(i));
    S=fileread(idffile(i));
    % Replace preset strings with custom file name and directory:
    S=strrep(S, 'grf_file',grfstr(i));
    fname=fopen(idffile(i),'w');
    fwrite(fname, S);
    fclose(fname);
end

%

% This section changes the preset IK setup file and copy the changed
% setup file to the IK results directory
for i=n
    idsfile(i)=string([ikdir '\' sprintf('idsetting%d.xml',i)]);
    copyfile([origpath2 '\' fileorig2],idsfile(i));
    S=fileread(idsfile(i));
    % Replace preset strings with custom files' names and directories:
    S=strrep(S, 'subjectdir', subjectdir);
    S=strrep(S, 'sidf', idffile(i));
    S=strrep(S, 't.mot', [ikdir '\' sprintf('t%d.mot',i)]);
    S=strrep(S, 'id.sto', sprintf('id%d.sto',i));
    S=strrep(S, 'bf.sto', sprintf('bf%d.sto',i));
    S=strrep(S, 'start', num2str(tstart(i)));
    S=strrep(S, 'end', num2str(tend(i)));
    fname=fopen(idsfile(i),'w');
    fwrite(fname, S);
    fclose(fname);
end

%

% This section sets up the OpenSim environment and runs the inverse
% dynamics
import org.opensim.modeling.*
[mfile modelpath index]=uigetfile('*.osim','select the IK model file');
% Get the OpenSim model file for the inverse dynamics
for i=n
    sDir= ikdir; %Subject directory
    % Choose a generic setup file to work from
    Setup=idsfile(i);
    idTool = InverseDynamicsTool(Setup);
    % Load the model and initialize
    model = Model(mfile);
    model.initSystem();
    % Tell Tool to use the loaded model
    idTool.setModel(model);
    idTool.run();
end