function plot_kin_grf_act(result_file)
% plots lower right limb kinematics and ground reaction forces along with
% data published in Winter (1991). It also plot a diagram of muscle
% activations at the right leg for a complete gait cycle.
% Input: result file, e.g. result_bk_07_50_3.mat (opt. track result)

% Loading the results in <result_file>
load(result_file);

% Number of nodes
n_nod = Result.Prob.user.n_nod;

% Walking velocity
v_walk = Result.Prob.user.v_walk; % prescribed walking velocity

% Mirroring vector
vm = Result.Prob.user.vm;  % vector of transformation to mirror states at the end to match states at the beginning

% Vector of optimization variables
P = Result.x_k;

% Extract the matrix of states and the matrix of neural excitations from 
% the vector of opt. variables P
for k = 1:n_nod-1
    X(:,k) = P((k-1)*66+1:(k-1)*66+50); % states
    U(:,k) = P((k-1)*66+51:k*66); % neural excitations
end

% Duration of half a gait cycle
T_dur = Result.x_k(end); 

% Vector of time instants for loaded results:
t_x = 0:T_dur/(n_nod-1):T_dur-T_dur/(n_nod-1);

% Concatenate results for whole gait cycle
tout = 0:T_dur/(n_nod-1):2*T_dur;			% 1 x n_nod matrix
xout = [X [X(1,:)+v_walk*T_dur ;X(vm(2:50),:)] [X(1,1)+2*T_dur*v_walk; X(2:50,1)]];	% 50 x n_nod matrix
uout = [U [U(9:16,:); U(1:8,:)] U(:,1)];	% 16 x n_nod matrix


% Use walkode to generate outputs at the n_nod nodes
[xoutdot, cm, grf, mforces, stick, fce] = walkode(tout, xout, uout);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Load mean and standard deviation of kinematics and ground reaction forces
% from Winter (1991)
% Note1: mat-files with Winter data in C:\Repository\MMS\alg\DC
% Note2: the minimal standard deviation is adjuste to be no less than 20%
%        of the maximal standard deviation throughout the whole gait cycle
%        to avoid division by zero in the cost function and to prevent
%        overweighting of regions with very low standard deviation  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load('kin_Winter1991.mat','hip_w91_norm','hip_sd_w91_norm','knee_w91_norm','knee_sd_w91_norm','ankle_w91_norm','ankle_sd_w91_norm');

% Convertion to our generalized coordinates and to radians
kin_W_hip = hip_w91_norm*pi/180; % assuming trunk aligned with vertical
kin_W_hip_sd = hip_sd_w91_norm*pi/180; % assuming trunk aligned with vertical
kin_W_hip_sd(find(kin_W_hip_sd<0.2*max(kin_W_hip_sd))) =  0.2*max(kin_W_hip_sd); % correction to avoid small values

kin_W_knee = -knee_w91_norm*pi/180;
kin_W_knee_sd = knee_sd_w91_norm*pi/180;
kin_W_knee_sd(find(kin_W_knee_sd<0.2*max(kin_W_knee_sd))) =  0.2*max(kin_W_knee_sd); % correction to avoid small values

kin_W_ankle = ankle_w91_norm*pi/180+pi/2;
kin_W_ankle_sd = ankle_sd_w91_norm*pi/180;
kin_W_ankle_sd(find(kin_W_ankle_sd<0.2*max(kin_W_ankle_sd))) =  0.2*max(kin_W_ankle_sd); % correction to avoid small values


% GRFs (mean and standard deviations for normal cadence)
load('GRF_Winter1991.mat','GRF_Z_norm','GRF_Z_dev_norm','GRF_X_norm','GRF_X_dev_norm');

% Converting to our coordinate system and for the specific body mass of
% the model
mass = 66.84; % [kg] total body mass (from walk.c)

grf_W_Y = mass*GRF_Z_norm;   
grf_W_Y_sd = mass*GRF_Z_dev_norm;
grf_W_Y_sd(find(grf_W_Y_sd<0.2*max(grf_W_Y_sd))) =  0.2*max(grf_W_Y_sd); % correction to avoid small values

grf_W_X = mass*GRF_X_norm;   
grf_W_X_sd = mass*GRF_X_dev_norm;
grf_W_X_sd(find(grf_W_X_sd<0.2*max(grf_W_X_sd))) =  0.2*max(grf_W_X_sd); % correction to avoid small values

%% Interpolating for the used number of nodes
% Computing the time step size (half walking cycle duration)
%dt_n = 1/(n_nod-1);
dt_n = T_dur/(n_nod-1);


% Vector of time instants (half gait cycle duration)
T_n = 0:dt_n:dt_n*(n_nod-2); % normalized time vector for half gait cycle
T_cycle_n = [T_n T_n(end)+dt_n+T_n 2*T_dur]; % time vector for whole gait cycle
t_w_n = 0:2*T_dur/50:2*T_dur; % time vector for experimental data of Winter (1991)

kin_W_hip_int = interp1(t_w_n, kin_W_hip, T_cycle_n); 
kin_W_hip_sd_int = interp1(t_w_n, kin_W_hip_sd, T_cycle_n); 
kin_W_knee_int = interp1(t_w_n, kin_W_knee, T_cycle_n); 
kin_W_knee_sd_int = interp1(t_w_n, kin_W_knee_sd, T_cycle_n); 
kin_W_ankle_int = interp1(t_w_n, kin_W_ankle, T_cycle_n); 
kin_W_ankle_sd_int = interp1(t_w_n, kin_W_ankle_sd, T_cycle_n); 

grf_W_X_int = interp1(t_w_n, grf_W_X, T_cycle_n); 
grf_W_X_sd_int = interp1(t_w_n, grf_W_X_sd, T_cycle_n); 
grf_W_Y_int = interp1(t_w_n, grf_W_Y, T_cycle_n); 
grf_W_Y_sd_int = interp1(t_w_n, grf_W_Y_sd, T_cycle_n); 

% Time vector in percentage of the whole gait cycle
tper = 0:100/(2*n_nod-2):100;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Plot right hip, knee and ankle angles in a single figure and compare
%% with Winter (1991) data.
color_win = [0.5 0.5 0.5];
color_sim = [0 0 0]; 
lw_win = 0.75; % line width for Winter data
lw_sim = 1; % line width for simulated data


% Hip angle
subplot(3,1,1)
hold on
plot(tper,kin_W_hip_int*180/pi,'Color',color_win,'LineWidth',lw_win); % hip angle mean of Winter(1991) data
plot(tper,(kin_W_hip_int+kin_W_hip_sd_int)*180/pi,'--','Color',color_win,'LineWidth',lw_win); % hip angle mean of Winter(1991) +1 strd dev.
plot(tper,(kin_W_hip_int-kin_W_hip_sd_int)*180/pi,'--','Color',color_win,'LineWidth',lw_win); % hip angle mean of Winter(1991) -1 strd dev.
plot(tper,xout(4,:)*180/pi,'Color',color_sim,'LineWidth',lw_sim); % simulated hip joint angle
ylim([-20 30])
ylabel('hip flex [deg]')
set(gca,'XTickLabel','')
box on
hold off

% Knee Angle
subplot(3,1,2)
hold on
plot(tper,kin_W_knee_int*180/pi,'Color',color_win,'LineWidth',lw_win); % knee angle mean of Winter(1991) data
plot([tper fliplr(tper)],[kin_W_knee_int+kin_W_knee_sd_int fliplr(kin_W_knee_int-kin_W_knee_sd_int)]*180/pi,'--','Color',color_win,'LineWidth',lw_win); % hip angle mean of Winter(1991) +1 strd dev.
% plot(tper,(kin_W_knee_int+kin_W_knee_sd_int)*180/pi,'--','Color',color_win,'LineWidth',lw_win); % knee angle mean of Winter(1991) +1 strd dev.
% plot(tper,(kin_W_knee_int-kin_W_knee_sd_int)*180/pi,'--','Color',color_win,'LineWidth',lw_win); % knee angle mean of Winter(1991) -1 strd dev.
plot(tper,xout(5,:)*180/pi,'Color',color_sim,'LineWidth',lw_sim); % simulated knee joint angle
ylim([-80 10])
ylabel('knee ext [deg]')
legend('exp mean','exp mean \pm\sigma','simulated')
legend('boxoff')
set(gca,'XTickLabel','')
box on
hold off

% Ankle angle
subplot(3,1,3)
hold on
plot(tper,(kin_W_ankle_int-pi/2)*180/pi,'Color',color_win,'LineWidth',lw_win); % ankle angle mean of Winter(1991) data
plot(tper,(kin_W_ankle_int-pi/2+kin_W_ankle_sd_int)*180/pi,'--','Color',color_win,'LineWidth',lw_win); % ankle angle mean of Winter(1991) +1 strd dev.
plot(tper,(kin_W_ankle_int-pi/2-kin_W_ankle_sd_int)*180/pi,'--','Color',color_win,'LineWidth',lw_win); % ankle angle mean of Winter(1991) -1 strd dev.
plot(tper,(xout(6,:)-pi/2)*180/pi,'Color',color_sim,'LineWidth',lw_sim); % simulated ankle joint angle
ylim([-30 30])
ylabel('dorsiflex [deg]')
xlabel('% gait cycle')
box on
hold off


% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %% Plot right ground reaction forces in a single figure and compare
% %% with Winter (1991) data.
% color_win = [0.5 0.5 0.5];
% color_sim = [0 0 0]; 
% lw_win = 0.75; % line width for Winter data
% lw_sim = 1; % line width for simulated data
% 
% figure
% % Vertical ground reaction force
% subplot(2,1,1)
% hold on
% plot(tper,grf_W_Y_int,'Color',color_win,'LineWidth',lw_win); % vertical grf of Winter(1991) data
% plot(tper,grf_W_Y_int+grf_W_Y_sd_int,'--','Color',color_win,'LineWidth',lw_win); % vertical grf mean of Winter(1991) +1 strd dev.
% plot(tper,grf_W_Y_int-grf_W_Y_sd_int,'--','Color',color_win,'LineWidth',lw_win); % vertical grf mean of Winter(1991) -1 strd dev.
% plot(tper,grf(2,:),'Color',color_sim,'LineWidth',lw_sim); % simulated vertical grf
% ylim([-50 1400])
% ylabel('vertical grf [N]')
% set(gca,'xtick',[0 20 40 60 80 100])
% set(gca,'XTickLabel','')
% box on
% hold off
% 
% % Horizontal ground reaction force
% subplot(2,1,2)
% hold on
% plot(tper,grf_W_X_int,'Color',color_win,'LineWidth',lw_win); % horizontal grf mean of Winter(1991) data
% plot([tper fliplr(tper)],[grf_W_X_int+grf_W_X_sd_int fliplr(grf_W_X_int-grf_W_X_sd_int)],'--','Color',color_win,'LineWidth',lw_win); % horizontal grf mean of Winter(1991) +1 strd dev.
% % plot(tper,grf_W_X_int+grf_W_X_sd_int,'--','Color',color_win,'LineWidth',lw_win); % horizontal grf mean of Winter(1991) +1 strd dev.
% % plot(tper,grf_W_X_int-grf_W_X_sd_int,'--','Color',color_win,'LineWidth',lw_win); % horizontal grf mean of Winter(1991) -1 strd dev.
% plot(tper,grf(1,:),'Color',color_sim,'LineWidth',lw_sim); % simulated horizontal grf
% ylim([-400 200])
% ylabel('horizontal grf [N]')
% legend('exp mean','exp mean \pm\sigma','simulated')
% legend('boxoff')
% set(gca,'xtick',[0 20 40 60 80 100])
% %set(gca,'XTickLabel','')
% xlabel('% gait cycle')
% box on
% hold off



% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %% Plot right lower limb muscle activations in a sigle figure
% color_win = [0.5 0.5 0.5];
% color_sim = [0 0 0]; 
% lw_win = 0.75; % line width for Winter data
% lw_sim = 1; % line width for simulated data
% xtick_vect = [0 20 40 60 80 100]; % vectro of xtick positions
% ytick_vect = [0 1]; % vector of ytick positions
% 
% figure
% hold on
% % Ilio
% subplot(8,1,1)
% hold on
% plot(tper,xout(43,:),'Color',color_sim,'LineWidth',lw_sim); % simulated hip joint angle
% ylim([0 1])
% ylabel('Ilio')
% set(gca,'ytick',ytick_vect)
% set(gca,'xtick',xtick_vect)
% set(gca,'XTickLabel','')
% box on
% hold off
% 
% % Glu
% subplot(8,1,2)
% hold on
% plot(tper,xout(44,:),'Color',color_sim,'LineWidth',lw_sim); % simulated hip joint angle
% ylim([0 1])
% ylabel('Glu')
% set(gca,'ytick',ytick_vect)
% set(gca,'xtick',xtick_vect)
% set(gca,'XTickLabel','')
% box on
% hold off
% 
% % Ham
% subplot(8,1,3)
% hold on
% plot(tper,xout(45,:),'Color',color_sim,'LineWidth',lw_sim); % simulated hip joint angle
% ylim([0 1])
% ylabel('Ham')
% set(gca,'ytick',ytick_vect)
% set(gca,'xtick',xtick_vect)
% set(gca,'XTickLabel','')
% box on
% hold off
% 
% % RF
% subplot(8,1,4)
% hold on
% plot(tper,xout(46,:),'Color',color_sim,'LineWidth',lw_sim); % simulated hip joint angle
% ylim([0 1])
% ylabel('RF')
% set(gca,'ytick',ytick_vect)
% set(gca,'xtick',xtick_vect)
% set(gca,'XTickLabel','')
% box on
% hold off
% 
% % Vas
% subplot(8,1,5)
% hold on
% plot(tper,xout(47,:),'Color',color_sim,'LineWidth',lw_sim); % simulated hip joint angle
% ylim([0 1])
% ylabel('Vas')
% set(gca,'ytick',ytick_vect)
% set(gca,'xtick',xtick_vect)
% set(gca,'XTickLabel','')
% box on
% hold off
% 
% % Gas
% subplot(8,1,6)
% hold on
% plot(tper,xout(48,:),'Color',color_sim,'LineWidth',lw_sim); % simulated hip joint angle
% ylim([0 1])
% ylabel('Gas')
% set(gca,'ytick',ytick_vect)
% set(gca,'xtick',xtick_vect)
% set(gca,'XTickLabel','')
% box on
% hold off
% 
% % Sol
% subplot(8,1,7)
% hold on
% plot(tper,xout(49,:),'Color',color_sim,'LineWidth',lw_sim); % simulated hip joint angle
% ylim([0 1])
% ylabel('Sol')
% set(gca,'ytick',ytick_vect)
% set(gca,'xtick',xtick_vect)
% set(gca,'XTickLabel','')
% box on
% hold off
% 
% % TA
% subplot(8,1,8)
% hold on
% plot(tper,xout(50,:),'Color',color_sim,'LineWidth',lw_sim); % simulated hip joint angle
% ylim([0 1])
% ylabel('TA')
% set(gca,'ytick',ytick_vect)
% set(gca,'xtick',xtick_vect)
% %set(gca,'XTickLabel','')
% xlabel('% gait cycle')
% box on
% hold off




% Use one of the fonts supporteb by Postscript and Ghostscript, for
% example, Courier (default if selected Font is not supported) or
% Helvetica, or Helvetica-Narrow




% % set(gca,'YLim',[-0.5 1.8])
% axis off
% set(gca,'Visible','off','Position',[0 0 1 1])
% %set(gca,'YColor',[1 1 1])
% %'Position',[])
% 
% % Generating eps
% %%%%%%%%%
% %%%%%%%%%
% sc = 1; % Skalierung der Schrift default 2.25 -> doppelte Grösse
% set(0,'DefaultAxesFontsize',sc*10)
% set(0,'DefaultAxesFontName','Times')
% set(0,'DefaultTextFontsize',sc*10)
% set(0,'DefaultTextFontName','Times')
% set(0,'DefaultTextUnits','normalized')
% epsxsize = 8; %Bildgröße in x in cm
% epsysize = epsxsize*(win_size(4)-win_size(3))/(win_size(2)-win_size(1)); %Bildgröße in y in cm
% %%%%%%%%%
% %%%%%%%%%
% 
% epssetup(epsxsize,epsysize,0,[1 1 1],1,1)
% 
% hold off;

%pause