% THIS CODE MUST BE IN THE SAME FOLDER AS THE SENSOR DATA FORCE
% THE FOLDER WITH THE SENSOR DATA FORCE MUST BE OPEN AS "CURRENT FOLDER"
% PLAYER INFO AND POSTITIONING WILL BE ENTERED VIA PROMPT
% WILL DELIVER HAMSTRING FORCE, JOINT FORCE, AND POSTITON WITH TIME
clear
clc
format short
%% Gravity
g=9.81;
%% Inputs

filename=input('Type filename, to inlcude file type (.xls, .xlsx ect)   ','s');

%Loads excel file into matlab
filename = convertCharsToStrings(filename);
Sensor=readtable(filename,'Range','B2:C30000');
Sensor=table2array(Sensor);

%detele excess data files
Sensor=rmmissing(Sensor);

%normalize start time to zero seconds.
t0=Sensor(1,1);
Sensor(:,1)=Sensor(:,1)-t0;

%Converts inputed data into seperate variables (time and force)
N=Sensor(:,2)';
t=Sensor(:,1)' ;



%% Exercise Conditions and Player Info


Pinsetting=input('What pin is the knee rest set at (1-10)   '); %angle of the tibia with respect to parallel
Phi_Hip =input('What hip angle will play hold for exercise (0 = fully extended)   '); %angle of the hip with respect to the femur (0 =extended)

Pin_Degree=[0 5 10 15 20 25 30 35 40 45];
Phi_Ankle=Pin_Degree(Pinsetting);

Lt=input('Input Player Lower Leg Length in meters   '); %tibia length in m
Lf=input('Input Player Thigh Length in meters   '); %femur length in m
Lc=input('Input Player Hip to Chest Length in meters   '); %length from hip to chest COM in m

m=input('Input Player weight in kg   ');
w=input('Input weight, if any, added to player in kg    ');
%% Physiological Adjustments/factors

m=(.95*m)+w; %on average, 5% of weight is from lower limb - remove from moving mass
r=0.025;  %average moment arm of hamstring in adult males in m

%% Geometric conversions

alpha=atand(r/Lf); %angle between Force vector and femur/tibia

%account for changes in hip angle for center of mass lever arm
Lcom=sqrt(Lf^2+Lc^2-2*Lf*Lc*cosd(180-Phi_Hip)); %length from knee to center of mass using law of cosines

Beta_1=asind(sind(180-Phi_Hip)*(Lc/Lcom)); %angle bewteen femur and center of mass vector (Lcom)

%% Moment conversion to geometric position
%taking the known external forces (N and mg) to find knee position)

N_moment= N.*(Lt.*cosd(Phi_Ankle)); % the effective moment in N-m of the ankle force on the knee

Beta_2=acosd((N_moment./(m*g*Lcom))); %angle between parallel and the center of mass vector (Lcom)

Phi_Knee = Beta_1 + Beta_2; % angle between parallel and femur

Theta=Phi_Knee - Phi_Ankle; % absolute knee angle, with respect to tibia position

%% Internal Force Determination

F = (m*g*cosd(Phi_Knee)/(sind(alpha)));
J = (m*g*sind(Phi_Knee)) + F*cosd(alpha);

RESULTS=[Theta;F;J;t];

%% Plots
%to be edited by UI team
subplot(2,1,1)
hold on
plot(t,F)

ylabel('Hamstring Force')
subplot(2,1,2)
plot(t, J)
ylabel('Internal Joint Force')