%reorder_force.m

% read in mot file

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

fclose(fid);

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

odered_force_data = [force_data(:,2:4), foce_data(:,8:10), force_data(:,5:7)];
newline6 ='time\tground_force_vx\tground_force_vy\tground_force_vz\tground_force_px\tground_force_py

% labels = cell(1, nc);
% for j = 1:nc
% 	[labels{j}, line6] = strtok(line6);
% end
% lables_new = cell(1,nc);
% 
% if rem(nc-1,3)
%     error('Input trajectories must have 3 components each.');
% end
% for j = 1:nc
%     if strncmpi(labels(j), 'ground_torque', length('ground_torque')) == 1

% 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_force_data = force_data;
for I = 1:nc/3
    rotated_force_data(:,3*-1:3*I+1) = [rotationmatrix*mkr_data(:,3*I-1:3*I+1)']';
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.mot'];
%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