function [flexion, abduction, rotation] = orientation_function(p_fixed_prox, p_fixed_mid, p_fixed_dist, p_moving_prox, p_moving_mid, p_moving_dist)

%{
PURPOSE: Use the Grood and Suntay rotation angle calculation method to get
 flexion, abduction, inter/ext. rotation for two given octopus segments

REQUIRED INPUTS: X, Y, and Z coordinate data for five markers (p_fixed_dist
will be the same as p_moving_prox)

x_fixed* --> x axis of fixed frame, perpendicular to the plane of curvature
y_fixed* --> y axis of fixed frame, lies near the direction of greatest
curvature, perp to x_fixed and z_fixed
z_fixed* --> z axis of fixed frame, points last marker to first marker 

* same steps repeated for local coordinate system of moving coordinate
sytem as was for the fixed

OUTPUTS: ************************************
flexion --> flexion angle of moving frame relative to fixed frame. Positive
angle is obtained by two planes traditionally curving in the same direction
abduction --> aduction angle. Positive angle is abducting away from the
non-moving plane to the left- although this is arbatrary and can be changed
for left handed vs. right handed
int_ext --> internal/external rotation (scalar) of moving frame relative to fixed
frame. Positive angle is internal rotation for 'right-handed' systems. As
with abduction, can be changed for left-handed systems

%}

%% NON-MOVING/FIXED LOCAL COORDINATE SYSTEM
z_fixed_m=p_fixed_prox-p_fixed_dist;
z_fixed=z_fixed_m./sqrt(dot(z_fixed_m,z_fixed_m,2));

v1_m=p_fixed_mid-p_fixed_dist;
v2_m=p_fixed_prox-p_fixed_dist; % unit vector is approximately perp. to z, x but not exactly. Therefore, y must be actually obtained after getting x
x_fixed_m=cross(v1_m,v2_m,2);
x_fixed=x_fixed_m./sqrt(dot(x_fixed_m,x_fixed_m,2)); %normalized x vector, AKA e1 in Grood and Suntay method

y_fixed_m=cross(z_fixed,x_fixed,2);
y_fixed=y_fixed_m./sqrt(dot(y_fixed_m,y_fixed_m,2));

%% MOVING LOCAL COORDINATE SYSTEM
z_moving_m=p_moving_prox-p_moving_dist;
z_moving=z_moving_m./sqrt(dot(z_moving_m,z_moving_m,2)); %normalized z vector, AKA e3 in Grood and Suntay method

v3_m=p_moving_mid-p_moving_dist;
v4_m=p_moving_prox-p_moving_dist;
x_moving_m=cross(v3_m,v4_m,2);
x_moving=x_moving_m./sqrt(dot(x_moving_m,x_moving_m,2));

y_moving_m=cross(z_moving,x_moving,2);
y_moving=y_moving_m./sqrt(dot(y_moving_m,y_moving_m,2));
%% FLOATING AXIS
e2_m = cross(z_moving, x_fixed,2); %e2 is floating axis
e2 = e2_m./sqrt(dot(e2_m,e2_m,2));  %make unit vector

%% FLEXION (negative is flexion, positive is extension)
%accounts for any hyper-flexion alpha< 0, flexion is negative, PP wrt MC

flexion_raw=asin(dot(-e2,z_fixed,2));
flexion = flexion_raw*(180/pi);  %convert to degrees, this is flexion angle

%% ABDUCTION (abduction positive, adduction negative)
abduction_raw=acos(dot(-x_fixed, z_moving,2));
abduction=abduction_raw*(180/pi) - 90;   

%% INTERNAL/EXTERNAL ROTATION (positive internal(nail facing away from palm, negative external (nail facing palm))
p_cross=cross(z_moving,e2,2); % to account for internal and external rotation between 180 and -180 deg
p=dot(p_cross,y_moving,2);
p_mag=sqrt(dot(p,p,2));
n=-p./p_mag;

gamma_cross=cross(y_moving,e2,2);
gamma_cross_mag=sqrt(dot(gamma_cross,gamma_cross,2));
gamma_dot=(dot(e2,y_moving,2));
gamma2=(atan2d(n.*gamma_cross_mag,gamma_dot));
rotation=smooth(gamma2);

end   %end function