function [] = shriners_conversion();
% this function converts Shriner's c3d data to trc and forces files

clc;
clear all;

disp('Pick the c3d file to be converted');
[filename,path] = uigetfile('*.c3d');
[pathstr, name, ext, versn] = fileparts(filename);

% c3d2trc(name)

c3d2forces_schmitz(name,3)

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

function done=c3d2forces_schmitz(file,nfp)
c3dobj = c3dserver;
openc3d(c3dobj,0,[file,'.c3d']);
% Frequency of kinematic data
kinfreq = c3dobj.GetVideoFrameRate;
% Loads the force data
nFirst = c3dobj.GetVideoFrame(0); % First video frame
nLast = c3dobj.GetVideoFrame(1); % Last Video Frame
nRatio = c3dobj.GetAnalogVideoRatio; % Analog / Video Ratio
nFrames = (nLast - nFirst + 1) * nRatio; % Total frames
forcedata=[];

% get the coordinates of the force plate origins (expressed in the force
% plate coordinate system)
nIndex = c3dobj.GetParameterIndex('FORCE_PLATFORM','ORIGIN');
nLength = c3dobj.GetParameterLength(nIndex);
origin_values = [];
for i = 0:nLength-1
    or_val = c3dobj.GetParameterValue(nIndex,i);
    origin_values = [origin_values;or_val];
end

% these are the coordinates of the force plate corners in the lab
% coordinate system
nIndex = c3dobj.GetParameterIndex('FORCE_PLATFORM','CORNERS');
nLength = c3dobj.GetParameterLength(nIndex);
corner_values = [];
for i = 0:nLength-1
    cor_val = c3dobj.GetParameterValue(nIndex,i);
    corner_values = [corner_values;cor_val];
end

for j=0:(nfp-1)
        
        % get force plate forces acting on the person. these are in the lab
        % reference frame.
        fx = c3dobj.GetForceData(0, j, -1, -1);
        fy = c3dobj.GetForceData(1, j, -1, -1);
        fz = c3dobj.GetForceData(2, j, -1, -1);
        fx = cell2mat(fx);
        fy = cell2mat(fy);
        fz = cell2mat(fz);
        fy = -fy;
        fz = -fz;
        
        % get force plate moments (analog data in c3d file in force plate
        % reference frame)
        mx_plat = getanalogchannel_schmitz(c3dobj,['MX' num2str(j+1)]);
        my_plat = getanalogchannel_schmitz(c3dobj,['MY' num2str(j+1)]);
        mz_plat = getanalogchannel_schmitz(c3dobj,['MZ' num2str(j+1)]);
        % these moments are transformed to the lab reference frame
        mx = my_plat; 
        my = mx_plat;
        mz = -mz_plat;
        
        % get z offset from c3d file. this is the z coordinate of the
        % origin of the force platform in the force platform coordinate
        % system.
        row = 3*(j+1);
        off_z = origin_values(row,1);     
        
        % center of pressure is only valid as long as the subject is
        % standing on the plate. therefore, find the 'times' or 'frames'
        % where this is contact on the plate.
        frame = 1:length(fz);
        close;
        figure;
        plot(frame,fz);
        xy = ginput(2);
        frame1 = round(xy(1,1));
        frame2 = round(xy(2,1));
        close;
        
        % calculate center of pressure (COP_FP) in the force plate
        % coordinate system using lab frame axes.
        format long;
        COP_x = [];
        COP_y = [];
        COP_z = [];
        for i = 1:frame1
            COP_x_val = (fx(frame1)*off_z-my(frame1))/(fz(frame1));
            COP_x = [COP_x;COP_x_val];
            COP_y_val = (off_z*fy(frame1)+mx(frame1))/(fz(frame1));
            COP_y = [COP_y;COP_y_val];
            COP_z_val = 0;
            COP_z = [COP_z;COP_z_val];
        end
        for i = (frame1+1):frame2
            COP_x_val = (fx(i)*off_z-my(i))/(fz(i));
            COP_x = [COP_x;COP_x_val];
            COP_y_val = (off_z*fy(i)+mx(i))/(fz(i));
            COP_y = [COP_y;COP_y_val];
            COP_z_val = 0;
            COP_z = [COP_z;COP_z_val];
        end
        for i = (frame2+1):length(fx)
            COP_x_val = (fx(frame2)*off_z-my(frame2))/(fz(frame2));
            COP_x = [COP_x;COP_x_val];
            COP_y_val = (off_z*fy(frame2)+mx(frame2))/(fz(frame2));
            COP_y = [COP_y;COP_y_val];
            COP_z_val = 0;
            COP_z = [COP_z;COP_z_val];
        end
        
        % convert the COP to be relative to the lab origin. 
        row = (j+1)*12 - 11;
        x = [corner_values(row,1);
            corner_values(row+3,1);
            corner_values(row+6,1);
            corner_values(row+9,1)];
        x_off = mean(x);
        
        row = ((j+1)*12 - 11)+1;
        y = [corner_values(row,1);
            corner_values(row+3,1);
            corner_values(row+6,1);
            corner_values(row+9,1)];
        y_off = mean(y);
        
        COFP_x = [];
        COFP_y = [];
        COFP_z = [];
        for i=1:length(fx)
            COFP_x = [COFP_x;(COP_x(i)+x_off)/1000];
            COFP_y = [COFP_y;(COP_y(i)+y_off)/1000];
            COFP_z = [COFP_z;(COP_z(i))/1000];
        end
        % COFP_xyz is in m
                      
        % calculate the free moment about z in the lab reference frame
        % use the center of pressure in the force plate frame
        Mom_z = [];
        for i = 1:length(fx) 
            T_z = (mz(i) - COP_x(i)*fy(i) + COP_y(i)*fx(i));
            Mom_z = [Mom_z;T_z];
        end
        % moments are in N-mm
        % COP is in m
        % forces are in N
        % free moment is in N-mm
        
        if (size(forcedata,1)==0);
            forcedata=[fx fy fz [COFP_x*1000 COFP_y*1000 COFP_z*1000 Mom_z]];
        else
            forcedata=[forcedata fx fy fz [COFP_x*1000 COFP_y*1000 COFP_z*1000 Mom_z]];
        end
end

closec3d(c3dobj);
forcefreq = nRatio*kinfreq;
time=[0:(nFrames-1)]/forcefreq;
% Write the forces file
done = writeForcesFile(time,forcedata,'',file);

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

function done=c3d2trc(file)
c3dobj = c3dserver;
openc3d(c3dobj,0,[file,'.c3d']);
% Loads the x, y and z coordinates of the marker kinematic data
mrkdata = get3dtargets(c3dobj,0);
% Get the marker names
mrknames = fieldnames(mrkdata);
% Extract the x, y, z position data
xyzpos=[];
nmrk=0;
ptr=[];
for i=1:size(mrknames)
    eval(['xyz=mrkdata.',char(mrknames(i)),';']);
    if (size(xyz,2)==3)
        if (size(xyzpos,1)==0)
            xyzpos=xyz;
        else
            xyzpos=[xyzpos xyz];
        end
        nmrk=nmrk+1;
        ptr=[ptr; i];
    end
end
% Convert from mm to meters if that units of coordinates are mm
if(mrkdata.units(1:2)=='mm')
    xyzpos=xyzpos/1000;
end
% Extract only the valid marker names
mrknames=mrknames(ptr);
% Frequency of kinematic data
kinfreq = c3dobj.GetVideoFrameRate;
nStartFrame = c3dobj.GetVideoFrame(0);
nEndFrame = c3dobj.GetVideoFrame(1);
closec3d(c3dobj);

% Write the TRC file
time=[((nStartFrame:nEndFrame)-1)]'/kinfreq;
done = writeTRCFile_schmitz(time,xyzpos,mrknames,'',file);

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

function done = writeTRCFile_schmitz(time,mrkdata,mrknames,directory,file)
T=time(2)-time(1);
f=1/T;
[mk,nk]=size(mrkdata);
nk=nk/3;
if (length(directory)==0)
    fid = fopen([file,'.trc'],'w');
else
    fid = fopen([directory,'/',file,'.trc'],'w');
end    
% write headers
fprintf(fid,['PathFileType  4	(X/Y/Z)\n']);
fprintf(fid,['DataRate	CameraRate	NumFrames	NumMarkers	Units	OrigDataRate	OrigDataStartFrame	OrigNumFrames\n']);
%fprintf(fid,['%7.1f \t%7.1f \t%7d \t%7d mm \t%7.1f \t%7d \t%7d\n'],f,f,mk,nk,f,1,mk);
fprintf(fid,['%7.1f    %7.1f    %7d    %7d mm    %7.1f   %7d   %7d\n'],f,f,mk,nk,f,1,mk);
fprintf(fid,['Frame#\tTime\t']);
for i=1:nk
    if (iscell(mrknames(i)))
        fprintf(fid,'%s\t\t\t',char(mrknames(i,:)));
    else
        fprintf(fid,'%s\t\t\t',mrknames(i,:));
    end
end
fprintf(fid,'\n');
fprintf(fid,'\t\t');
for i=1:nk
    if (i<10)
%        fprintf(fid,'X%1d         Y%1d         Z%1d         ',i,i,i);
        fprintf(fid,'X%1d\tY%1d\tZ%1d\t',i,i,i);
    elseif (i<100)
%        fprintf(fid,'X%2d         Y%2d         Z%2d         ',i,i,i);
        fprintf(fid,'X%2d\tY%2d\tZ%2d\t',i,i,i);
    else
%        fprintf(fid,'X%3d         Y%3d         Z%3d         ',i,i,i);
        fprintf(fid,'X%3d\tY%3d\tZ%3d\t',i,i,i);
    end
end
fprintf(fid,'\n');
fprintf(fid,'\n');
% write actual coordinates  
for i=1:mk                                        % go through all frames
    fprintf(fid,'\t%d',i);                       % print frame number
    fprintf(fid,'\t%.5f',time(i));               % print time
    for j=1:nk                                    % go through all markers
        if j < nk;
            if (isnan(mrkdata(i,(j-1)*3+1)))==1;  % if the number is NaN
                fprintf(fid,'\t\t\t');
            else
                fprintf(fid,'\t%.3f',1000.*mrkdata(i,((j-1)*3+1):j*3));
            end
        else
            if (isnan(mrkdata(i,(j-1)*3+1)))==1;   % if the number is NaN
                fprintf(fid,'\t\t\t\t');
            else
                fprintf(fid,'\t%.3f',1000.*mrkdata(i,((j-1)*3+1):j*3));
                fprintf(fid,'\t');
            end
        end
    end
    fprintf(fid,'\n');  % make a new line for each frame
end
disp(['Wrote ' num2str(mk) ' frames of 3d data (' num2str(nk) ' markers) to ',file]);
fclose(fid);
done=1;
