%append_joint_centres_to_static_trial.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
mkr_data = dlmread(static_trc_fname,'\t',6,0);
[nframes ncolumns] = 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);



% preallocate arrays for hip, knee, and ankle centres
LHJC=zeros(nframes,3);
RHJC=zeros(nframes,3);
LKJC=zeros(nframes,3);
RKJC=zeros(nframes,3);
LHA1=zeros(nframes,3);
LHA2=zeros(nframes,3);
new_LHA1=zeros(nframes,3);
new_LHA2=zeros(nframes,3);
RHA1=zeros(nframes,3);
RHA2=zeros(nframes,3);
new_RHA1=zeros(nframes,3);
new_RHA2=zeros(nframes,3);

LHA1_ankle=zeros(nframes,3);
LHA2_ankle=zeros(nframes,3);
RHA1_ankle=zeros(nframes,3);
RHA2_ankle=zeros(nframes,3);

% First, define a Pelvis Coordinate System
% ----------------------------------------------------------
% define the pelvis origin
originPelvis=(LASI+RASI)/2;

%calculate unit vectors of Pelvis segment relative to global
[e1Pelvis,e2Pelvis,e3Pelvis]=segmentorientationV1V3(originPelvis-SACR,RASI-LASI);

% now project HJC back into Global CS
for i=1:nframes
    rotationmatrix=[e1Pelvis(i,:);e2Pelvis(i,:);e3Pelvis(i,:)];
    rotationmatrix=inv(rotationmatrix);
    LHJC(i,:)= rotationmatrix*LHJC_location + originPelvis(i,:)';
    RHJC(i,:)= rotationmatrix*RHJC_location + originPelvis(i,:)';
end

% Second, define a Thigh Coordinate System - LEFT LEG
% ----------------------------------------------------------
originThigh=LTH2;

%calculate unit vectors of Thigh segment relative to global
[e1Thigh,e2Thigh,e3Thigh]=segmentorientationV2V1(originThigh-LTH1,LTH3-originThigh);

% now project LKJC back into Global CS
for i=1:nframes
    rotationmatrix=[e1Thigh(i,:);e2Thigh(i,:);e3Thigh(i,:)];
    rotationmatrix=inv(rotationmatrix);
    LHA1(i,:)= rotationmatrix*LHAxes(:,1) + originThigh(i,:)';
    LHA2(i,:)= rotationmatrix*LHAxes(:,2) + originThigh(i,:)';
end

% Define a Thigh Coordinate System - RIGHT LEG
% define the thigh origin and yaxis, which lies parallel to the line bisecting TH1 and TH3
originThigh=RTH2;

%calculate unit vectors of Thigh segment relative to global
[e1Thigh,e2Thigh,e3Thigh]=segmentorientationV2V1(originThigh-RTH1,RTH3-originThigh);

% now project LKJC back into Global CS
for i=1:nframes
    rotationmatrix=[e1Thigh(i,:);e2Thigh(i,:);e3Thigh(i,:)];
    rotationmatrix=inv(rotationmatrix);
    RHA1(i,:)= rotationmatrix*RHAxes(:,1) + originThigh(i,:)';
    RHA2(i,:)= rotationmatrix*RHAxes(:,2) + originThigh(i,:)';
end

% Define epicondylar axis of the knee
Lepicondylar_axis = (LLFC-LMFC);
Repicondylar_axis = (RLFC-RMFC);

% Define starting knee centre position
Lknee_centre = (LLFC+LMFC)/2;
Rknee_centre = (RLFC+RMFC)/2;

% Define helical axes vectors
Lhelical_axis_vector = LHA1-LHA2;
Rhelical_axis_vector = RHA1-RHA2;

% Define a new knee joint centre, using the helical axes
% -------------------------------------------------------------------------
% given that we have two axes defined, and the centre of the knee joint, we can define a plane perpendicular
% to the Epicondyle axis that intersects the centre of the knee joint. The point that this plane intersects onto
% the helical axis is defined as the new joint centre, and the helical axis can be redefined in body builder using
% two new virtual markers.

% The line of the optimal helical axis defined as:
%             x = xo + at
%             y = yo + bt               Eq(1)
%             z = zo + ct
% where xo,yo,zo describe a point along the line (such as Sopt) and <a,b,c> describe the unit vector of the line (helical_axis_vector)
%
%The plane perpendicular to the epicondylar axis and running through the centre of the knee is defined by:
%           a(x-xo)+b(y-yo)+c(z-zo)=0           Eq(2)
% where xo,yo,zo define a point on the plane (knee_centre) and <a,b,c> define the unit vector of the perpendicular plane (epicondylar_axis)
%
% need to join Eq (1) and (2) to solve for t, then substitute this value back into eq (1)

for i=1:nframes
    t = (dot(Lepicondylar_axis(i,:),Lknee_centre(i,:))-dot(Lepicondylar_axis(i,:),LHA1(i,:)))/dot(Lepicondylar_axis(i,:),Lhelical_axis_vector(i,:));
    new_x = LHA1(i,1)+Lhelical_axis_vector(i,1)*t;
    new_y = LHA1(i,2)+Lhelical_axis_vector(i,2)*t;
    new_z = LHA1(i,3)+Lhelical_axis_vector(i,3)*t;
    LKJC(i,:)=[new_x;new_y;new_z];

    % Reposition the Lateral and Medial Femoral Condyles, so that they lie on
    % the helical axis, knee width apart
    t = (dot(Lepicondylar_axis(i,:),LLFC(i,:))-dot(Lepicondylar_axis(i,:),LHA1(i,:)))/dot(Lepicondylar_axis(i,:),Lhelical_axis_vector(i,:));
    new_x = LHA1(i,1)+Lhelical_axis_vector(i,1)*t;
    new_y = LHA1(i,2)+Lhelical_axis_vector(i,2)*t;
    new_z = LHA1(i,3)+Lhelical_axis_vector(i,3)*t;
    LLFC(i,:)=[new_x;new_y;new_z];

    t = (dot(Lepicondylar_axis(i,:),LMFC(i,:))-dot(Lepicondylar_axis(i,:),LHA1(i,:)))/dot(Lepicondylar_axis(i,:),Lhelical_axis_vector(i,:));
    new_x = LHA1(i,1)+Lhelical_axis_vector(i,1)*t;
    new_y = LHA1(i,2)+Lhelical_axis_vector(i,2)*t;
    new_z = LHA1(i,3)+Lhelical_axis_vector(i,3)*t;
    LMFC(i,:)=[new_x;new_y;new_z];
end

for i=1:nframes
    t = (dot(Repicondylar_axis(i,:),Rknee_centre(i,:))-dot(Repicondylar_axis(i,:),RHA1(i,:)))/dot(Repicondylar_axis(i,:),Rhelical_axis_vector(i,:));
    new_x = RHA1(i,1)+Rhelical_axis_vector(i,1)*t;
    new_y = RHA1(i,2)+Rhelical_axis_vector(i,2)*t;
    new_z = RHA1(i,3)+Rhelical_axis_vector(i,3)*t;
    RKJC(i,:)=[new_x;new_y;new_z];

      t = (dot(Repicondylar_axis(i,:),RLFC(i,:))-dot(Repicondylar_axis(i,:),RHA1(i,:)))/dot(Repicondylar_axis(i,:),Rhelical_axis_vector(i,:));
    new_x = RHA1(i,1)+Rhelical_axis_vector(i,1)*t;
    new_y = RHA1(i,2)+Rhelical_axis_vector(i,2)*t;
    new_z = RHA1(i,3)+Rhelical_axis_vector(i,3)*t;
    RLFC(i,:)=[new_x;new_y;new_z];

     t = (dot(Repicondylar_axis(i,:),RMFC(i,:))-dot(Repicondylar_axis(i,:),RHA1(i,:)))/dot(Repicondylar_axis(i,:),Rhelical_axis_vector(i,:));
    new_x = RHA1(i,1)+Rhelical_axis_vector(i,1)*t;
    new_y = RHA1(i,2)+Rhelical_axis_vector(i,2)*t;
    new_z = RHA1(i,3)+Rhelical_axis_vector(i,3)*t;
    RMFC(i,:)=[new_x;new_y;new_z];
end



% Now define a Tibia Coordinate System - LEFT LEG
% ----------------------------------------------------------
originTibia=LTB2;

%calculate unit vectors of Tibia segment relative to global
[e1Tibia,e2Tibia,e3Tibia]=segmentorientationV2V1(originTibia-LTB1,LTB3-originTibia);

% now project LKJC back into Global CS
for i=1:nframes
    rotationmatrix=[e1Tibia(i,:);e2Tibia(i,:);e3Tibia(i,:)];
    rotationmatrix=inv(rotationmatrix);
    LHA1_ankle(i,:)= rotationmatrix*LAJaxes(:,1) + originTibia(i,:)';
    LHA2_ankle(i,:)= rotationmatrix*LAJaxes(:,2) + originTibia(i,:)';
end


% Define a Tibia Coordinate System - RIGHT LEG
% ----------------------------------------------------------
originTibia=RTB2;

%calculate unit vectors of Tibia segment relative to global
[e1Tibia,e2Tibia,e3Tibia]=segmentorientationV2V1(originTibia-RTB1,RTB3-originTibia);

% now project LKJC back into Global CS
for i=1:nframes
    rotationmatrix=[e1Tibia(i,:);e2Tibia(i,:);e3Tibia(i,:)];
    rotationmatrix=inv(rotationmatrix);
    RHA1_ankle(i,:)= rotationmatrix*RAJaxes(:,1) + originTibia(i,:)';
    RHA2_ankle(i,:)= rotationmatrix*RAJaxes(:,2) + originTibia(i,:)';
end


% Define malleoli axis of the ankle
Lmalleoli_axis = (LLMAL-LMMAL);
Rmalleoli_axis = (RLMAL-RMMAL);

% Define starting knee centre position
Lankle_centre = (LMMAL+LLMAL)/2;
Rankle_centre = (RMMAL+RLMAL)/2;

% Define helical axes vectors
Lankle_helical_axis_vector = LHA1_ankle-LHA2_ankle;
Rankle_helical_axis_vector = RHA1_ankle-RHA2_ankle;



for i=1:nframes
    t = (dot(Lmalleoli_axis(i,:),Lankle_centre(i,:))-dot(Lmalleoli_axis(i,:),LHA1(i,:)))/dot(Lmalleoli_axis(i,:),Lankle_helical_axis_vector(i,:));
    new_x = LHA1(i,1)+Lhelical_axis_vector(i,1)*t;
    new_y = LHA1(i,2)+Lhelical_axis_vector(i,2)*t;
    new_z = LHA1(i,3)+Lhelical_axis_vector(i,3)*t;
    LKJC(i,:)=[new_x;new_y;new_z];

    % Reposition the Lateral and Medial Malleoli, so that they lie on
    % the helical axis, knee width apart
    t = (dot(Lmalleoli_axis(i,:),LLMAL(i,:))-dot(Lmalleoli_axis(i,:),LHA1(i,:)))/dot(Lmalleoli_axis(i,:),Lankle_helical_axis_vector(i,:));
    new_x = LHA1(i,1)+Lhelical_axis_vector(i,1)*t;
    new_y = LHA1(i,2)+Lhelical_axis_vector(i,2)*t;
    new_z = LHA1(i,3)+Lhelical_axis_vector(i,3)*t;
    LLFC(i,:)=[new_x;new_y;new_z];

    t = (dot(Lmalleoli_axis(i,:),LMMAL(i,:))-dot(Lmalleoli_axis(i,:),LHA1(i,:)))/dot(Lmalleoli_axis(i,:),Lankle_helical_axis_vector(i,:));
    new_x = LHA1(i,1)+Lhelical_axis_vector(i,1)*t;
    new_y = LHA1(i,2)+Lhelical_axis_vector(i,2)*t;
    new_z = LHA1(i,3)+Lhelical_axis_vector(i,3)*t;
    LMFC(i,:)=[new_x;new_y;new_z];
end



% 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 LHJC RHJC LKJC RKJC LAJC RAJC];

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

for I = 1:nc/3
    rotated_mkr_data(:,3*I-2:3*I) = [rotationmatrix*new_mkr_data(:,3*I-2:3*I)']';
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 = [dat_path '\' subject_name '_OptimalJointCentres.trc'];
fid = fopen(new_file,'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');
% Print data
for i= 1:nt
    fprintf(fid,'%d\t%f\t',i,Time_array(i));
    for j=1:nc
        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