subjects = ['DS100611';'EM072611';'AD052611';'AM062411']; 

% AD looks like right ankle is doing slight negative
% work at 1.25 m/s- don't use this.  Every other side
% on every other subject looks good.

close all;
colors = ['k';'b';'r';'g';'m';'c';'y'];

speeds = [75 100 125 150 175 200];

for i = 1:size(subjects,1)
  eval(['m_stance_work_' subjects(i,:) ' = [];']);
  eval(['sd_stance_work_' subjects(i,:) ' = [];']);
  eval(['m_peak_power_' subjects(i,:) ' = [];']);
  eval(['sd_peak_power_' subjects(i,:) ' = [];']);
  eval(['m_pk_pow_pct_' subjects(i,:) ' = [];']);
  eval(['sd_pk_pow_pct_' subjects(i,:) ' = [];']);
  eval(['m_pk_ang_' subjects(i,:) ' = [];']);
  eval(['sd_pk_ang_' subjects(i,:) ' = [];']);
  eval(['m_Q_power_' subjects(i,:) ' = [];']);
  eval(['sd_Q_power_' subjects(i,:) ' = [];']);
  eval(['m_to_ang_' subjects(i,:) ' = [];']);
  eval(['sd_to_ang_' subjects(i,:) ' = [];']);
  eval(['m_ang_rat_' subjects(i,:) ' = [];']);
  eval(['sd_ang_rat_' subjects(i,:) ' = [];']);
  eval(['m_hs_ang_' subjects(i,:) ' = [];']);
  eval(['sd_hs_ang_' subjects(i,:) ' = [];']);
  
  for j = 1:length(speeds)
    if speeds(j) == 75;
      numstr = '075';
    else
      numstr = num2str(speeds(j));
    end;
    if strcmp(subjects(i,:),'AD052611') == 1
      EMGleg = 'l';
    elseif strcmp(subjects(i,:),'AM062411') == 1
      EMGleg = 'l';
    elseif strcmp(subjects(i,:),'EM072611') == 1  
      EMGleg = 'r';
    elseif strcmp(subjects(i,:),'DS100611') == 1
      EMGleg = 'r';
    else
      fprintf(['Unrecognized subject: ' subjects(i,:) '.\n']);
      return;
    end;
      
    path_full1 = ['~/Documents/MATLAB/data/' subjects(i,:) '/processed/mot/mat/mech/'];

    eval(['load ' path_full1 'walk' numstr '.mat']);
    eval(['load ''~/Documents/MATLAB/data/' subjects(i,:) '/processed/mot/mat/shnk/walk' numstr '_shnk.mat'';']);

    mshnk_ang_hs = mean(shnk_ang_hs);
    outl1 = find(abs(shnk_ang_hs- mean(shnk_ang_hs)) > 20);
    mshnk_ang_pp = mean(shnk_ang_pp);
    outl2 = find(abs(shnk_ang_pp- mean(shnk_ang_pp)) > 20);
    outl3 = union(outl1,outl2);
    shnk_ang(outl3) = [];
    n_shnk_ang(outl3) = [];
    m_shnk_ang  = mean(n_shnk_ang,1);
    sd_shnk_ang = std(n_shnk_ang,0,1);
    shnk_ang_hs(outl3) = [];
    shnk_ang_to(outl3) = [];
    shnk_ang_pp(outl3) = [];
    
    %figure(10)
    %hold on
    %plot(shnk_ang_pp,'r.')
    %hold on
    %figure(11)
    %hold on
    %plot(shnk_ang_hs,'b.')    
    %hold on
    %keyboard
    eval(['num_gc = length(ankle_ang_' EMGleg ');'])
   
    for k = 1:num_gc
      eval(['stance_work(k) = (ankle_work_' EMGleg '{k}(endstance_' EMGleg '(k))-ankle_work_' EMGleg '{k}(1))/mass;']);
      eval(['[peak_power(k),ind1] = max(ankle_pow_' EMGleg '{k}(1:endstance_' EMGleg '(k))/mass);']); 
      eval(['pk_ang(k) = ankle_ang_' EMGleg '{k}(ind1);']);
      eval(['t1 = 0:1/125:((length(ankle_pow_' EMGleg '{k})-1)/125);']);
      eval(['t2 = 0:1/12500:((length(ankle_pow_' EMGleg '{k})-1)/125);']);      
      eval(['iankle_pow_' EMGleg ' = interp1(t1,ankle_pow_' EMGleg '{k},t2,''linear'');']);
      eval(['[val1i,ind1i] = max(iankle_pow_' EMGleg ');']);
      eval(['[val2,ind2] = min(abs(iankle_pow_' EMGleg '(1:ind1i)-peak_power(k)*mass/2));']);
      eval(['[val3,ind3] = min(abs(iankle_pow_' EMGleg '(ind1i:end)-peak_power(k)*mass/2));']);      
      ind3 = ind3+ind1i-1;      
      eval(['Q_power(k) = (ind3-ind2)*100/(length(iankle_pow_' EMGleg ')-1);']);     
      eval(['[val4,ind4] = max(n_ankle_pow_' EMGleg '(k,:));']); 
      pk_pow_pct(k) = ind4 - 1; 
      eval(['to_ang(k) = ankle_ang_' EMGleg '{k}(endstance_' EMGleg '(k));']);      
      eval(['hs_ang(k) = ankle_ang_' EMGleg '{k}(1);']);      
    end;

    ang_rat = 1-shnk_ang_pp./shnk_ang_hs;        
    
    eval(['m_stance_work_' subjects(i,:) '=cat(1,m_stance_work_' subjects(i,:) ',mean(stance_work));']);
    eval(['sd_stance_work_' subjects(i,:) '=cat(1,sd_stance_work_' subjects(i,:) ',std(stance_work,0));']);
    eval(['m_peak_power_' subjects(i,:) '=cat(1,m_peak_power_' subjects(i,:) ',mean(peak_power));']);
    eval(['sd_peak_power_' subjects(i,:) '=cat(1,sd_peak_power_' subjects(i,:) ',std(peak_power,0));']);
    eval(['m_pk_pow_pct_' subjects(i,:) '=cat(1,m_pk_pow_pct_' subjects(i,:) ',mean(pk_pow_pct));']);
    eval(['sd_pk_pow_pct_' subjects(i,:) '=cat(1,sd_pk_pow_pct_' subjects(i,:) ',std(pk_pow_pct,0));']);
    eval(['m_pk_ang_' subjects(i,:) '=cat(1,m_pk_ang_' subjects(i,:) ',mean(pk_ang));']);
    eval(['sd_pk_ang_' subjects(i,:) '=cat(1,sd_pk_ang_' subjects(i,:) ',std(pk_ang,0));']);
    eval(['m_Q_power_' subjects(i,:) '=cat(1,m_Q_power_' subjects(i,:) ',mean(Q_power));']);
    eval(['sd_Q_power_' subjects(i,:) '=cat(1,sd_Q_power_' subjects(i,:) ',std(Q_power,0));']);
    eval(['m_to_ang_' subjects(i,:) '=cat(1,m_to_ang_' subjects(i,:) ',mean(to_ang));']);
    eval(['sd_to_ang_' subjects(i,:) '=cat(1,sd_to_ang_' subjects(i,:) ',std(to_ang,0));']);
    eval(['m_ang_rat_' subjects(i,:) '=cat(1,m_ang_rat_' subjects(i,:) ',mean(ang_rat));']);
    eval(['sd_ang_rat_' subjects(i,:) '=cat(1,sd_ang_rat_' subjects(i,:) ',std(ang_rat,0));']);
    eval(['m_hs_ang_' subjects(i,:) '=cat(1,m_hs_ang_' subjects(i,:) ',mean(hs_ang));']);
    eval(['sd_hs_ang_' subjects(i,:) '=cat(1,sd_hs_ang_' subjects(i,:) ',std(hs_ang,0));']);

    figure(1)
    errorbar(speeds(j)/100,mean(stance_work),std(stance_work,0),'color',colors(i),'linewidth',2);
    hold on;
    plot(speeds(j)/100,mean(stance_work),'.','color',colors(i),'markersize',20);
    figure(2)
    errorbar(speeds(j)/100,mean(peak_power),std(peak_power,0),'color',colors(i),'linewidth',2);
    hold on;
    plot(speeds(j)/100,mean(peak_power),'.','color',colors(i),'markersize',20);
    figure(3)
    errorbar(speeds(j)/100,mean(pk_pow_pct),std(pk_pow_pct,0),'color',colors(i),'linewidth',2);
    hold on;
    plot(speeds(j)/100,mean(pk_pow_pct),'.','color',colors(i),'markersize',20);
    figure(4)
    errorbar(speeds(j)/100,mean(pk_ang),std(pk_ang,0),'color',colors(i),'linewidth',2);
    hold on;
    plot(speeds(j)/100,mean(pk_ang),'.','color',colors(i),'markersize',20);
    figure(5)
    errorbar(speeds(j)/100,mean(Q_power),std(Q_power,0),'color',colors(i),'linewidth',2);
    hold on;
    plot(speeds(j)/100,mean(Q_power),'.','color',colors(i),'markersize',20);
    figure(6)
    errorbar(speeds(j)/100,mean(to_ang),std(to_ang,0),'color',colors(i),'linewidth',2);
    hold on;
    plot(speeds(j)/100,mean(to_ang),'.','color',colors(i),'markersize',20);
    figure(7)
    errorbar(speeds(j)/100,mean(ang_rat),std(ang_rat,0),'color',colors(i),'linewidth',2);
    hold on;
    plot(speeds(j)/100,mean(ang_rat),'.','color',colors(i),'markersize',20);
    figure(8)
    errorbar(speeds(j)/100,mean(hs_ang),std(hs_ang,0),'color',colors(i),'linewidth',2);
    hold on;
    plot(speeds(j)/100,mean(hs_ang),'.','color',colors(i),'markersize',20);

    clear stance_work peak_power pk_ang Q_power to_ang ang_rat hs_ang;
  end;    
  
  %figure(1)
  %eval(['errorbar(speeds/100,m_stance_work_' subjects(i,:) ',sd_stance_work_' subjects(i,:) ',''color'',colors(i),''linewidth'',2);']);
  %hold on;
  %eval(['plot(speeds/100,m_stance_work_' subjects(i,:) ',''.'',''color'',colors(i),''markersize'',20);']);
  %figure(2)
  %eval(['errorbar(speeds/100,m_peak_power_' subjects(i,:) ',sd_peak_power_' subjects(i,:) ',''color'',colors(i),''linewidth'',2);']);
  %hold on;
  %eval(['plot(speeds/100,m_peak_power_' subjects(i,:) ',''.'',''color'',colors(i),''markersize'',20);']);
  %figure(3)
  %eval(['errorbar(speeds/100,m_pk_pow_pct_' subjects(i,:) ',sd_pk_pow_pct_' subjects(i,:) ',''color'',colors(i),''linewidth'',2);']);
  %hold on;
  %eval(['plot(speeds/100,m_pk_pow_pct_' subjects(i,:) ',''.'',''color'',colors(i),''markersize'',20);']);
  %figure(4)
  %eval(['errorbar(speeds/100,m_pk_ang_' subjects(i,:) ',sd_pk_ang_' subjects(i,:) ',''color'',colors(i),''linewidth'',2);']);
  %hold on;
  %eval(['plot(speeds/100,m_pk_ang_' subjects(i,:) ',''.'',''color'',colors(i),''markersize'',20);']);
  %%figure(5)
  %%eval(['errorbar(speeds/100,m_Q_power_' subjects(i,:) ',sd_Q_power_' subjects(i,:) ',''color'',colors(i),''linewidth'',2);']);
  %%hold on;
  %%eval(['plot(speeds/100,m_Q_power_' subjects(i,:) ',''.'',''color'',colors(i),''markersize'',20);']);
  %figure(6)
  %eval(['errorbar(speeds/100,m_to_ang_' subjects(i,:) ',sd_to_ang_' subjects(i,:) ',''color'',colors(i),''linewidth'',2);']);
  %hold on;
  %eval(['plot(speeds/100,m_to_ang_' subjects(i,:) ',''.'',''color'',colors(i),''markersize'',20);']);
  %figure(7)
  %eval(['errorbar(speeds/100,m_ang_rat_' subjects(i,:) ',sd_ang_rat_' subjects(i,:) ',''color'',colors(i),''linewidth'',2);']);
  %hold on;
  %eval(['plot(speeds/100,m_ang_rat_' subjects(i,:) ',''.'',''color'',colors(i),''markersize'',20);']);
  %figure(8)
  %eval(['errorbar(speeds/100,m_hs_ang_' subjects(i,:) ',sd_hs_ang_' subjects(i,:) ',''color'',colors(i),''linewidth'',2);']);
  %hold on;
  %eval(['plot(speeds/100,m_hs_ang_' subjects(i,:) ',''.'',''color'',colors(i),''markersize'',20);']);

end;

figure(1)
grid on;
xlabel('Walking Speed (m/s)','fontsize',14);
ylabel('Ankle Work in Stance / Body Mass (J/kg)','fontsize',14);
title('Ankle Work in Stance / Body Mass vs. Walking Speed','fontsize',14);
print -djpeg -r300 'mech_plots/stance_work_vs_speed.jpeg';
figure(2)
grid on;
xlabel('Walking Speed (m/s)','fontsize',14);
ylabel('Ankle Peak Power / Body Mass in Stance (W/kg)','fontsize',14);
title('Ankle Peak Power / Body Mass in Stance vs. Walking Speed','fontsize',14);
print -djpeg -r300 'mech_plots/peak_power_vs_speed.jpeg';
figure(3)
grid on;
xlabel('Walking Speed (m/s)','fontsize',14);
ylabel('Percent GC where Ankle Peak Power occurs','fontsize',14);
title('Percent GC where Ankle Peak Power occurs vs. Walking Speed','fontsize',14);
print -djpeg -r300 'mech_plots/pk_pow_pct_vs_speed.jpeg';
figure(4)
grid on;
xlabel('Walking Speed (m/s)','fontsize',14);
ylabel('Dorsiflexion Angle at Peak Power (^\circ)','fontsize',14);
title('Ankle Angle at Peak Power vs. Walking Speed','fontsize',14);
print -djpeg -r300 'mech_plots/pk_ang_vs_speed.jpeg';
figure(5)
grid on;
xlabel('Walking Speed (m/s)','fontsize',14);
ylabel('Ankle Power Q Value (Pct GC)','fontsize',14);
title('Ankle Power Q Value vs. Walking Speed','fontsize',14);
print -djpeg -r300 'mech_plots/Q_power_vs_speed.jpeg';
figure(6)
grid on;
xlabel('Walking Speed (m/s)','fontsize',14);
ylabel('Dorsiflexion Angle at Toe Off (^\circ)','fontsize',14);
title('Ankle Angle at Toe Off vs. Walking Speed','fontsize',14);
print -djpeg -r300 'mech_plots/to_ang_vs_speed.jpeg';
figure(7)
grid on;
xlabel('Walking Speed (m/s)','fontsize',14);
ylabel('1 - \theta_{shnk,pp}/\theta_{shnk,hs}','fontsize',14);
title('1 - \theta_{shnk,pp}/\theta_{shnk,hs} vs. Walking Speed','fontsize',14);
print -djpeg -r300 'mech_plots/ang_rat_vs_speed.jpeg';
figure(8)
grid on;
xlabel('Walking Speed (m/s)','fontsize',14);
ylabel('Dorsiflexion Angle at Heel Strike (^\circ)','fontsize',14);
title('Ankle Angle at Heel Strike vs. Walking Speed','fontsize',14);
print -djpeg -r300 'mech_plots/hs_ang_vs_speed.jpeg';

speeds = speeds/100;
save('mech_plots/ankle_mech_data.mat','*_AD052611','*_AM062411','*_EM072611','*_DS100611','speeds');