function [AJC_axes] = findankleaxes(fname,knee)

% read in trc file
mkr_data = dlmread(fname,'\t',6,2);

if knee == 'L'
    % create xyz 3D column arrays for each marker
    CAL = mkr_data(:,55:57);
    TOE = mkr_data(:,58:60);
    MT5 = mkr_data(:,61:63);
    TB1 = mkr_data(:,46:48);
    TB2 = mkr_data(:,49:51);
    TB3 = mkr_data(:,52:54);
else
    CAL = mkr_data(:,28:30);
    TOE = mkr_data(:,31:33);
    MT5 = mkr_data(:,34:36);
    TB1 = mkr_data(:,19:21);
    TB2 = mkr_data(:,22:24);
    TB3 = mkr_data(:,25:27);
end

% define the tibia origin 
originTibia=TB2;

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

%now transform the marker points into the Tibia coordinate system
for i=1:length(e1Tibia)
    rotationmatrix=[e1Tibia(i,:);e2Tibia(i,:);e3Tibia(i,:)];
    originTibiavector=originTibia(i,:)';
    TB1Tibia(i,:)=(rotationmatrix*(TB1(i,:)')-rotationmatrix*originTibiavector)';
    TH2Tibia(i,:)=(rotationmatrix*(TH2(i,:)')-rotationmatrix*originTibiavector)';
    TB3Tibia(i,:)=(rotationmatrix*(TB3(i,:)')-rotationmatrix*originTibiavector)';
    CALTibia(i,:)=(rotationmatrix*(CAL(i,:)')-rotationmatrix*originTibiavector)';
    TOETibia(i,:)=(rotationmatrix*(TOE(i,:)')-rotationmatrix*originTibiavector)';
    MT5Tibia(i,:)=(rotationmatrix*(MT5(i,:)')-rotationmatrix*originTibiavector)';
end

% decompose arrays to [x y z] 
CALxTibia=CALTibia(:,1);
CALyTibia=CALTibia(:,2);
CALzTibia=CALTibia(:,3);
TOExTibia=TOETibia(:,1);
TOEyTibia=TOETibia(:,2);
TOEzTibia=TOETibia(:,3);
MT5xTibia=MT5Tibia(:,1);
MT5yTibia=MT5Tibia(:,2);
MT5zTibia=MT5Tibia(:,3);
TB1xTibia=TB1Tibia(:,1);
TB1yTibia=TB1Tibia(:,2);
TB1zTibia=TB1Tibia(:,3);
TB2xTibia=TB2Tibia(:,1);
TB2yTibia=TB2Tibia(:,2);
TB2zTibia=TB2Tibia(:,3);
TB3xTibia=TB3Tibia(:,1);
TB3yTibia=TB3Tibia(:,2);
TB3zTibia=TB3Tibia(:,3);

close;
scatter3(CALxTibia,CALyTibia,CALzTibia);
hold;
scatter3(TOExTibia,TOEyTibia,TOEzTibia);
scatter3(MT5xTibia,MT5yTibia,MT5zTibia);
scatter3(TB1xTibia,TB1yTibia,TB1zTibia,'r');
scatter3(TB3xTibia,TB3yTibia,TB3zTibia,'r');
scatter3(TB2xTibia,TB2yTibia,TB2zTibia,'r');
text(TB1xTibia(1),TB1yTibia(1),TB1zTibia(1),'TB1');
text(TB2xTibia(1),TB2yTibia(1),TB2zTibia(1),'TB2');
text(TB3xTibia(1),TB3yTibia(1),TB3zTibia(1),'TB3');
title('Left Knee Joint Instantaneous and Optimal Helical Axes. Enter to continue');
axis equal;
axis manual;

% create input matrix of tibia markers relative to Tibia segment for calculation of helical axis.
% Use soder.m and screw.m from Reinschmidt, Calgary (1996).

vectlength = size(TB1Tibia,1);
count = 0;
I=eye(3,3);
Q=zeros(3,3);
Qsum=zeros(3,3);
Qn=zeros(3,1);
Qnsum=zeros(3,1);
Qssum=zeros(3,1);

threshold = 2;

while count ==0
    threshold = threshold-0.5;
    for i=1:vectlength-4

        input1= TB1Tibia(i:i+3,1:3);
        input2= TB2Tibia(i:i+3,1:3);
        input3= TB3Tibia(i:i+3,1:3);
        input = [input1 input2 input3];

        % create transformation matrix using singular value decomposition
        [T,res]=soder(input);

        % find helical axis using this transformation matrix
        intersect = 1;  %      location of the screw axis where it intersects either the x=0 (intersect=1),
        %    the y=0 (intersect=2), or the z=0 (intersect=3) plane.
        [n,point,phi,t]=screw(T,intersect);

        % only use helical axes that are predicted with phi greater than 5 degrees
        if phi>threshold
            scatter3(point(1),point(2),point(3),'r')
            %  text(point(1),point(2),point(3),'point')
            % create another 2 points along the helical axis approx the width of a knee (100 mm)
            vect=(50/n(1))*n;
            point2=point-vect;
            point3=point+vect;
            axisline=[point point2 point3];
            plot3(axisline(1,:),axisline(2,:),axisline(3,:),'k')
            s=point;
            count=count+1;
            Q=I-n*n';
            Qn=Q*n;
            Qs=Q*s;
            Qsum=Q+Qsum;
            Qnsum=Qn+Qnsum;
            Qssum=Qs+Qssum;
        end
    end
end

Qnmean=Qnsum/count;
Qsmean=Qssum/count;
Qmean=Qsum/count;

Nopt=inv(Qmean)*Qnmean;
Sopt=inv(Qmean)*Qsmean;

half_knee_width = 50;  %distance(LLFC,LMFC)/2;
% epicondylar_axis=LLFC-LMFC;  % v
% knee_centre = (LLFC+LMFC)/2;  %P

helical_axis_vector=(half_knee_width/Nopt(1))*Nopt;
HA1=Sopt-helical_axis_vector;
HA2=Sopt+helical_axis_vector;
axisline=[Sopt HA1 HA2];

HA1x=HA1(1);
HA1y=HA1(2);
HA1z=HA1(3);
HA2x=HA2(1);
HA2y=HA2(2);
HA2z=HA2(3);

plot3(axisline(1,:),axisline(2,:),axisline(3,:),'b','LineWidth',3)
pause;
close;

KJC_axes = [HA1 HA2];


% 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)

% t = (dot(epicondylar_axis,knee_centre)-dot(epicondylar_axis,Sopt))/dot(epicondylar_axis,helical_axis_vector);
%
% new_knee_centre_x = Sopt(1)+helical_axis_vector(1)*t;
% new_knee_centre_y = Sopt(2)+helical_axis_vector(2)*t;
% new_knee_centre_z = Sopt(3)+helical_axis_vector(3)*t;
% new_knee_centre=[new_knee_centre_x;new_knee_centre_y;new_knee_centre_z];

% define new virtual markers on the helical axis (make them roughly the width of the knee).
% LHA1=new_knee_centre+helical_axis_vector;
% LHA2=new_knee_centre-helical_axis_vector;
%
% axisline=[LHA1 LHA2];
% plot3(axisline(1,:),axisline(2,:),axisline(3,:),'b','LineWidth',3)
% scatter3(new_knee_centre(1),new_knee_centre(2),new_knee_centre(3),'bo')
% text(new_knee_centre(1),new_knee_centre(2),new_knee_centre(3),'new knee centre')
% scatter3(LHA1(1),LHA1(2),LHA1(3),'ko')
% text(LHA2(1),LHA2(2),LHA2(3),'LHA 2')
% scatter3(LHA2(1),LHA2(2),LHA2(3),'ko')
% text(LHA1(1),LHA1(2),LHA1(3),'LHA 1')
%
% title('Left Knee Joint Instantaneous and Optimal Helical Axes')
% LeftKneeCentre=new_knee_centre;