% main_opensim_workflow: This file will search for any trc data files in a
% folder and get the force plate data for that trial. Note that this will
% only work after running the Export ASCII plugin from Nexus for all the
% trials that you have collected
clear
clc

this_folder = pwd;
dat_path = input('Analyse which folder??\n','s');
mass = input('What is the subjects body mass (kg)?\n');
height= input('What is the subjects height (m)?\n');
age= input('What is the subjects age (yrs)?\n');
subject_name = input('What is the subjects name?\n','s');

% make sure this directory has the appropriate OpenSIM xms files
functionDirectory='C:\Program Files\OpenSim 1.1\models\HPLmodel';
% 1. HPL_ScaleSet.xml
% 2. HPLgait.osim
% 3. HPL_MeasurementSet.xml
% 4. HPLgait_IK_Settings.xml
% 5. HPL_IKTaskSet.xml
% 6. HPLMarkerSet.xml


% find out how many trc files there are and convert them all to mot files
cd(dat_path)
trcFilenames = dir('*trc');
static_mkr_file = dir('*static*.trc');
nTrials = size(trcFilenames,1);
% return to the original folder containing the code
cd (this_folder)

for i = 1:nTrials
    trc_fname = [dat_path '\' trcFilenames(i).name];
    trialname =strrep(trcFilenames(i).name,'.trc','');
    % Load in trc data to get the position of the calcaneus marker. This is
    % used to determine which foot is on which plate
    trc_data = dlmread(trc_fname,'\t',6,1);
    RCALx=trc_data(:,29);
    RCALz=trc_data(:,31);
    LCALx=trc_data(:,56);
    LCALz=trc_data(:,58);

    marker_time = trc_data(:,1);
    txt_fname = strrep(trc_fname,'.trc','.txt');

    % Determine if a matching text file exists. If not, maybe this is
    % because the trial is a static trial, or no force plate data
    % exist...it could be a knee or hip joint centre trial
% ----------------------------------------------------------------------
    fid1=fopen(txt_fname);
    if fid1 == -1
        disp('No Force Plate data exists for the following trial:')
        txt_fname
    else
        fclose(fid1);

        file_infoname = strrep(trc_fname,'.trc','.info');
        file_info = dlmread(file_infoname,'\t',1,1);

        sample_rate =file_info(1);
        analog_rate=file_info(2);
        timediff = 1/analog_rate;
        rotation_matrix = [file_info(3) file_info(4) file_info(5);file_info(6) file_info(7) file_info(8);file_info(9) file_info(10) file_info(11)];

        data=dlmread(txt_fname,'\t',2,0);

        time = data(:,1);
        time_end_sec = time(end)/analog_rate;
        time=[timediff:timediff:time_end_sec];

        ForcePlate1_forces=data(:,8:10);
        ForcePlate1_COP=data(:,2:4)/1000; % COP data has to be in metres for OpenSIM
        ForcePlate2_forces=data(:,20:22);
        ForcePlate2_COP=data(:,14:16)/1000;
        ForcePlate3_forces=data(:,32:34);
        ForcePlate3_COP=data(:,26:28)/1000;


        % Calculate the free moment of each force plate
        % -----------------------------------------------------------------
        calculate_free_moment;


        % Concatenate the forces, moments, and COP data, depending on
        % which force plates are used
        % -----------------------------------------------------------------
        
        % Correct order is vector1, point1, vector2, point2, torque1,
        % torque2)
        
        Forces =[];
        COP=[];
        ForceThreshold = 10;
        if mean(data(:,10)) > ForceThreshold
            Forces=[Forces ForcePlate1_forces ForcePlate1_moments];
            COP=[COP ForcePlate1_COP];
        end
        if mean(data(:,22)) > ForceThreshold
            Forces=[Forces ForcePlate2_forces ForcePlate2_moments];
            COP=[COP ForcePlate2_COP];
        end
        if mean(data(:,34)) > ForceThreshold
            Forces=[Forces ForcePlate3_forces ForcePlate3_moments];
            COP=[COP ForcePlate3_COP];
        end

        % filter the forces
        % -----------------------------------------------------------------
        if analog_rate > sample_rate
            cut_off_frequency = 15;
            Wn=cut_off_frequency/(analog_rate/2);
            % describe filter characteristics using matlab function for 2nd order
            % Butterworth filter
            [B,A]=butter(2,Wn);
            Forces_filt = filtfilt(B,A,Forces);

            % filter the COP
            cut_off_frequency = 100;
            Wn=cut_off_frequency/(analog_rate/2);
            [B,A]=butter(2,Wn);
            COP_filt = filtfilt(B,A,COP);
        else % dont filter, as analog rate is same as sample rate
            Forces_filt = Forces;
            COP_filt = COP;
        end

        % find when vertical forces drop below threshold, and then make
        % all of the force and COP values 0.0 at these points
        % -----------------------------------------------------------------
        force_zero=find(Forces_filt(:,3)<ForceThreshold);
        Forces_filt(force_zero,1:6)=0.0;
        COP_filt(force_zero,1:3)=0.0;

        if size(Forces_filt,2)>6
            force_zero=find(Forces_filt(:,9)<ForceThreshold);
            Forces_filt(force_zero,7:12)=0.0;
            COP_filt(force_zero,4:6)=0.0;
        end

        % Now transform the forces and COP data into a coordinate system that
        % is consistent with the marker data and OpenSIM
        % -----------------------------------------------------------------
        force_data = [Forces_filt COP_filt];
        [nt, nc] = size(force_data);

        % First, rotate 90deg about X, so that Y is the vertical axis
        rot90aboutX = [1 0 0;  0 0 1; 0 -1 0];
        if rem(nc,3)
            error('Force columns must have 3 components each.');
        end
        for I = 1:nc/3
            rotated_force_data(:,3*I-2:3*I) = [rot90aboutX*force_data(:,3*I-2:3*I)']';
        end

        % now rotate again, depending on the direction of movement, so that x
        % is always in the positive direction
        for I = 1:nc/3
            rotated_force_data(:,3*I-2:3*I) = [rotation_matrix*rotated_force_data(:,3*I-2:3*I)']';
        end

        grf_complete = [time' rotated_force_data];
        [m,n]=size(grf_complete);

        % now that the force data is in the same coordinate system as the
        % marker data, search for when the L and R CAL markers are closest
        % to the COP for each plate
        % -----------------------------------------------------------------
        determine_force_connection;

        % Now output the data to a mot file
        % -----------------------------------------------------------------
        write_motion_file;

        % To DO: automatically generate open sim files for each trial and run IK
        % and ID
        clear rotated_force_data

    end
end


% % open static.trc file and create OpenSIM Scaling .xml files
static_trc_fname = [dat_path '\' static_mkr_file(1).name];
% write_OpenSim_ScaleWorkflow;

% search for hip joint and knee joint calibration files
% ----------------------------------------------------------------------
cd(dat_path)
L_HiptrcFilenames = dir('*L*hip*trc');
R_HiptrcFilenames = dir('*R*hip*trc');
squat_trcFilenames = dir('*squat*trc');
cd (this_folder)

% Find left hip joint centre location
% ----------------------------------------------------------------------
if isempty(L_HiptrcFilenames)
    disp('No left hip calibration file found...will use regression equation for hip joint centre')
    % open up the static trial instead and place hip centres using regression equation
    LHJC_location=findhipcentre_regression(static_trc_fname,'L_HJC');
else
    L_HiptrcFilename = [dat_path '\' L_HiptrcFilenames(1).name];
    LHJC_location =findhipcentre(L_HiptrcFilename,'L_HJC');
end

% Find right hip joint centre location
% ----------------------------------------------------------------------
if isempty(R_HiptrcFilenames)
    disp('No right hip calibration file found...will use regression equation for hip joint centre')
    % open up the static trial instead and place hip centres using regression equation
    RHJC_location=findhipcentre_regression(static_trc_fname,'R_HJC');
else
    R_HiptrcFilename = [dat_path '\' R_HiptrcFilenames(1).name];
    RHJC_location =findhipcentre(R_HiptrcFilename,'R_HJC');
end

% Find knee joint helical axes location
% ----------------------------------------------------------------------
if isempty(squat_trcFilenames)
    disp('No knee calibration [squat] file found...will use anatomical markers for knee joint centre')
    % open up the static trial instead and place knee centre between
    % femoral condyles

else
    squat_trcFilename=[dat_path '\' squat_trcFilenames(1).name];
     LHAxes =findkneeaxes(squat_trcFilename,'L')
     RHAxes =findkneeaxes(squat_trcFilename,'R')
end

% Reopen the static trc file and append the hip, knee, and ankle joint centres 
% ----------------------------------------------------------------------
append_joint_centres_to_static_trial;

clear all