function checkGuidelines(residualFile,kinematicsFile,grfFile,timeRange,allowableZMPdistance,allowableFreeMoment_pErr,allowableForce_pErr)
% Purpose:                  This function tests if the simulation residuals satisfy the guidelines of Gupta et al.
%                           This function is designed to test residuals from OpenSim simulations, users can modify it based on the software package they use.
% 
% Inputs:
% residualFile:             Name of the file that contains the residuals (preferably .sto or .mot)
%                           First column should be time
%                           The residuals should be labeled FX, FY, FZ, MX, MY and MZ
% kinematicsFile:           Name of the file that contains the simulation kinematics (preferably .sto or .mot)
%                           First column should be time
%                           The pelvis coordinates should be labeled pelvis_tx, pelvis_ty and pelvis_tz
% grfFile:                  Name of the file that contains the experimenatal ground reaction forces (GRF) (preferably .sto or .mot)
%                           First column should be time
%                           For each force plate, the order of appearance of the elements should be Forces, Center of pressure (COP) and moments
%                           This code only supports a maximum of 2 force plates. Labels of one of the force plates should start with '1_'
% timeRange:                Time range over which to test if the residuals follow the Gupta et al. guidelines.
%                           Input should be a 1x2 matrix of the form [startTime endTime]
% allowableZMPdistance:     User defined maximum allowable distance between ZMP and COP (in cm) [Value of d (Gupta et al.)]
% allowableFreeMoment_pErr: User defined maximum allowable percent error between ZMP free moment and GRF experimental GRF free moment[Value of b (Gupta et al.)]
% allowableForce_pErr:      User defined maximum allowable percent error between ZMP forces and experimental GRF forces [Value of a (Gupta et al.)]
%                           Input should be a 1x3 matrix of the form [aX aY aZ]
% 
% Outputs:                  A single visual that displays 4 plots:
%                           1. Trajectories of ZMP and COP, with the allowable area for ZMP (Gupta et al.)
%                           2. Trajectories of ZMP free moment and experimental free moment with the allowable range for ZMP free moment (Gupta et al.)
%                           3. Where during the movement each of the guidelines are violated.
%                           4. Experimenat vertical GRF.
% 
% Credits:                  This function was written by Dhruv Gupta


%% spline all data to 0 to 100% of movement and extract relevant data
% Set time vector from 0 to 100%
splineTimeStep=(timeRange(1,2)-timeRange(1,1))/100;
Time=[timeRange(1,1):splineTimeStep:timeRange(1,2)]';
timeVector=[0:100]';

% read and spline the GRF
GRF=read_motionFile(grfFile);
for c=1:GRF.nc
    GRFdata(:,c)=spline(GRF.data(:,1),GRF.data(:,c),Time);
end

% extract data of each force plate separately
c1=1;
c2=1;
GRF1=zeros(101,9);
GRF2=zeros(101,9);
for c=1:GRF.nc
    if strfind(GRF.labels{c},'1_')
        GRF2(:,c2)=GRFdata(:,c);
        c2=c2+1;
    elseif strcmp(GRF.labels{c},'time')
    else
        GRF1(:,c1)=GRFdata(:,c);
        c1=c1+1;
    end
end

% read and spline simulation kinematics
ikMotion = read_motionFile(kinematicsFile);
for c=1:ikMotion.nc
    Motion(:,c)=spline(ikMotion.data(:,1),ikMotion.data(:,c),Time);
end

% extract the pelvis kinematics
positionPelvis = zeros(101, 3);
for c = 1:ikMotion.nc
    if(strcmp(char(ikMotion.labels(c)), 'pelvis_tx'))
        positionPelvis(:, 1) = Motion(:, c);
    elseif(strcmp(char(ikMotion.labels(c)), 'pelvis_ty'))
        positionPelvis(:, 2) = Motion(:, c);
    elseif(strcmp(char(ikMotion.labels(c)), 'pelvis_tz'))
        positionPelvis(:, 3) = Motion(:, c);
    end
end

% read and spline residuals
Residuals = read_motionFile(residualFile);
for c=1:Residuals.nc
    residualData(:,c)=spline(Residuals.data(:,1),Residuals.data(:,c),Time);
end

% extract simulation residusals
residualForces = zeros(101, 3);
for c = 1:Residuals.nc
    if(strcmp(char(Residuals.labels(c)), 'FX'))
        residualForces(:, 1) = residualData(:, c);
    elseif(strcmp(char(Residuals.labels(c)), 'FY'))
        residualForces(:, 2) = residualData(:, c);
    elseif(strcmp(char(Residuals.labels(c)), 'FZ'))
        residualForces(:, 3) = residualData(:, c);
    end
end

residualMoments = zeros(101, 3);
for c = 1:Residuals.nc
    if(strcmp(char(Residuals.labels(c)), 'MX'))
        residualMoments(:, 1) = residualData(:, c);
    elseif(strcmp(char(Residuals.labels(c)), 'MY'))
        residualMoments(:, 2) = residualData(:, c);
    elseif(strcmp(char(Residuals.labels(c)), 'MZ'))
        residualMoments(:, 3) = residualData(:, c);
    end
end

%% find set of forces and moments equivalent to experimental GRF applied at pelvis [FYp and MYp (Gupta et al.)]
idealPelvisForces=GRF1(:,1:3)+GRF2(:,1:3);
idealPelvisMoments=GRF1(:,7:9)+GRF2(:,7:9)+cross(GRF1(:,4:6),GRF1(:,1:3))+cross(GRF2(:,4:6),GRF2(:,1:3))-cross(positionPelvis,idealPelvisForces);

%% find the resultant experimental GRF, that is, net effect of both force plates.
resultantGRF(:,1:3)=GRF1(:,1:3)+GRF2(:,1:3);
resultantGRF(:,4:6)=((GRF1(:,4:6).*GRF1(:,2))+(GRF2(:,4:6).*GRF2(:,2)))./resultantGRF(:,2);
resultantGRF(:,7:9)=GRF1(:,7:9)+GRF2(:,7:9)+cross(GRF1(:,4:6),GRF1(:,1:3))+cross(GRF2(:,4:6),GRF2(:,1:3))-cross(resultantGRF(:,4:6),resultantGRF(:,1:3));

%% Extract the net COP and GRF free moment, based on both force plates. And establish force and free moment thresholds (Gupta et al.)
COP_XZ=[resultantGRF(:,4) resultantGRF(:,6)];
experimentalFreeMoment=resultantGRF(:,8);
freeMomentLimit=(allowableFreeMoment_pErr/100)*max(abs(experimentalFreeMoment));
forceLimits=(allowableForce_pErr/100).*max(abs(resultantGRF(:,1:3)));

%% ZMP computations to find the ZMP forces, ZMP and ZMP free moment and test if they violate any the guidelines (Gupta et al.)
errForces=residualForces>forceLimits;
violateForceGuideline = ~~sum(errForces,2);

forceGroundToPelvis_P = idealPelvisForces + residualForces;
momentGroundToPelvis_P = idealPelvisMoments + residualMoments;

forceGroundToPelvis_G = forceGroundToPelvis_P;
momentGroundToPelvis_G = momentGroundToPelvis_P + cross(positionPelvis, forceGroundToPelvis_P);

ZMP_point_XZ(:,1) = momentGroundToPelvis_G(:, 3) ./ forceGroundToPelvis_G(:, 2); % X_ZMP
ZMP_point_XZ(:,2) = -momentGroundToPelvis_G(:, 1) ./ forceGroundToPelvis_G(:, 2); % Z_ZMP
ZMP_freeMoment = momentGroundToPelvis_G(:, 2) - (forceGroundToPelvis_G(:, 1) .* ZMP_point_XZ(:,2)) + (forceGroundToPelvis_G(:, 3) .* ZMP_point_XZ(:,1)); % freeMoment_ZMP

distZMPCOP=100*(sqrt(sum(((ZMP_point_XZ-COP_XZ).^2),2))); % distance between ZMP and COP in cm

violateZMPpointGuideline=distZMPCOP>allowableZMPdistance;

errFreeMoment = abs(ZMP_freeMoment - experimentalFreeMoment); % error in freeMoment of ZMP

violateFreeMomentGuideline=errFreeMoment>freeMomentLimit;

violateGuideline = ~~(violateForceGuideline + violateZMPpointGuideline + violateFreeMomentGuideline);

%% Visual to display results
pauseTime=mean(diff(Time));

for fr=1:size(Time,1)
    
    % Trajectories of ZMP and COP, with the allowable area for ZMP (Gupta et al.)
    subplot(2,2,1)
    hold off
    plot(COP_XZ(:,2),COP_XZ(:,1),'k','LineWidth',1.5)
    hold on
    plot(ZMP_point_XZ(:,2),ZMP_point_XZ(:,1),'b','LineWidth',1.5)
    hold on
    circle(resultantGRF(fr,6),resultantGRF(fr,4),allowableZMPdistance/100);
    hold on
    if violateZMPpointGuideline(fr,1)==1
        scatter(ZMP_point_XZ(fr,2),ZMP_point_XZ(fr,1),'r')
    else
        scatter(ZMP_point_XZ(fr,2),ZMP_point_XZ(fr,1),'g')
    end
    hold on
    scatter(COP_XZ(fr,2),COP_XZ(fr,1),'kx')
    hold off
    legend('COP','ZMP')
    xlim([min([min(COP_XZ(:,2))-0.2 min(ZMP_point_XZ(:,2))-0.2]) max([max(COP_XZ(:,2))+0.2 max(ZMP_point_XZ(:,2))+0.2])])
    ylim([min([min(COP_XZ(:,1))-0.2 min(ZMP_point_XZ(:,1))-0.2]) max([max(COP_XZ(:,1))+0.2 max(ZMP_point_XZ(:,1))+0.2])])
    axis equal
    xticks({})
    yticks({})
    xlabel('Z direction -->')
    ylabel('X direction -->')
    title('Trajectories of COP vs ZMP')
    
    % Trajectories of ZMP free moment and experimental free moment with the allowable range for ZMP free moment (Gupta et al.)
    subplot(2,2,2)
    hold off
    plot(timeVector(:,1),experimentalFreeMoment,'k','LineWidth',1.5)
    hold on
    plot(timeVector(:,1),ZMP_freeMoment,'b','LineWidth',1.5)
    hold on
    plot(timeVector(:,1),(experimentalFreeMoment+freeMomentLimit),'--k')
    hold on
    plot(timeVector(:,1),(experimentalFreeMoment-freeMomentLimit),'--k')
    hold on
    scatter(timeVector(fr,1),experimentalFreeMoment(fr,1),'k')
    hold on
    if violateFreeMomentGuideline(fr,1)==1
        scatter(timeVector(fr,1),ZMP_freeMoment(fr,1),'r')
    else
        scatter(timeVector(fr,1),ZMP_freeMoment(fr,1),'g')
    end
    legend('Experimental free moment','ZMP free momemt')
    xlim([0 100])
    ylim([min([min(experimentalFreeMoment)-15 min(ZMP_freeMoment)-15]) max([max(experimentalFreeMoment)+15 max(ZMP_freeMoment)+15])])
    xlabel('Time (% Movement)')
    ylabel('Free moment (Nm)')
    title('Experimental vs ZMP free moment')

    % Where during the movement each of the guidelines are violated.
    subplot(2,2,3)
    hold off
    plot(timeVector(:,1),0.25*violateForceGuideline,'k:')
    hold on
    plot(timeVector(:,1),0.5*violateZMPpointGuideline,'k-.')
    hold on
    plot(timeVector(:,1),0.75*violateFreeMomentGuideline,'k--')
    hold on
    plot(timeVector(:,1),violateGuideline,'k','LineWidth',1.5)
    hold on
    scatter(timeVector(fr,1),0.5*violateZMPpointGuideline(fr,1),'k')
    hold on
    scatter(timeVector(fr,1),0.75*violateFreeMomentGuideline(fr,1),'k')
    hold on
    scatter(timeVector(fr,1),0.25*violateForceGuideline(fr,1),'k')
    hold on
    scatter(timeVector(fr,1),violateGuideline(fr,1),'k')
    hold off
    xlim([0 100])
    ylim([0 1.6])
    yticks([0.25 0.5 0.75 1])
    yticklabels({'Force','ZMP','Free moment','Any violation'})
    legend('Force guideline','ZMP guideline','Free moment guideline','Any guideline')
    xlabel('Time (% Movement)')
    ylabel('Guideline violated')
    title('Time of violation of guidelines')

    % Experimenat vertical GRF. 
    subplot(2,2,4)
    hold off
    plot(timeVector(:,1),GRF1(:,2),'c')
    hold on
    plot(timeVector(:,1),GRF2(:,2),'m')
    hold on
    plot(timeVector(:,1),GRF1(:,2)+GRF2(:,2),'k')
    hold on
    scatter(timeVector(fr,1),GRF1(fr,2),'c')
    hold on
    scatter(timeVector(fr,1),GRF2(fr,2),'m')
    hold on
    scatter(timeVector(fr,1),GRF1(fr,2)+GRF2(fr,2),'k')
    xlim([0 100])
    ylim([0 max(resultantGRF(:,2))+200])
    hold off
    legend({'Force plate 1','Force plate 2','Total'});
    xlabel('Time (% Movement)')
    ylabel('Force (N)')
    title('Experimental vertical ground reaction force')
    
    pause(pauseTime)
%     pause(0.001)
end

end

function q = read_motionFile(fname)
% Purpose:  This function reads a file in the format of a SIMM motion file
%           and returns a data structure
%
% Input:    fname is the name of the ascii datafile to be read 
%           ('character array') 
%
% Output:   q returns a structure with the following format:
%				q.labels 	= array of column labels
%				q.data 		= matrix of data
%				q.nr 		= number of matrix rows
%				q.nc 		= number of matrix columns
%
% ASA 12/03
% Modified by Eran Guendelman 09/06
% Open ascii data file for reading.
fid = fopen(fname, 'r');	
if fid == -1								
	error(['unable to open ', fname])		
end
% Process the file header;
% store # data rows, # data columns.
q.nr = 0; % Added to ensure that the q structures from reading a motion file
q.nc = 0; % are always the same, even if nr and nc are different orders in file.
nextline = fgetl(fid);	
while ~strncmpi(nextline, 'endheader', length('endheader'))
	if strncmpi(nextline, 'datacolumns', length('datacolumns'))
		q.nc = str2num(nextline(findstr(nextline, ' ')+1 : length(nextline)));
	elseif strncmpi(nextline, 'datarows', length('datarows'))
		q.nr = str2num(nextline(findstr(nextline, ' ')+1 : length(nextline)));
	elseif strncmpi(nextline, 'nColumns', length('nColumns'))
		q.nc = str2num(nextline(findstr(nextline, '=')+1 : length(nextline)));
	elseif strncmpi(nextline, 'nRows', length('nRows'))
		q.nr = str2num(nextline(findstr(nextline, '=')+1 : length(nextline)));
	end
	nextline = fgetl(fid);
end
% Process the column labels.
nextline = fgetl(fid);
if (all(isspace(nextline))) % Blank line, so the next one must be the one containing the column labels
	nextline = fgetl(fid);
end
q.labels = cell(1, q.nc);
for j = 1:q.nc
	[q.labels{j}, nextline] = strtok(nextline);
end
% Process the data.
% Note:  transpose is needed since fscanf fills columns before rows.
q.data = fscanf(fid, '%f', [q.nc, q.nr])';
fclose(fid);
return;
end

function h = circle(x,y,r)
% plot a circle
hold on
th = 0:pi/50:2*pi;
xunit = r * cos(th) + x;
yunit = r * sin(th) + y;
h = plot(xunit, yunit ,'--');
set(h,'Color', 'k');
hold off
end