%rotate_trc.m
% this file will open up a static trial.trc file and append the hip, knee,
% and ankle joint centres to this trial

% read in trc file
clear
clc

file_name = 'static';
fid = fopen([file_name,'.trc'], 'r');
line1 = fgetl(fid);
line2 = fgetl(fid);
line3 = fgetl(fid);
line4 = fgetl(fid);
line5 = fgetl(fid);
line6 = fgetl(fid);

fclose(fid);

mkr_data = dlmread([file_name,'.trc'],'\t',6,0);
[nt nc] = size(mkr_data);

% arrange all the marker data, based on their order, which is pre-assigned
% from the PECS code 'Export_static_trc.m'
RASI=mkr_data(:,3:5);
LASI=mkr_data(:,6:8);
% SACR=mkr_data(:,9:11);
% RTH1=mkr_data(:,12:14);
% RTH2=mkr_data(:,15:17);
% RTH3=mkr_data(:,18:20);
% RLFC=mkr_data(:,21:23);
% RMFC=mkr_data(:,24:26);
% RTB1=mkr_data(:,27:29);
% RTB2=mkr_data(:,30:32);
% RTB3=mkr_data(:,33:35);
% RLMAL=mkr_data(:,36:38);
% RMMAL=mkr_data(:,39:41);
% RCAL=mkr_data(:,42:44);
% RTOE=mkr_data(:,45:47);
% RMT5=mkr_data(:,48:50);
% LTH1=mkr_data(:,51:53);
% LTH2=mkr_data(:,54:56);
% LTH3=mkr_data(:,57:59);
% LLFC=mkr_data(:,60:62);
% LMFC=mkr_data(:,63:65);
% LTB1=mkr_data(:,66:68);
% LTB2=mkr_data(:,69:71);
% LTB3=mkr_data(:,72:74);
% LLMAL=mkr_data(:,75:77);
% LMMAL=mkr_data(:,78:80);
% LCAL=mkr_data(:,81:83);
% LTOE=mkr_data(:,84:86);
% LMT5=mkr_data(:,87:89);
% LACR=mkr_data(:,90:92);
% RACR=mkr_data(:,93:95);
% CLAV=mkr_data(:,96:98);
% C7=mkr_data(:,99:101);




% % now append two sets of marker data for OpenSIM scaling.
% new_mkr_data=[RASI LASI SACR RTH1 RTH2 RTH3 RLFC RMFC RTB1 RTB2 RTB3 ...
%     RLMAL RMMAL RCAL RTOE RMT5 LTH1 LTH2 LTH3 LLFC LMFC LTB1 LTB2 LTB3 ...
%     LLMAL LMMAL LCAL LTOE LMT5 LACR RACR CLAV C7];

% mkr_labels ={'RASI' 'LASI' 'SACR' 'RTH1' 'RTH2' 'RTH3' 'RLFC' 'RMFC'...
%     'RTB1' 'RTB2' 'RTB3' 'RLMAL' 'RMMAL' 'RCAL' 'RTOE' 'RMT5'...
%     'LTH1' 'LTH2' 'LTH3' 'LLFC' 'LMFC' 'LTB1' ...
%     'LTB2' 'LTB3' 'LLMAL' 'LMMAL' 'LCAL' 'LTOE' 'LMT5' ...
%     'LHJC' 'RHJC' 'LKJC' 'RKJC' 'LAJC' 'RAJC'};


% [nt, nc] = size(new_mkr_data);
if rem(nc,3)
    error('Input trajectories must have 3 components each.');
end

% Also check to make sure the CS is consistent with OpenSIM, which is as follows:
% X-axis positive anterior
% Y-axis positive superior
% Z-axis positive toward the right, as the subject is facing

% note that the coordinate data should have already been rotated from the
% original global coordinate system so that the y-axis is vertical. THis is
% performed in the export.trc functions

if abs((RASI(1,1)-LASI(1,1))) > 150 % then subject is facing along the x-direction of GLobal CS
    if RASI(1,1) > LASI(1,1) % then they are facing -z direction
        rotationmatrix = [0 0 -1; 0 1 0; 1 0 0]; %rotate 90 deg around the vertical y-axis
    else % they are facing the z direction
        rotationmatrix = [0 0 1; 0 1 0; -1 0 0]; % rotate -90 deg around the y-axis
    end
else % subject must be facing with feet along the z-axis
    if RASI(1,3) > LASI(1,3) % then they are facing x direction
        rotationmatrix = [1 0 0; 0 1 0; 0 0 1];
    else
        rotationmatrix = [-1 0 0; 0 1 0; 0 0 -1];
    end
end

rotated_mkr_data = mkr_data;
for I = 1:nc/3-1
    rotated_mkr_data(:,3*I:3*I+2) = [rotationmatrix*mkr_data(:,3*I:3*I+2)']';
end

% Print data to a new static joint centre file
% ------------------------------------------------------------
% nMarkers=size(mkr_labels,2);
% sampleRate = round(1/(mkr_data(2,2)-mkr_data(1,2)));
% Time_array=mkr_data(:,2);

new_file_name = [file_name,'_rot.trc'];
%dlmwrite(new_file_name,rotated_mkr_data,'\t',6,0);
fid = fopen(new_file_name,'w');

% Print header information
% fprintf(fid,'PathFileType\t4\t(X/Y/Z)\t%s\n',subject_name);
% fprintf(fid,'DataRate\tCameraRate\tNumFrames\tNumMarkers\tUnits\tOrigDataRate\tOrigDataStartFrame\tOrigNumFrames\n');
% fprintf(fid,'%d\t%d\t%d\t%d\tmm\t%d\t%d\t%d\n',sampleRate,sampleRate,nt,nMarkers,sampleRate,mkr_data(1,1),mkr_data(end,1))
% fprintf(fid,'Frame#\tTime');
% for i = 1:length(mkr_labels)
%     fprintf(fid,'\t%s\t\t',mkr_labels{i});
% end
% fprintf(fid,'\n\t\t');
% for i = 1:length(mkr_labels)
%     fprintf(fid,'X%d\tY%d\tZ%d\t',i,i,i);
% end
% fprintf(fid,'\n\n');


fprintf(fid,line1);
fprintf(fid,'\n');
fprintf(fid,line2);
fprintf(fid,'\n');
fprintf(fid,line3);
fprintf(fid,'\n');
fprintf(fid,line4);
fprintf(fid,'\n');
fprintf(fid,line5);
fprintf(fid,'\n');
fprintf(fid,line6);
fprintf(fid,'\n');



% Print data
for i= 1:nt
    fprintf(fid,'%d\t',i);
    for j=2:nc-1
        fprintf(fid,'%f\t',rotated_mkr_data(i,j));
    end
    fprintf(fid,'\n');
end
fclose(fid);

% Plot marker data to check that the joint centres look ok....
% -------------------------------------------------------------------------

% figure(1)
% hold on
% axis equal
% title('3D Positions of markers. Check that black markers resemble joint centres. Hit Enter to finish')
% scatter3(RASI(1,1),RASI(1,2),RASI(1,3),'go');
% scatter3(LASI(1,1),LASI(1,2),LASI(1,3),'go');
% scatter3(SACR(1,1),SACR(1,2),SACR(1,3),'go');
% scatter3(LHJC(1,1),LHJC(1,2),LHJC(1,3),'ko');
% scatter3(RHJC(1,1),RHJC(1,2),RHJC(1,3),'ko');
% 
% scatter3(LTH1(1,1),LTH1(1,2),LTH1(1,3),'ro');
% scatter3(LTH2(1,1),LTH2(1,2),LTH2(1,3),'ro');
% scatter3(LTH3(1,1),LTH3(1,2),LTH3(1,3),'ro');
% scatter3(LMFC(1,1),LMFC(1,2),LMFC(1,3),'ro');
% scatter3(LLFC(1,1),LLFC(1,2),LLFC(1,3),'ro');
%  scatter3(LHA1(1,1),LHA1(1,2),LHA1(1,3),'ko');
%  scatter3(LHA2(1,1),LHA2(1,2),LHA2(1,3),'ko');
% scatter3(RTH1(1,1),RTH1(1,2),RTH1(1,3),'bo');
% scatter3(RTH2(1,1),RTH2(1,2),RTH2(1,3),'bo');
% scatter3(RTH3(1,1),RTH3(1,2),RTH3(1,3),'bo');
% scatter3(RLFC(1,1),RLFC(1,2),RLFC(1,3),'bo');
% scatter3(RMFC(1,1),RMFC(1,2),RMFC(1,3),'bo');
%  scatter3(RHA1(1,1),RHA1(1,2),RHA1(1,3),'ko');
%  scatter3(RHA2(1,1),RHA2(1,2),RHA2(1,3),'ko');
% 
% scatter3(Lknee_centre(1,1),Lknee_centre(1,2),Lknee_centre(1,3),'go');
% scatter3(LKJC(1,1),LKJC(1,2),LKJC(1,3),'ko');
% scatter3(Rknee_centre(1,1),Rknee_centre(1,2),Rknee_centre(1,3),'go');
% scatter3(RKJC(1,1),RKJC(1,2),RKJC(1,3),'ko');
% 
% scatter3(LTB1(1,1),LTB1(1,2),LTB1(1,3),'ro');
% scatter3(LTB2(1,1),LTB2(1,2),LTB2(1,3),'ro');
% scatter3(LTB3(1,1),LTB3(1,2),LTB3(1,3),'ro');
% 
% scatter3(RTB1(1,1),RTB1(1,2),RTB1(1,3),'bo');
% scatter3(RTB2(1,1),RTB2(1,2),RTB2(1,3),'bo');
% scatter3(RTB3(1,1),RTB3(1,2),RTB3(1,3),'bo');
% 
% scatter3(LMMAL(1,1),LMMAL(1,2),LMMAL(1,3),'ro');
% scatter3(LLMAL(1,1),LLMAL(1,2),LLMAL(1,3),'ro');
% scatter3(LAJC(1,1),LAJC(1,2),LAJC(1,3),'ko');
% 
% scatter3(RMMAL(1,1),RMMAL(1,2),RMMAL(1,3),'bo');
% scatter3(RLMAL(1,1),RLMAL(1,2),RLMAL(1,3),'bo');
% scatter3(RAJC(1,1),RAJC(1,2),RAJC(1,3),'ko');
% 
% pause
% close all
