%% Project 485 medial knee compartment contact force figure generation for presentation

close all;
clear all;

%subject specific information
H=1.57; % m 
BW=52.5*9.81; %kg*N

%load in data from joint reaction analysis in open sim for 6 steps
[B1data,B1headers]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','Baseline1_JR.sto');
[B2data,B2headers]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','Baseline2_JR.sto');
[B3data,B3headers]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','Baseline3_JR.sto');
[E1data,E1headers]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','eval1_JR.sto');
[E2data,E2headers]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','eval2_JR.sto');
[E3data,E3headers]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','eval3_JR.sto');

%build time array
b1t=B1data(:, 1);
%make length the same
b2t=B2data(:, 1);
b2t=[b2t; 0; 0];
b3t=B3data(:, 1);
e1t=E1data(:, 1);
e2t=E2data(:, 1);
e3t=E3data(:, 1);

%build medial contact force arrays (medcond on sagital Fy) 
b1mf=B1data(:, 39);
b2mf=B2data(:, 39);
b2mf=[b2mf; 0; 0];
b3mf=B3data(:, 39);
e1mf=E1data(:, 39);
e2mf=E2data(:, 39);
e3mf=E3data(:, 39);

% build averages
baselinemedf=[b1mf b2mf b3mf];
evalmedf=[e1mf e2mf e3mf];

for i=1:86
    b_avg_medf(i)=mean(baselinemedf(i,:));
end 
for j=1:84
    e_avg_medf(j)=mean(evalmedf(j,:));
end

%%plot medial contact force averages only
figure; 
plot(b1t,b_avg_medf/BW, 'b', 'LineWidth',5); 
hold on; 
plot(e1t,e_avg_medf/BW, 'r', 'LineWidth',5);
xlabel('time (s)', 'FontSize',18);
xt = get(gca, 'XTick');
xlim([0 0.83]);
set(gca, 'FontSize', 16);
box off;
ylabel('Medial Compartment Contact Force (BW)', 'FontSize',18);
title('Medial Knee Contact Force', 'FontSize',18);
%legend({'average baseline (n=3)','average toe-in (n=3)'}, 'FontSize',18);
hold off; 

%%
%%plot medial contact force averages and trials
figure; 
plot(b1t,b_avg_medf/BW, 'b', 'LineWidth',5); 
hold on; 
plot(e1t,e_avg_medf/BW, 'r', 'LineWidth',5);
hold on; 
xlabel('time (s)', 'FontSize',18);
xt = get(gca, 'XTick');
xlim([0 0.83]);
set(gca, 'FontSize', 16);
box off;
ylabel('Medial Compartment Contact Force (BW)', 'FontSize',18);
title('Medial Knee Contact Force', 'FontSize',18);
hold on; plot(b1t,b1mf/BW, 'b', 'LineWidth',0.5);
hold on; plot(b2t(1:82),b1mf(1:82)/BW, 'b', 'LineWidth',0.5);
hold on; plot(b3t,b3mf/BW, 'b', 'LineWidth',0.5);
hold on; plot(e1t,e1mf/BW, 'r', 'LineWidth',0.5);
hold on; plot(e2t,e2mf/BW, 'r', 'LineWidth',0.5);
hold on; plot(e3t,e3mf/BW, 'r', 'LineWidth',0.5);
%legend('average baseline (n=3)','average toe-in evals (n=3)');
hold off; 

%%
%build lateral contact force arrays (latcond on sagital Fy)
b1lf=B1data(:, 48);
b2lf=B2data(:, 48);
b2lf=[b2lf;0;0];
b3lf=B3data(:, 48);
e1lf=E1data(:, 48);
e2lf=E2data(:, 48);
e3lf=E3data(:, 48);

%build averages
baselinelatf=[b1lf b2lf b3lf];
evallatf=[e1lf e2lf e3lf];

for i=1:86
    b_avg_latf(i)=mean(baselinelatf(i,:));
end 
for j=1:84
    e_avg_latf(j)=mean(evallatf(j,:));
end

%plot lateral knee force 
figure;
plot(b1t,b_avg_latf/BW, 'b', 'LineWidth',5); 
hold on; 
plot(e1t, e_avg_latf/BW, 'r','LineWidth',5);
xlabel('time (s)', 'FontSize',18);
xt = get(gca, 'XTick');
xlim([0 0.83]);
set(gca, 'FontSize', 16);
box off;
ylabel('lateral compartment contact force (BW)', 'FontSize',18);
%legend('average baseline (n=3)','average toe-in evals (n=3)');
title('Lateral Knee Contact Force', 'FontSize',18);

%%
%medial to lateral force ratio
bf_ratio=b_avg_medf./b_avg_latf;
ef_ratio=e_avg_medf./e_avg_latf;

figure;
plot(b1t(1:82), bf_ratio(1:82), 'b', 'LineWidth',5); 
hold on; 
plot(e1t(1:82), ef_ratio(1:82), 'r', 'LineWidth',5);
xlabel('time (s)', 'FontSize',18);
xt = get(gca, 'XTick');
xlim([0 0.83]);
set(gca, 'FontSize', 16);
box off;
ylabel('ratio of medial to lateral knee force', 'FontSize',18);
%legend('baseline ratio (n=3)','toe-in eval ratio (n=3)');
title('Medial to Lateral Knee Contact Force Ratio', 'FontSize',18);

%%
%total force ratio
bftot_ratio=b_avg_medf+b_avg_latf;
eftot_ratio=e_avg_medf+e_avg_latf;

%plot
figure;
plot(b1t, bftot_ratio/BW, 'b', 'LineWidth',5); 
hold on; 
plot(e1t, eftot_ratio/BW, 'r', 'LineWidth',5);
xlabel('time (s)', 'FontSize',18);
xt = get(gca, 'XTick');
xlim([0 0.83]);
set(gca, 'FontSize', 16);
box off
ylabel('Average Total Knee Force (BW)', 'FontSize',18);
%legend('average baseline (n=3)','average toe-in evals (n=3)');
title('Total Knee Force', 'FontSize',18);

%%
%Knee flexion moment 
%build KFM arrays (WalkerKnee Mx)
b1KFM=B1data(:, 33);
b2KFM=B2data(:, 33);
b2KFM=[b2KFM;0;0];
b3KFM=B3data(:, 33);
e1KFM=E1data(:, 33);
e2KFM=E2data(:, 33);
e3KFM=E3data(:, 33);
baselineKFM=[b1KFM b2KFM b3KFM];
evalKFM=[e1KFM e2KFM e3KFM];

for i=1:86
    b_avg_KFM(i)=mean(baselineKFM(i,:));
end 
for j=1:84
    e_avg_KFM(j)=mean(evalKFM(j,:));
end

%normalize to percent body weight times height
b_avg_KFMnorm=b_avg_KFM/(BW*H)*100;
e_avg_KFMnorm=e_avg_KFM/(BW*H)*100;

figure;
plot(b1t, b_avg_KFMnorm*(-1), 'b', 'LineWidth',5); 
hold on; 
plot(e1t, e_avg_KFMnorm*(-1), 'r', 'LineWidth',5);
hold on; 
xlabel('time (s)', 'FontSize',18);
xt = get(gca, 'XTick');
xlim([0 0.83]);
set(gca, 'FontSize', 16);
box off;
ylabel('Knee Flexion Moment (%BW*H)', 'FontSize',18);
%legend('average baseline (n=3)','average toe-in evals (n=3)');
title('Knee Flexion Moment', 'FontSize',18);
hold off;

%%
%Knee Adduction Moment
%build KAM arrays(WalkerKnee Mz) 
b1KAM=B1data(:, 34);
b2KAM=B2data(:, 34);
b2KAM=[b2KAM;0;0];
b3KAM=B3data(:, 34);
e1KAM=E1data(:, 34);
e2KAM=E2data(:, 34);
e3KAM=E3data(:, 34);

baselineKAM=[b1KAM b2KAM b3KAM];
evalKAM=[e1KAM e2KAM e3KAM];

for i=1:86
    b_avg_KAM(i)=mean(baselineKAM(i,:));
end 

for j=1:84
    e_avg_KAM(j)=mean(evalKAM(j,:));
end

%normalize to percent body weight times height
b_avg_KAMnorm=b_avg_KAM/(BW*H)*100;
e_avg_KAMnorm=e_avg_KAM/(BW*H)*100;

%plot KAM averages
figure;
plot(b1t,b_avg_KAMnorm, 'b', 'LineWidth',5); 
hold on; 
plot(e1t, e_avg_KAMnorm, 'r', 'LineWidth',5);
xlabel('time (s)', 'FontSize',18);
xt = get(gca, 'XTick');
xlim([0 0.83]);
ylim([0 2.5])
set(gca, 'FontSize', 16);
box off;
ylabel('Knee Adduction Moment (%BW*H)', 'FontSize',18);
%legend('average baseline (n=3)','average toe-in evals (n=3)');
title('OpenSim Knee Adduction Moment', 'FontSize',18);

%plot KAM averages and trials
figure; 
plot(b1t,b_avg_KFMnorm, 'b', 'LineWidth',5); 
hold on; 
plot(e1t, e_avg_KFMnorm, 'r', 'LineWidth',5);
hold on; plot(b1t,b1KFM/(BW*H)*100, 'b', 'LineWidth',0.5);
hold on; plot(b2t(1:82),b1KFM(1:82)/(BW*H)*100, 'b', 'LineWidth',0.5);
hold on; plot(b3t,b3KFM/(BW*H)*100, 'b', 'LineWidth',0.5);
hold on; plot(e1t,e1KFM/(BW*H)*100, 'r', 'LineWidth',0.5);
hold on; plot(e2t,e2KFM/(BW*H)*100, 'r', 'LineWidth',0.5);
hold on; plot(e3t,e3KFM/(BW*H)*100, 'r', 'LineWidth',0.5);
%legend('average baseline (n=3)','average toe-in evals (n=3)');
xlabel('time (s)', 'FontSize',18);
xt = get(gca, 'XTick');
xlim([0 0.83]);
set(gca, 'FontSize', 16);
box off;
ylabel('Knee Adduction Moment (%BW*H)', 'FontSize',18);
title('Knee Adduction Moment', 'FontSize',18);
hold off; 

%%
%Quad force 

%load SO muscle forces files
[B1Mdata,B1Mheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','B1_musforce.sto');
[B2Mdata,B2Mheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','B2_musforce.sto');
[B3Mdata,B3Mheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','B3_musforce.sto');
[E1Mdata,E1Mheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','E1_musforce.sto');
[E2Mdata,E2Mheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','E2_musforce.sto');
[E3Mdata,E3Mheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','E3_musforce.sto');

%vastiint_r column 39 in data files
%vastilateral_r column 40 in data files 
%vastimedial_r  column 41 in data files

b1vastint=B1Mdata(:, 39);
b2vastint=B2Mdata(:, 39);
b2vastint=[b2vastint;0;0];
b3vastint=B3Mdata(:, 39);
e1vastint=E1Mdata(:, 39);
e2vastint=E2Mdata(:, 39);
e3vastint=E3Mdata(:, 39);

b1vastlat=B1Mdata(:, 40);
b2vastlat=B2Mdata(:, 40);
b2vastlat=[b2vastlat;0;0];
b3vastlat=B3Mdata(:, 40);
e1vastlat=E1Mdata(:, 40);
e2vastlat=E2Mdata(:, 40);
e3vastlat=E3Mdata(:, 40);

b1vastmed=B1Mdata(:, 41);
b2vastmed=B2Mdata(:, 41);
b2vastmed=[b2vastmed;0;0];
b3vastmed=B3Mdata(:, 41);
e1vastmed=E1Mdata(:, 41);
e2vastmed=E2Mdata(:, 41);
e3vastmed=E3Mdata(:, 41);

b1recf=B1Mdata(:, 31);
b2recf=B2Mdata(:, 31);
b2recf=[b2recf;0;0];
b3recf=B3Mdata(:, 31);
e1recf=E1Mdata(:, 31);
e2recf=E2Mdata(:, 31);
e3recf=E3Mdata(:, 31);

%build quad muscle averages

b1qm=[b1vastmed b1vastlat b1vastint b1recf];
b2qm=[b2vastmed b2vastlat b2vastint b2recf];
b3qm=[b3vastmed b3vastlat b3vastint b3recf];
e1qm=[e1vastmed e1vastlat e1vastint e1recf];
e2qm=[e2vastmed e2vastlat e2vastint e2recf];
e3qm=[e3vastmed e3vastlat e3vastint e3recf];

for i=1:86
    b1qm_summed(i)=sum(b1qm(i,:));
    b2qm_summed(i)=sum(b2qm(i,:));
    b3qm_summed(i)=sum(b3qm(i,:));
end 

for j=1:84
    e1qm_summed(j)=sum(e1qm(j,:));
    e2qm_summed(j)=sum(e2qm(j,:));
    e3qm_summed(j)=sum(e3qm(j,:));
end

baselinequads =[b1qm_summed' b2qm_summed' b3qm_summed'];
evalquads = [e1qm_summed' e2qm_summed' e3qm_summed'];

for i=1:86
    b_avg_quads(i)=mean(baselinequads(i,:));
end 
for j=1:84
    e_avg_quads(j)=mean(evalquads(j,:));
end

%plot quad force
figure;
plot(b_avg_quads/BW, 'b', 'LineWidth',5); 
hold on; 
plot(e_avg_quads/BW, 'r', 'LineWidth',5);
xlabel('% stance phase', 'FontSize',18);
xt = get(gca, 'XTick');
set(gca, 'FontSize', 16);
ylabel('Quad Force (BW)', 'FontSize',18);
ylim([0 2]);
box off;
%legend('average baseline (n=3)','average toe-in evals (n=3)');
title('Quadriceps Force', 'FontSize',18);

%%
%Hamstring forces 

%8 bifem long in data 
%9 bifem short in data 

b1bifeml=B1Mdata(:, 8);
b2bifeml=B2Mdata(:, 8);
b2bifeml=[b2bifeml;0;0];
b3bifeml=B3Mdata(:, 8);
e1bifeml=E1Mdata(:, 8);
e2bifeml=E2Mdata(:, 8);
e3bifeml=E3Mdata(:, 8);

b1bifems=B1Mdata(:, 9);
b2bifems=B2Mdata(:, 9);
b2bifems=[b2bifems;0;0];
b3bifems=B3Mdata(:, 9);
e1bifems=E1Mdata(:, 9);
e2bifems=E2Mdata(:, 9);
e3bifems=E3Mdata(:, 9);

%build hamstring average forces

b1hm=[b1bifeml b1bifems];
b2hm=[b2bifeml b2bifems];
b3hm=[b3bifeml b3bifems];
e1hm=[e1bifeml e1bifems];
e2hm=[e2bifeml e2bifems];
e3hm=[e3bifeml e3bifems];

for i=1:86
    b1hm_summed(i)=sum(b1hm(i,:));
    b2hm_summed(i)=sum(b2hm(i,:));
    b3hm_summed(i)=sum(b3hm(i,:));
end 

for j=1:84
    e1hm_summed(j)=sum(e1hm(j,:));
    e2hm_summed(j)=sum(e2hm(j,:));
    e3hm_summed(j)=sum(e3hm(j,:));
end

baselinehams =[b1hm_summed' b2hm_summed' b3hm_summed'];
evalhams = [e1hm_summed' e2hm_summed' e3hm_summed'];

for i=1:86
    b_avg_hams(i)=mean(baselinehams(i,:));
end 
for j=1:84
    e_avg_hams(j)=mean(evalhams(j,:));
end

%plot hamstring force
figure;
plot(b_avg_hams/BW, 'b', 'LineWidth',5); 
hold on; 
plot(e_avg_hams/BW, 'r', 'LineWidth',5);
xlabel('% stance phase', 'FontSize',18);
xt = get(gca, 'XTick');
set(gca, 'FontSize', 16);
box off;
ylabel('Hamstring Force (BW)', 'FontSize',18);
%legend('average baseline (n=3)','average toe-in evals (n=3)');
title('Hamstring Force', 'FontSize',18);

%%
%Gastroc Force

%gastroc lat 14
%gastroc med 15

b1gasl=B1Mdata(:, 14);
b2gasl=B2Mdata(:, 14);
b2gasl=[b2gasl;0;0];
b3gasl=B3Mdata(:, 14);
e1gasl=E1Mdata(:, 14);
e2gasl=E2Mdata(:, 14);
e3gasl=E3Mdata(:, 14);

b1gasm=B1Mdata(:, 15);
b2gasm=B2Mdata(:, 15);
b2gasm=[b2gasm;0;0];
b3gasm=B3Mdata(:, 15);
e1gasm=E1Mdata(:, 15);
e2gasm=E2Mdata(:, 15);
e3gasm=E3Mdata(:, 15);

%build gastroc force

b1gm=[b1gasl b1gasm];
b2gm=[b2gasl b2gasm];
b3gm=[b3gasl b3gasm];
e1gm=[e1gasl e1gasm];
e2gm=[e2gasl e2gasm];
e3gm=[e3gasl e3gasm];

for i=1:86
    b1gm_summed(i)=sum(b1gm(i,:));
    b2gm_summed(i)=sum(b2gm(i,:));
    b3gm_summed(i)=sum(b3gm(i,:));
end 

for j=1:84
    e1gm_summed(j)=sum(e1gm(j,:));
    e2gm_summed(j)=sum(e2gm(j,:));
    e3gm_summed(j)=sum(e3gm(j,:));
end

baselinegas =[b1gm_summed' b2gm_summed' b3gm_summed'];
evalgas = [e1gm_summed' e2gm_summed' e3gm_summed'];

for i=1:86
    b_avg_gas(i)=mean(baselinegas(i,:));
end 
for j=1:84
    e_avg_gas(j)=mean(evalgas(j,:));
end

%plot gastroc force
figure;
plot(b_avg_gas/BW, 'b', 'LineWidth',5); 
hold on; 
plot(e_avg_gas/BW, 'r', 'LineWidth',5);
xlabel('% stance phase', 'FontSize',18);
xt = get(gca, 'XTick');
set(gca, 'FontSize', 16);
box off;
ylabel('Gatroc Force (BW)', 'FontSize',18);
%legend('average baseline (n=3)','average toe-in evals (n=3)');
title('Gastrocnemius Force', 'FontSize',18);

%%
%Knee Flexion Angle

[B1IKdata,B1IKheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','b1IK.sto');
[B2IKdata,B2IKheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','b2IK.sto');
[B3IKdata,B3IKheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','b3IK.sto');
[E1IKdata,E1IKheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','e1IK.sto');
[E2IKdata,E2IKheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','e2IK.sto');
[E3IKdata,E3IKheaders]=load_sto('/Users/kseagers/Documents/MATLAB/Project485/','e3IK.sto');

%column 11 for knee angle r in the data file

b1IKk=B1IKdata(:, 11);
b2IKk=B2IKdata(:, 11);
b2IKk=[b2IKk;0;0];
b3IKk=B3IKdata(:, 11);
e1IKk=E1IKdata(:, 11);
e2IKk=E2IKdata(:, 11);
e3IKk=E3IKdata(:, 11);

baselineIKk=[b1IKk b2IKk b3IKk];
evalIKk=[e1IKk e2IKk e3IKk];

for i=1:86
    b_avg_IKk(i)=mean(baselineIKk(i,:));
end 

for j=1:84
    e_avg_IKk(j)=mean(evalIKk(j,:));
end

%plot knee angle
figure;
plot(b1t(1:82),b_avg_IKk(1:82), 'b', 'LineWidth',5); 
hold on; 
plot(e1t, e_avg_IKk, 'r', 'LineWidth',5);
xlabel('time (s)', 'FontSize',18);
xt = get(gca, 'XTick');
xlim([0 0.83]);
set(gca, 'FontSize', 16);
box off;
ylabel('Knee Angle (degrees)', 'FontSize',18);
%legend('average baseline (n=3)','average toe-in evals (n=3)');
title('Knee Angle', 'FontSize',18);

%% 
%Hip angle

b1IKh=B1IKdata(:, 8);
b2IKh=B2IKdata(:, 8);
b2IKh=[b2IKh;0;0];
b3IKh=B3IKdata(:, 8);
e1IKh=E1IKdata(:, 8);
e2IKh=E2IKdata(:, 8);
e3IKh=E3IKdata(:, 8);

baselineIKh=[b1IKh b2IKh b3IKh];
evalIKh=[e1IKh e2IKh e3IKh];

for i=1:86
    b_avg_IKh(i)=mean(baselineIKh(i,:));
end 

for j=1:84
    e_avg_IKh(j)=mean(evalIKh(j,:));
end

%plot hip angle 
figure;
plot(b1t,b_avg_IKh, 'b', 'LineWidth',5); 
hold on; 
plot(e1t, e_avg_IKh, 'r', 'LineWidth',5);
xlabel('time (s)', 'FontSize',18);
xt = get(gca, 'XTick');
xlim([0 0.83]);
set(gca, 'FontSize', 16);
box off;
ylabel('Hip Angle (degrees)', 'FontSize',18);
%legend('average baseline (n=3)','average toe-in evals (n=3)');
title('Hip Angle', 'FontSize',18);

%%
% Ankle angle

b1IKa=B1IKdata(:, 15);
b2IKa=B2IKdata(:, 15);
b2IKa=[b2IKa;0;0];
b3IKa=B3IKdata(:, 15);
e1IKa=E1IKdata(:, 15);
e2IKa=E2IKdata(:, 15);
e3IKa=E3IKdata(:, 15);

baselineIKa=[b1IKa b2IKa b3IKa];
evalIKa=[e1IKa e2IKa e3IKa];

for i=1:86
    b_avg_IKa(i)=mean(baselineIKa(i,:));
end 

for j=1:84
    e_avg_IKa(j)=mean(evalIKa(j,:));
end

%plot ankle angle 
figure;
plot(b1t,b_avg_IKa, 'b', 'LineWidth',5); 
hold on; 
plot(e1t, e_avg_IKa, 'r', 'LineWidth',5);
xlabel('time (s)', 'FontSize',18);
xt = get(gca, 'XTick');
xlim([0 0.83]);
set(gca, 'FontSize', 16);
box off;
ylabel('Ankle Angle (degrees)', 'FontSize',18);
%legend('average baseline (n=3)','average toe-in evals (n=3)');
title('Ankle Angle', 'FontSize',18);

%%
%plot experimental data for the subject 
KAMexp=load('DATA_InverseDynamics.mat');
expKAM=KAMexp.DATA.KAM_splines;
expKFM=KAMexp.DATA.KFM_splines;

%plot the experimental KAM
figure;
plot(expKAM(:,1),'b','LineWidth',5); 
hold on;
plot(expKAM(:,2),'r','LineWidth',5);
title('Experimental Knee Adduction Moment', 'FontSize',18); 
xlabel('% stance phase', 'FontSize',18);
xt = get(gca, 'XTick');
set(gca, 'FontSize', 16);
box off;
ylabel('Knee Adduction Moment (%BW*H)', 'FontSize',18);
xlim([12
    92]);
hold off;

%plot the experimental KFM
figure;
plot(expKFM(:,1),'b','LineWidth',5); 
hold on;
plot(expKFM(:,2),'r','LineWidth',5);
title('Experimental Knee Flexion Moment', 'FontSize',18); 
xlabel('% stance phase', 'FontSize',18);
xt = get(gca, 'XTick');
set(gca, 'FontSize', 16);
box off;
ylabel('Knee Flexion Moment (%BW*H)', 'FontSize',18);
xlim([0 100]);
hold off;