%% SwingTestDataAnalysis.m
% This function is to analyze the motion capture data from the swing test
% and extract the needed metrics (TC, peak knee angle, swing duration,
% etc.)
% PData structure: PData{knee,ankle}.Trial{trial}(timestep,coordinate,marker)
% Coordinate order: x(lateral) y(ant-post) z(prox-dist)
% Marker order: 1) Hip 2) Knee 3) Ankle 4) Heel 5) Toes 6) Actuator Bottom 7) Actuator Top

clear
close all
%% Boolean Inputs
% Plot individual trials
plot_animate = 1;           % Plot an animation of each trial one at a time
plot_angles = 1;            % Plot the joint angle profiles of each trial
plot_all_moments = 1;       % plot the moment profiles of each trial
plot_Toe = 1;               % Plot the toe clearance of each trial

% Plot Trial comparisons
plot_ani_comp = 1;          % Plot animations of multiple trials in the same figure
plot_angle_comp = 10;        % Plot the average joint moment profiles of multiple conditions in the same figure

% Plot Test metric Data
plot_metrics = 1;           % Plot the box plots for the metrics
tab_metrics = 1;            % Output a table of the metrics

%% Load the Data
% load the data into a cell with indices correlating to knee and ankle ma's
% These are the folders housing the motion capture data. Update as
% necessary
folder = [{'./Passive MoCap Data/'},{'./Active MoCap Data/'},{'./Transfemoral Amputee Kinematic Data/TF19 (3R60 Knee)/Matlab Workspace/TF19_DATA.mat'},{'./Low Friction MoCap Data/'}];


% passive test data, standard friction model
[PMetrics,PAngles,PData,Pind] = process_tests(folder(1),[]);

% active test data, high friction model
[AMetrics,AAngles,AData,Aind] = process_tests(folder(2),[]);

% Hood's transfemoral amputee gait data
% This data can be downloaded from https://doi.org/10.6084/m9.figshare.12221543
% Base article: 
% Hood, S., Ishmael, M.K., Gunnell, A. et al. A kinematic and
% kinetic dataset of 18 above-knee amputees walking at various speeds. Sci
% Data 7, 150 (2020). https://doi.org/10.1038/s41597-020-0494-7
[HMetrics,HAngles,HData,Hind] = process_hood(folder{3});

% passive, active, and active 2 data, low friction model
[ALFMetrics,ALFAngles,ALFData,ALFind] = process_tests({[folder{4},'*spring1']},[]);
[ALF2Metrics,ALF2Angles,ALF2Data,ALF2ind] = process_tests({[folder{4},'*spring2']},7);
[PLFMetrics,PLFAngles,PLFData,PLFind] = process_tests({[folder{4},'Passive']},[]);

[SAngles] = process_sim;

%% Get Moments and Forces
% [Forces,Moments,MA] = calc_forces(AData,Aind);

%% Animate
if plot_animate
   animate(PData,5*ones(1,2),[1:2],Pind) 
   animate(AData,1*ones(1,3),[1:3],Aind)
%    animate(AData,2*ones(1,3),[1:3],Aind)
%    animate(AData,3*ones(1,3),[1:3],Aind)
%    animate(AData,4*ones(1,2),[1:2],Aind)
%    animate(AData,5*ones(1,3),[1:3],Aind)
%    animate(AData,6*ones(1,3),[1:3],Aind)
%    animate(AData,7*ones(1,3),[1:3],Aind)
%    animate(AData,8*ones(1,3),[1:3],Aind)
%    animate(AData,9*ones(1,3),[1:3],Aind)
end


%% Plot Joint Angles
if plot_angles
    plotting_angles(AAngles,AData,Aind)
    plotting_angles(PAngles,PData,Pind)

    plotting_angles(PLFAngles,PLFData,PLFind)
    plotting_angles(ALFAngles,ALFData,ALFind)
    plotting_angles(ALF2Angles,ALF2Data,ALF2ind)
end

%% Plot Joint Moments
if plot_all_moments
   plotting_moments(AAngles,AData,Aind,Moments,Forces,MA) 
end

   

%% Plot Metrics
AllMetrics(:,:,2) = PMetrics;
AllMetrics(1:6,1:6,1) = AMetrics;
AllAngles(:,:,2) = PAngles;
AllAngles(1:6,1:6,1) = AAngles;
AllData(:,:,2) = PData;
AllData(:,:,1) = AData;

AllMetricsLF(:,:,1) = PLFMetrics;
AllMetricsLF(:,:,2) = ALFMetrics;
AllMetricsLF(:,:,3) = ALF2Metrics;


if plot_metrics
    plotting_metrics(AllMetrics,{'Active','Passive'})
%     plotting_metrics2(AllMetricsLF,{'Passive','Active','Active2'})
    
    plotting_metrics_together(AllMetricsLF,{'Passive','Active','Active2'})
end


if tab_metrics
    metrics_tab = output_metrics(AllMetrics)
%     writetable(metrics_tab,'Test Result Metrics.xlsx','Sheet',2)
    
    metrics_tab_lf = output_metrics(AllMetricsLF)
%     writetable(metrics_tab_lf,'Test Result Metrics Low Friction.xlsx','Sheet',2,'Range','B1:M16')
    
end

%% Plot Toe Clearance
if plot_Toe
%    plotting_TC(AAngles,AData,AMetrics,Aind); 
%    plotting_TC(PAngles,PData,PMetrics,Pind);
   plotting_TC(AllAngles,AllData,AllMetrics,{Aind,Pind});
end

%% Compare Animations
if plot_ani_comp
    % Plot the mean of the 1st active condition with the trials
%     ani_comp({AData{Aind(1)}.Mean,AData{Aind(1)}.Trial{1},AData{Aind(1)}.Trial{2},AData{Aind(1)}.Trial{3}},...
%         {'Mean','Trial 1','Trial 2','Trial 3'},{'-ok','-o','-o','-o'},[2,1,1,1],[1,0,0,0])

%      disp('Continue? (Press "Enter")')
%     pause
    
    % Compare active and passive modes
    ani_comp({AData{2,4}.Mean,PData{1,4}.Mean},{'Active, k4, a4','Passive, k4, a4'},{'-or','-ob'},[1,1],[1,0])

end
%% Compare Angles
if plot_angle_comp
    
%     angle_comp({AAngles{2,2}.Mean,PAngles{1,2}.Mean},{'Active, k2, a2','Passive, k1, a2'},{'-r','--b'},[1,1])
%     angle_comp({AAngles{2,4}.Mean,PAngles{1,4}.Mean},{'Active, k2, a4','Passive, k1, a4'},{'-r','--b'},[1,1])
%     angle_comp({AAngles{2,6}.Mean,PAngles{1,6}.Mean},{'Active, k2, a6','Passive, k1, a6'},{'-r','--b'},[1,1])
    
%     angle_comp({AAngles{4,2}.Mean,PAngles{4,2}.Mean},{'Active, k4, a2','Passive, k4, a2'},{'-r','--b'},[1,1])
%     angle_comp({AAngles{4,4}.Mean,PAngles{4,4}.Mean},{'Active, k4, a4','Passive, k4, a4'},{'-r','--b'},[1,1])
%     angle_comp({AAngles{4,6}.Mean,PAngles{4,6}.Mean},{'Active, k4, a6','Passive, k4, a6'},{'-r','--b'},[1,1])
    
%     angle_comp({AAngles{6,2}.Mean,PAngles{6,2}.Mean},{'Active, k6, a2','Passive, k6, a2'},{'-r','--b'},[1,1])
%     angle_comp({AAngles{6,4}.Mean,PAngles{6,4}.Mean},{'Active, k6, a4','Passive, k6, a4'},{'-r','--b'},[1,1])
%     angle_comp({AAngles{6,6}.Mean,PAngles{6,6}.Mean},{'Active, k6, a6','Passive, k6, a6'},{'-r','--b'},[1,1])
    
%     angle_comp({PAngles{1,2}.Mean,PAngles{4,2}.Mean,PAngles{6,2}.Mean,HAngles{5}.Mean,SAngles},{'Passive, K1, A2','Passive, K4, A2','Passive, K6, A2','Reference','Simulated'},{'-b','--r','-.g','-k','--k'},[1,1,1,2,2])
%     angle_comp({PAngles{1,4}.Mean,PAngles{4,4}.Mean,PAngles{6,4}.Mean,HAngles{5}.Mean,SAngles},{'Passive, K1, A4','Passive, K4, A4','Passive, K6, A4','Reference','Simulated'},{'-b','--r','-.g','-k','--k'},[1,1,1,2,2])
    angle_comp({PAngles{1,6}.Mean,PAngles{4,6}.Mean,PAngles{6,6}.Mean,HAngles{5}.Mean,SAngles},{'Passive, K1, A6','Passive, K4, A6','Passive, K6, A6','Reference','Simulated'},{'-b','--r','-.g','-k','--k'},[1,1,1,2,2],'Passive-Reference Angle Comparisons; Full Friction')
        
    angle_comp({PLFAngles{4,4}.Mean,ALFAngles{4,4}.Mean,ALF2Angles{4,4}.Mean,HAngles{5}.Mean,SAngles},{'Passive, K4, A4','Active, K4, A4','Active2, K4, A4','Reference','Simulated'},{'-b','--r','-.g','-k','--k'},[1,1,1,2,2],'Active-Passive Angle Comparison; K4, A4, Low Friction')
    angle_comp({PLFAngles{6,4}.Mean,ALFAngles{6,4}.Mean,ALF2Angles{6,4}.Mean,HAngles{5}.Mean,SAngles},{'Passive, K6, A4','Active, K6, A4','Active2, K6, A4','Reference','Simulated'},{'-b','--r','-.g','-k','--k'},[1,1,1,2,2],'Active-Passive Angle Comparison; K6, A4, Low Friction')
    angle_comp({PLFAngles{2,4}.Mean,ALFAngles{2,4}.Mean,ALF2Angles{2,4}.Mean,HAngles{5}.Mean,SAngles},{'Passive, K2, A4','Active, K2, A4','Active2, K2, A4','Reference','Simulated'},{'-b','--r','-.g','-k','--k'},[1,1,1,2,2],'Active-Passive Angle Comparison; K2, A4, Low Friction')

    angle_comp({PLFAngles{2,4}.Mean,PLFAngles{4,4}.Mean,PLFAngles{6,4}.Mean,HAngles{5}.Mean,SAngles},{'Passive, K2, A4','Passive, K4, A4','Passive, K6, A4','Reference','Simulated'},{'-b','--r','-.g','-k','--k'},[1,1,1,2,2],'Passive-Reference Angle Comparisons; Low Friction')
end


%% Sub Functions

function [Metrics,Angles,Data,ind] = process_tests(folder,skip)
% This function takes in a folder location and processes all .mat test
% files in it, outputting 3 structures, 1 for metrics, 1 for joint angles,
% and 1 for marker locations


    %% Get Data
    j=1;
    Data = cell(6,7);
    files{j} = dir([folder{j},'*.mat']);
    iter = 0;
    % ind = ones(length(files{j}));
    % take off any wildcards
    pass = 0;
    LF = 0;
    while pass==0
        if folder{1}(end) ~= '/'
            folder{1}=folder{1}(1:end-1);
            LF=1;
        else
            pass=1;
        end
    end
    
    for i = 1:length(files{j})
        if ~sum(i == skip)
            data = load([folder{j},files{1}(i).name]);
            filename = fields(data);
            % check if active or passive file
            if filename{1}(1) == 'A'
                ki  = 9;
                ai = 12;
            elseif filename{1}(1) == 'P'
                ki  = 10;
                ai = 13;
            else
                error('shit')
            end

            knee = str2double(filename{1}(ki));
            ankle = str2double(filename{1}(ai));
            %trial = str2double(filename{1}(end));
            data = eval(['data.',filename{1}]);
            % truncate front and after 200 sec, normalize to ground
            if length(data.Trajectories.Labeled.Data(:,1,1)) == 10
                clear structure
                time_length = 499;
                ground = mean(data.RigidBodies.Positions(1,3,:),'omitnan');
                start = 1; %find(data.Trajectories.Labeled.Data(9,3,:)<(ground),1);
                structure(1,:,:) = mean(data.Trajectories.Labeled.Data([3,6],1:3,start:start+time_length),'omitnan'); % hip centerline
                structure(2,:,:) = mean(data.Trajectories.Labeled.Data([4,7],1:3,start:start+time_length),'omitnan'); % knee centerline
                structure(3,:,:) = mean(data.Trajectories.Labeled.Data([5,8],1:3,start:start+time_length),'omitnan'); % ankle centerline
                structure(5,:,:) = mean(data.Trajectories.Labeled.Data([9,10],1:3,start:start+time_length),'omitnan'); % toes centerline
                structure(6,:,:) = data.Trajectories.Labeled.Data(1,1:3,start:start+time_length); % actuator bottom
                structure(7,:,:) = data.Trajectories.Labeled.Data(2,1:3,start:start+time_length); % actuator top
                structure(:,3,:) = structure(:,3,:) - ground;
                structure = permute(structure,[3,2,1]);
                if std(structure(100:250,2,3)) > 5
                    if isempty(Data{knee,ankle}) % indexes of different test conditions
                        iter = iter+1;
                        ind(iter) = knee + (ankle-1)*6;
                        trial = 1;
                    else
                        trial = length(Data{knee,ankle}.Trial)+1;
                        if trial ==4
                            p=1;
                        end
                    end
                    if LF ==0
                        Data{knee,ankle}.Trial{trial} = structure; %permute(structure,[3,2,1]);
                    else
                        Data{knee,ankle}.Trial{trial}(:,1,:) = -structure(:,2,:);
                        Data{knee,ankle}.Trial{trial}(:,2,:) = structure(:,1,:);
                        Data{knee,ankle}.Trial{trial}(:,3,:) = structure(:,3,:);
                    end
                    Data{knee,ankle}.knee = knee;
                    Data{knee,ankle}.ankle = ankle;
                    Data{knee,ankle}.start(trial) = start;
                end
            end
        end
    end

    %% Get Angles
    for i = 1:length(ind) % for each condition
        for j = 1:length(Data{ind(i)}.Trial) % for each trial
            for k = 1:time_length % for each timestep
                % thigh LoA
                X_thigh = [Data{ind(i)}.Trial{j}(k,2:3,1),0] - [Data{ind(i)}.Trial{j}(k,2:3,2),0]; % thigh - shank
                Xn_thigh = X_thigh/dist(Data{ind(i)}.Trial{j}(k,2:3,1) , Data{ind(i)}.Trial{j}(k,2:3,2));

                % Hip Angle 
                th_hip = acos(dot([0,1,0],Xn_thigh))*sign(Data{ind(i)}.Trial{j}(k,2,2) - Data{ind(i)}.Trial{j}(k,2,1));

                % shank LoA
                X_shank = [Data{ind(i)}.Trial{j}(k,2:3,2),0] - [Data{ind(i)}.Trial{j}(k,2:3,3),0];
                Xn_shank = X_shank/(dist(Data{ind(i)}.Trial{j}(k,2:3,2),Data{ind(i)}.Trial{j}(k,2:3,3)));

                % Knee Angle
                th_knee = acos(dot(Xn_thigh,Xn_shank))*sign(Xn_shank(1) - Xn_thigh(1));

                % Toes LoA
                X_toes = [Data{ind(i)}.Trial{j}(k,2:3,3),0] - [Data{ind(i)}.Trial{j}(k,2:3,5),0];
                Xn_toes = X_toes/dist(Data{ind(i)}.Trial{j}(k,2:3,3), Data{ind(i)}.Trial{j}(k,2:3,5));

                % Toes Angle
                th_toes = acos(dot(Xn_shank,Xn_toes))*sign(Xn_shank(1) - Xn_toes(1));

                % Ankle Angle
                th_foot = atan(63/173);
                th_ankle = (th_toes + th_foot) - pi/2;

                % Foot LoA
                Xn_foot = [cos(th_foot)*Xn_toes(1) - sin(th_foot)*Xn_toes(2), sin(th_foot)*Xn_toes(1) + cos(th_foot)*Xn_toes(2),0];
                X_foot = Xn_foot*173;

                % Ankle LoA
                Xn_ankle = [Xn_foot(2),-Xn_foot(1),0];
                X_ankle = Xn_ankle*63;

                % Heel Location
                q_heel1 = [Data{ind(i)}.Trial{j}(k,2:3,5),0] + X_foot;
                q_heel2 = [Data{ind(i)}.Trial{j}(k,2:3,3),0] - X_ankle;

                Data{ind(i)}.Trial{j}(k,:,4) = [0,mean([q_heel1(1),q_heel2(1)],'omitnan'),mean([q_heel1(2),q_heel2(2)],'omitnan')];

                % Make Angles Matrix
                Angles{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Trial{j}(k,:) = [th_hip,th_knee,th_ankle];
            end
                if i ==5 && j==4
                    p=1;
                end
        end
    end

    %% Determine Start Time for Trial (and mean)
    for i = 1:length(ind)
        for j = 1:length(Data{ind(i)}.Trial)
            % Use rate of knee flexion (ie. knee buckle) as indicator of TO
            filt_ang = filter(0.125*ones(1,8),0.5*ones(1,2),Angles{ind(i)}.Trial{j}(:,:));
            
%%%%%%%%%%%%%%%%%%% Sidebar, check filtering
%                     figure; clf; 
%                     subplot(3,1,1) % hip
%                     plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,rad2deg(Angles{ind(i)}.Trial{j}(:,1)))
%                     hold on
%                     plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,rad2deg(filt_ang(:,1)))
%                     ylabel('hip angle (deg)')
%                     title(['Knee ',num2str(Data{ind(i)}.knee),', Ankle ',num2str(Data{ind(i)}.ankle),', Trial ',num2str(j)])
%                     legend('Raw Data','Filtered','Location','NorthWest')
%                     subplot(3,1,2) % knee
%                     plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,rad2deg(Angles{ind(i)}.Trial{j}(:,2)))
%                     hold on
%                     plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,rad2deg(filt_ang(:,2)))
%                     ylabel('knee angle (deg)')
%                     grid minor
%                     subplot(3,1,3) % ankle - or TC
%                     plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,rad2deg(Angles{ind(i)}.Trial{j}(:,3)))
%                     hold on
%                     plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,rad2deg(filt_ang(:,3)))
%                     ylabel('ankle angle (deg)')
%                     xlabel('time (sec)')
%%%%%%%%%%%%%%%%%%%%%%%%%            
            st_ind = find((filt_ang(2:100,2) - filt_ang(1:100-1,2)) >= deg2rad(2),1) - 10;
            mark = 'kx';
%             st_ind = find((Angles{ind(i)}.Trial{j}(2:200,2) - Angles{ind(i)}.Trial{j}(1:199,2)) >= deg2rad(2),1);
            if isempty(st_ind)
%                 st_ind = find((Angles{ind(i)}.Trial{j}(2:200,2) - Angles{ind(i)}.Trial{j}(1:199,2)) >= deg2rad(1),1);
                st_ind = find((filt_ang(2:100,2) - filt_ang(1:100-1,2)) >= deg2rad(1.4),1) - 10;
                mark = 'rx';
                if isempty(st_ind)
                    st_ind = find((filt_ang(2:end,2) - filt_ang(1:end-1,2)) >= deg2rad(2),1) - 10;
                    mark = 'kx';
                end
            end
%%%%%%%%%%%%%%%%%%%%%%%%%            
%                     subplot(3,1,2) % knee
%                     plot((st_ind-1)/100,rad2deg(Angles{ind(i)}.Trial{j}(st_ind,2)),mark)
%%%%%%%%%%%%%%%%%%%%%%%%%            
            Data{ind(i)}.Trial{j} = Data{ind(i)}.Trial{j}(st_ind:st_ind+96,:,:);
            Angles{ind(i)}.Trial{j} = Angles{ind(i)}.Trial{j}(st_ind:st_ind+96,:);
            Angles{ind(i)}.st_ind(j) = st_ind;
        end
        % Get average Angles
        AngleMat = [Angles{ind(i)}.Trial{:}];
        AvgHip = mean(AngleMat(:,(1:length(Angles{ind(i)}.Trial))*3 - 2),2,'omitnan');
        AvgKnee = mean(AngleMat(:,(1:length(Angles{ind(i)}.Trial))*3 - 1),2,'omitnan');
        AvgAnkle = mean(AngleMat(:,(1:length(Angles{ind(i)}.Trial))*3),2,'omitnan');
        Angles{ind(i)}.Mean = [AvgHip,AvgKnee,AvgAnkle];
        
        % Get average Positions
        DataMat = [Data{ind(i)}.Trial{:}];
        AvgMarkerx = mean(DataMat(:,(1:length(Data{ind(i)}.Trial))*3 - 2,:),2,'omitnan');
        AvgMarkery = mean(DataMat(:,(1:length(Data{ind(i)}.Trial))*3 - 1,:),2,'omitnan');
        AvgMarkerz = mean(DataMat(:,(1:length(Data{ind(i)}.Trial))*3 ,:),2,'omitnan');
        Data{ind(i)}.Mean =  [AvgMarkerx,AvgMarkery,AvgMarkerz];
    end

    %% Determine End time for trial
    iter = 0;
    for i = 1:length(ind)
        filt_ang_mean = filter(0.125*ones(1,8),0.5*ones(1,2),Angles{ind(i)}.Mean(:,:));
        et_ind = find(islocalmin(filt_ang_mean(11:60,2),'MaxNumExtrema', 1),1)+10; % for plotting purposes
        if isempty(et_ind)
            et_ind = find(islocalmin(filt_ang_mean(11:end,2),'MaxNumExtrema', 1),1)+10; % for plotting purposes
        end
        Angles{ind(i)}.Mean = Angles{ind(i)}.Mean(1:et_ind,:);
        Data{ind(i)}.Mean = Data{ind(i)}.Mean(1:et_ind,:,:);
        for j = 1:length(Data{ind(i)}.Trial)
            iter = iter+1;
            filt_ang = filter(0.125*ones(1,8),0.5*ones(1,2),Angles{ind(i)}.Trial{j}(:,:));
            temp = find(islocalmin(filt_ang(11:60,2),'MaxNumExtrema', 1),1)+8;
            if isempty(temp)
                Data{ind(i)}.et_ind(j) = find(islocalmin(filt_ang(11:end,2),'MaxNumExtrema', 1),1)+8;
            else
                Data{ind(i)}.et_ind(j) = temp;
            end
            clear temp;
%%%%%%%%%%%%%%%
%                 figure(iter)
%                 subplot(3,1,2)
%                 plot((Angles{ind(i)}.st_ind(j) + Data{ind(i)}.et_ind(j) - 2)/100, rad2deg(Angles{ind(i)}.Trial{j}(Data{ind(i)}.et_ind(j),2)),'gx')
%                 plot((Angles{ind(i)}.st_ind(j) + et_ind - 2)/100, rad2deg(Angles{ind(i)}.Trial{j}(et_ind,2)),'kx')
%%%%%%%%%%%%%%%   
            Data{ind(i)}.Trial{j} = Data{ind(i)}.Trial{j}(1:Data{ind(i)}.et_ind(j),:,:);
            Angles{ind(i)}.Trial{j} = Angles{ind(i)}.Trial{j}(1:Data{ind(i)}.et_ind(j),:);
             
        end
    end

    %% Calculate Metrics

    for i =1:length(ind)
        for j = 1:length(Data{ind(i)}.Trial)
            % Peak Knee Flexion
            [val,val_ind] = max(rad2deg(Angles{ind(i)}.Trial{j}(:,2)));
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.PeakKneeFlex.data(j) = val;
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.PeakKneeFlex.ind(j) = val_ind;

            % Peak Dorsiflexion
            [val,val_ind] = max(rad2deg(Angles{ind(i)}.Trial{j}(:,3)));
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.PeakDorsiFlex.data(j) = val;
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.PeakDorsiFlex.ind(j) = val_ind;

            % Swing Duration
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.SwingDur.data(j) = length(Data{ind(i)}.Trial{j});

            % MTC
            val_ind = find(islocalmin(Data{ind(i)}.Trial{j}(:,3,5),'MaxNumExtrema',2));
            val = Data{ind(i)}.Trial{j}(val_ind,3,5);
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.MTC.data(j) = val(end);
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.MTC.ind(j) = val_ind(end);
        end
        % Peak Knee Flexion
        [val,val_ind] = max(rad2deg(Angles{ind(i)}.Mean(:,2)));
        Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.PeakKneeFlex.mean = val;
        Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.PeakKneeFlex.meanind = val_ind;

        % Peak Dorsiflexion
        [val,val_ind] = max(rad2deg(Angles{ind(i)}.Mean(:,3)));
        Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.PeakDorsiFlex.mean = val;
        Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.PeakDorsiFlex.meanind = val_ind;

        % Swing Duration
        Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.SwingDur.mean = Data{ind(i)}.et_ind(j); %find(islocalmin(Angles{ind(i)}.Mean(:,2)),1);

    end


end

function [Metrics,Angles,Data,ind] = process_Ptests(folder)
% This function takes in a folder location and processes all .mat test
% files in it, outputting 3 structures, 1 for metrics, 1 for joint angles,
% and 1 for marker locations


    %% Get Data
    j=1;
    Data = cell(6,7);
    files{j} = dir([folder{j},'*.mat']);
    iter = 0;
    % ind = ones(length(files{j}));
    for i = 1:length(files{j})
        data = load([folder{j},files{1}(i).name]);
        filename = fields(data);
        % check if active or passive file
        if filename{1}(1) == 'A'
            ki  = 9;
            ai = 12;
        elseif filename{1}(1) == 'P'
            ki  = 10;
            ai = 13;
        else
            error('shit')
        end
        
        knee = str2double(filename{1}(ki));
        ankle = str2double(filename{1}(ai));
        %trial = str2double(filename{1}(end));
        data = eval(['data.',filename{1}]);
        % truncate front and after 200 sec, normalize to ground
        if length(data.Trajectories.Labeled.Data(:,1,1)) == 10
            clear structure
            time_length = 239;
            ground = mean(data.RigidBodies.Positions(1,3,:),'omitnan');
            start = 1; %find(data.Trajectories.Labeled.Data(9,3,:)<(ground),1);
            structure(1,:,:) = mean(data.Trajectories.Labeled.Data([3,6],1:3,start:start+time_length),'omitnan'); % hip centerline
            structure(2,:,:) = mean(data.Trajectories.Labeled.Data([4,7],1:3,start:start+time_length),'omitnan'); % knee centerline
            structure(3,:,:) = mean(data.Trajectories.Labeled.Data([5,8],1:3,start:start+time_length),'omitnan'); % ankle centerline
            structure(5,:,:) = mean(data.Trajectories.Labeled.Data([9,10],1:3,start:start+time_length),'omitnan'); % toes centerline
            structure(6,:,:) = data.Trajectories.Labeled.Data(1,1:3,start:start+time_length); % actuator bottom
            structure(7,:,:) = data.Trajectories.Labeled.Data(2,1:3,start:start+time_length); % actuator top
            structure(:,3,:) = structure(:,3,:) - ground;
            structure = permute(structure,[3,2,1]);
            if std(structure(100:end,2,3)) > 5
                if isempty(Data{knee,ankle}) % indexes of different test conditions
                    iter = iter+1;
                    ind(iter) = knee + (ankle-1)*6;
                    trial = 1;
                else
                    trial = length(Data{knee,ankle}.Trial)+1;
                    if trial ==4
                        p=1;
                    end
                end
                Data{knee,ankle}.Trial{trial} = structure; %permute(structure,[3,2,1]);
                Data{knee,ankle}.knee = knee;
                Data{knee,ankle}.ankle = ankle;
                Data{knee,ankle}.start(trial) = start;
            end
        end
    end

    %% Get Angles
    % hip angle 
    for i = 1:length(ind) % for each condition
        for j = 1:length(Data{ind(i)}.Trial) % for each trial
            for k = 1:time_length % for each timestep
                % thigh LoA
                X_thigh = [Data{ind(i)}.Trial{j}(k,2:3,1),0] - [Data{ind(i)}.Trial{j}(k,2:3,2),0]; % thigh - shank
                Xn_thigh = X_thigh/dist(Data{ind(i)}.Trial{j}(k,2:3,1) , Data{ind(i)}.Trial{j}(k,2:3,2));

                % Hip Angle 
                th_hip = acos(dot([0,1,0],Xn_thigh))*sign(Data{ind(i)}.Trial{j}(k,2,2) - Data{ind(i)}.Trial{j}(k,2,1));

                % shank LoA
                X_shank = [Data{ind(i)}.Trial{j}(k,2:3,2),0] - [Data{ind(i)}.Trial{j}(k,2:3,3),0];
                Xn_shank = X_shank/(dist(Data{ind(i)}.Trial{j}(k,2:3,2),Data{ind(i)}.Trial{j}(k,2:3,3)));

                % Knee Angle
                th_knee = acos(dot(Xn_thigh,Xn_shank))*sign(Xn_shank(1) - Xn_thigh(1));

                % Toes LoA
                X_toes = [Data{ind(i)}.Trial{j}(k,2:3,3),0] - [Data{ind(i)}.Trial{j}(k,2:3,5),0];
                Xn_toes = X_toes/dist(Data{ind(i)}.Trial{j}(k,2:3,3), Data{ind(i)}.Trial{j}(k,2:3,5));

                % Toes Angle
                th_toes = acos(dot(Xn_shank,Xn_toes))*sign(Xn_shank(1) - Xn_toes(1));

                % Ankle Angle
                th_foot = atan(63/173);
                th_ankle = th_toes -pi/2 - th_foot;

                % Foot LoA
                Xn_foot = [cos(th_foot)*Xn_toes(1) - sin(th_foot)*Xn_toes(2), sin(th_foot)*Xn_toes(1) + cos(th_foot)*Xn_toes(2),0];
                X_foot = Xn_foot*173;

                % Ankle LoA
                Xn_ankle = [Xn_foot(2),-Xn_foot(1),0];
                X_ankle = Xn_ankle*63;

                % Heel Location
                q_heel1 = [Data{ind(i)}.Trial{j}(k,2:3,5),0] + X_foot;
                q_heel2 = [Data{ind(i)}.Trial{j}(k,2:3,3),0] - X_ankle;

                Data{ind(i)}.Trial{j}(k,:,4) = [0,mean([q_heel1(1),q_heel2(1)],'omitnan'),mean([q_heel1(2),q_heel2(2)],'omitnan')];

                % Make Angles Matrix
                Angles{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Trial{j}(k,:) = [th_hip,th_knee,th_ankle];
            end
                if i ==5 && j==4
                    p=1;
                end
        end
    end

    %% Determine Start Time for Trial (and mean)
    for i = 1:length(ind)
        for j = 1:length(Data{ind(i)}.Trial)
            % Use rate of knee flexion (ie. knee buckle) as indicator of TO
            st_ind = find((Angles{ind(i)}.Trial{j}(2:end,2) - Angles{ind(i)}.Trial{j}(1:end-1,2)) >= deg2rad(2),1);
            if isempty(st_ind)
                st_ind = find((Angles{ind(i)}.Trial{j}(2:end,2) - Angles{ind(i)}.Trial{j}(1:end-1,2)) >= deg2rad(1),1);
            end
            Data{ind(i)}.Trial{j} = Data{ind(i)}.Trial{j}(st_ind:st_ind+100,:,:);
            Angles{ind(i)}.Trial{j} = Angles{ind(i)}.Trial{j}(st_ind:st_ind+100,:);
        end
        AngleMat = [Angles{ind(i)}.Trial{:}];
        AvgHip = mean(AngleMat(:,(1:length(Angles{ind(i)}.Trial))*3 - 2),2,'omitnan');
        AvgKnee = mean(AngleMat(:,(1:length(Angles{ind(i)}.Trial))*3 - 1),2,'omitnan');
        AvgAnkle = mean(AngleMat(:,(1:length(Angles{ind(i)}.Trial))*3),2,'omitnan');
        Angles{ind(i)}.Mean = [AvgHip,AvgKnee,AvgAnkle];
                
        % Get average Positions
        DataMat = [Data{ind(i)}.Trial{:}];
        AvgMarkerx = mean(DataMat(:,(1:length(Data{ind(i)}.Trial))*3 - 2,:),2,'omitnan');
        AvgMarkery = mean(DataMat(:,(1:length(Data{ind(i)}.Trial))*3 - 1,:),2,'omitnan');
        AvgMarkerz = mean(DataMat(:,(1:length(Data{ind(i)}.Trial))*3 ,:),2,'omitnan');
        Data{ind(i)}.Mean =  [AvgMarkerx,AvgMarkery,AvgMarkerz];
    end

%     %% Determine End time for trial
%     for i = 1:length(ind)
%         et_ind = find(islocalmin(Angles{ind(i)}.Mean(:,2)),1) + 5;
%         Angles{ind(i)}.Mean = Angles{ind(i)}.Mean(1:et_ind,:);
%         for j = 1:length(Data{ind(i)}.Trial)
%             Data{ind(i)}.Trial{j} = Data{ind(i)}.Trial{j}(1:et_ind,:,:);
%             Angles{ind(i)}.Trial{j} = Angles{ind(i)}.Trial{j}(1:et_ind,:);
%         end
%     end

%% Determine End time for trial
    iter = 0;
    for i = 1:length(ind)
        filt_ang_mean = filter(0.125*ones(1,8),0.5*ones(1,2),Angles{ind(i)}.Mean(:,:));
        et = find(islocalmin(filt_ang_mean(11:end,2),'MaxNumExtrema', 1),1)+10; % for plotting purposes
        for j = 1:length(Data{ind(i)}.Trial)
            iter = iter+1;
            filt_ang = filter(0.125*ones(1,8),0.5*ones(1,2),Angles{ind(i)}.Trial{j}(:,:));
            Data{ind(i)}.et_ind(j) = find(islocalmin(filt_ang(11:end,2),'MaxNumExtrema', 1),1)+8;
            et(end+1) = Data{ind(i)}.et_ind(j);
        end 
        et_ind = max(et);
        for j = 1:length(Data{ind(i)}.Trial)
            Data{ind(i)}.Trial{j} = Data{ind(i)}.Trial{j}(1:et_ind,:,:);
            Angles{ind(i)}.Trial{j} = Angles{ind(i)}.Trial{j}(1:et_ind,:);
        end
        Angles{ind(i)}.Mean = Angles{ind(i)}.Mean(1:et_ind,:);
        Data{ind(i)}.Mean = Data{ind(i)}.Mean(1:et_ind,:,:);
    end

    %% Calculate Metrics

    for i =1:length(ind)
        for j = 1:length(Data{ind(i)}.Trial)
            % Peak Knee Flexion
            [val,val_ind] = max(rad2deg(Angles{ind(i)}.Trial{j}(:,2)));
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Trial{j}.PeakKneeFlex = val;
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Trial{j}.PeakKneeFlex_ind = val_ind;

            % Peak Dorsiflexion
            [val,val_ind] = max(rad2deg(Angles{ind(i)}.Trial{j}(:,3)));
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Trial{j}.PeakDorsiFlex = val;
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Trial{j}.PeakDorsiFlex_ind = val_ind;

            % Swing Duration
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Trial{j}.SwingDur = find(islocalmin(Angles{ind(i)}.Trial{j}(:,2)),1);

            % MTC
            val_ind = find(islocalmin(Data{ind(i)}.Trial{j}(:,3,5),'MaxNumExtrema',2));
            val = Data{ind(i)}.Trial{j}(val_ind,3,5);
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Trial{j}.MTC = val;
            Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Trial{j}.MTC_ind = val_ind;
        end
        % Peak Knee Flexion
        [val,val_ind] = max(rad2deg(Angles{ind(i)}.Mean(:,2)));
        Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Mean.PeakKneeFlex = val;
        Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Mean.PeakKneeFlex_ind = val_ind;

        % Peak Dorsiflexion
        [val,val_ind] = max(rad2deg(Angles{ind(i)}.Mean(:,3)));
        Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Mean.PeakDorsiFlex = val;
        Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Mean.PeakDorsiFlex_ind = val_ind;

        % Swing Duration
        Metrics{Data{ind(i)}.knee,Data{ind(i)}.ankle}.Mean.SwingDur = Data{ind(i)}.et_ind(j); %find(islocalmin(Angles{ind(i)}.Mean(:,2)),1);

    end


end

function [Metrics,Angles,Data,ind] = process_hood(folder)
data = load(folder);
filename = fields(data);
subject = eval(['data.',filename{1}]);
 
plot_all = 0;
plot_mom = 0;
plot_pos = 0;
l_leg = subject.info.height/100*0.45; % leg length in m
% loop over different speeds
speeds = fields(subject.data);
n = length(speeds);
for i = 1:n
    % generalize speed
    data_string = ['subject.data.',speeds{i}];
    
    % get speed as number
    speed_str = [speeds{i}(end-2),'.',speeds{i}(end)];
    speed(i) = str2num(speed_str);
    
    


    % Get distance travelled in an ipsi gait cycle
        % max ipsilateral hip angle in stance, degrees
        max_hip_ips = max(eval([data_string,'.ipsilateral.hip.position.avg(1:601)'])); 
        % min ipsilateral hip angle in stance, degrees
        min_hip_ips = min(eval([data_string,'.ipsilateral.hip.position.avg(1:601)'])); 
        % max contralateral hip angle while ipsi in swing in degrees
        max_hip_con_1090 = max(eval([data_string,'.contralateral.hip.position.avg(101:501)'])); 
        % min contralateral hip angle while ipsi in swing in degrees
        min_hip_con_1090 = min(eval([data_string,'.contralateral.hip.position.avg(101:501)'])); 
        % distance travelled during ipsilateral gait cycle
        l_stride_ips = (sin(deg2rad(max_hip_ips))-sin(deg2rad(min_hip_ips)))*l_leg + (sin(deg2rad(max_hip_con_1090))-sin(deg2rad(min_hip_con_1090)))*l_leg;


    % Get distance travelled in a contralateral gait cycle
        % max contralateral hip angle in stance degrees
        max_hip_con = max(eval([data_string,'.contralateral.hip.position.avg(1:601)'])); 
        % min contralateral hip angle in stance degrees
        min_hip_con = min(eval([data_string,'.contralateral.hip.position.avg(1:601)'])); 
        % max ipsilateral hip angle while contra in swing in degrees
        max_hip_ips_1090 = max(eval([data_string,'.ipsilateral.hip.position.avg(101:501)'])); 
        % min ipsi hip angle while contra in swing in degrees
        min_hip_ips_1090 = min(eval([data_string,'.ipsilateral.hip.position.avg(101:501)'])); 
        % distance travelled during ipsilateral gait cycle
        l_stride_con = (sin(deg2rad(max_hip_con))-sin(deg2rad(min_hip_con)))*l_leg + (sin(deg2rad(max_hip_ips_1090))-sin(deg2rad(min_hip_ips_1090)))*l_leg;

    % Get the average stride length
        l_stride_avg(i) = (l_stride_con+l_stride_ips)/2;

    % Get the average gait cycle length:
        cycle_time(i) = l_stride_avg(i)/speed(i); % sec

    % Get the average timestep
        time_step(i) = cycle_time(i)/1000; % sec
        
        diff(i,:) = eval([data_string,'.ipsilateral.knee.position.avg(2:end) - ',data_string,'.ipsilateral.knee.position.avg(1:end-1)']);
        
    % Get TO time
    TO_time(i)= find(min(eval([data_string,'.ipsilateral.hip.position.avg']))==eval([data_string,'.ipsilateral.hip.position.avg'])); %find(diff(i,:)<-0.14,1);
    
    % Get the joint angles at TO
        TO_angles(i,:) = [eval([data_string,'.ipsilateral.hip.position.avg(TO_time(i))']),eval([data_string,'.ipsilateral.knee.position.avg(TO_time(i))']),eval([data_string,'.ipsilateral.ankle.position.avg(TO_time(i))'])];
     
    % Get the hip speed at TO
        TO_hip_speed(i) = (eval([data_string,'.ipsilateral.hip.position.avg(TO_time(i)+1)']) - eval([data_string,'.ipsilateral.hip.position.avg(TO_time(i))']))/time_step(i);
        
    % Save how I need for this
        start = TO_time(i) - 110;
        duration = time_step(i)*(1001-start);
        numsteps = floor(duration*100);
        
        Angles{i}.Mean(:,1) = interp1((1:1001-start)*time_step(i)*100,deg2rad(eval([data_string,'.ipsilateral.hip.position.avg(start+1:end)'])),1:numsteps);
        Angles{i}.Mean(:,2) = -interp1((1:1001-start)*time_step(i)*100,deg2rad(eval([data_string,'.ipsilateral.knee.position.avg(start+1:end)'])),1:numsteps);
        Angles{i}.Mean(:,3) = -interp1((1:1001-start)*time_step(i)*100,deg2rad(eval([data_string,'.ipsilateral.ankle.position.avg(start+1:end)'])),1:numsteps);

        
end
Metrics = [];
Data = [];
ind = 1:length(Angles);
end

function [Angles] = process_sim
load('PassiveSimulationData.mat');

finish = floor(SimData(end,1)*100);

off = 9;

Angles = -deg2rad([[ones(off,1),ones(off,1),ones(off,1)].*SimData(1,2:end);interp1(SimData(:,1),SimData(:,2:end),(1:finish)/100)]);

Angles(:,2) = -Angles(:,2);

end

function animate(Data,condition,trial,ind)
    fig = figure;
    for i = 1:length(Data{ind(condition(1))}.Trial{trial(1)}(:,1,1))
        clf(fig);
        
        for j = 1:length(condition)
            if Data{ind(condition(j))}.et_ind(trial(j)) < i
                mark = 'y-o';
            else
                mark = '-o';
            end
            plot(permute(Data{ind(condition(j))}.Trial{trial(j)}(i,2,1:5),[1,3,2]),permute(Data{ind(condition(j))}.Trial{trial(j)}(i,3,1:5),[1,3,2]),mark) 
            hold on;
            plot(permute(Data{ind(condition(j))}.Trial{trial(j)}(i,2,6:7),[1,3,2]),permute(Data{ind(condition(j))}.Trial{trial(j)}(i,3,6:7),[1,3,2]),'r','Linewidth',2)
            
            ank = Data{ind(condition(j))}.ankle;
            kne = Data{ind(condition(j))}.knee;
            entries{j} = ['Knee ',num2str(kne),', Ankle ', num2str(ank),', Trial ',num2str(trial(j))];
        %     axis([0,900,-100,0])
            
        end
        plot([0,1000],[0,0],'k') % ground
        
        xlabel('x mm')
        ylabel('y mm')
        axis([000,1000,-150,850])
        title([num2str(i*10),' ms'])
        axis square
        legend(entries,'Location','northeast')
        drawnow
    end
    pause
    close(fig)
end

function plotting_angles(Angles,Data,ind)
    marker = {'b-','r--','g.','c.-','m.--'};
    marker2 = {'bx','rx','gx','cx','mx'};
    for i = 1:length(ind) % loop over conditions
        figure; clf;
        %title(['Passive Condition; Knee ',num2str(Data{ind(i)}.knee),'; Ankle ',num2str(Data{ind(i)}.ankle)])
        for j = 1:length(Data{ind(i)}.Trial) % loop over trials
            subplot(3,1,1) % hip
            plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,rad2deg(Angles{ind(i)}.Trial{j}(:,1)),marker{j})
            hold on
            plot((Data{ind(i)}.et_ind(j)-1)/100, rad2deg(Angles{ind(i)}.Trial{j}(Data{ind(i)}.et_ind(j),1)),marker2{j})
            subplot(3,1,2) % knee
            plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,rad2deg(Angles{ind(i)}.Trial{j}(:,2)),marker{j})
            hold on
            plot((Data{ind(i)}.et_ind(j)-1)/100, rad2deg(Angles{ind(i)}.Trial{j}(Data{ind(i)}.et_ind(j),2)),marker2{j})
            subplot(3,1,3) % ankle - or TC
            plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,rad2deg(Angles{ind(i)}.Trial{j}(:,3)),marker{j})
%             plot((0:length(Data{ind(i)}.Trial{j})-1)/100,rad2deg(Data{ind(i)}.Trial{j}(:,3,5)),marker{j})
            hold on
            plot((Data{ind(i)}.et_ind(j)-1)/100, rad2deg(Angles{ind(i)}.Trial{j}(Data{ind(i)}.et_ind(j),3)),marker2{j})
        end
        subplot(3,1,1)
        xlabel('time (sec)')
        ylabel('hip angle (deg)')
        title(['Knee ',num2str(Data{ind(i)}.knee),', Ankle ',num2str(Data{ind(i)}.ankle)])
        plot((0:length(Angles{ind(i)}.Mean)-1)/100,rad2deg(Angles{ind(i)}.Mean(:,1)),'k','Linewidth',2)
        
        subplot(3,1,2)
        xlabel('time (sec)')
        ylabel('knee angle (deg)')
%         title('Knee Angle')
        plot((0:length(Angles{ind(i)}.Mean)-1)/100,rad2deg(Angles{ind(i)}.Mean(:,2)),'k','Linewidth',2)
        
        subplot(3,1,3)
        xlabel('time (sec)')
        ylabel('ankle angle (deg)')
%         title('Ankle Angle')
        plot((0:length(Angles{ind(i)}.Mean)-1)/100,rad2deg(Angles{ind(i)}.Mean(:,3)),'k','Linewidth',2)
%         ylabel('Clearance (mm)')
%         title('Toe Clearance')

        trial_names = {'Trial 1','Trial 2','Trial 3','Trial 4','Trial 5'};
%         legend(trial_names{1:length(Angles{ind(i)}.Trial)},'Mean')
        set(gcf,'Position',[1000 100 560,800])
    end
end

function ani_comp(Data,Names,LineT,LineW,active)
fig = figure;
    for i = 1:length(Data)
        duration(i) = length(Data{i}(:,1,1));
    end
    for i = 1:max(duration)
        clf(fig);
        for j = 1:length(Data)
            if duration(j)>=i
                plot(permute(Data{j}(i,2,1:5),[1,3,2]),permute(Data{j}(i,3,1:5),[1,3,2]),LineT{j},'LineWidth',LineW(j)) 
                hold on;
            end
        end
        for j = 1:length(Data)
            if duration(j)>=i && active(j)
                    plot(permute(Data{j}(i,2,6:7),[1,3,2]),permute(Data{j}(i,3,6:7),[1,3,2]),'r','Linewidth',2)
            end
        end
        plot([0,1000],[0,0],'k') % ground
        xlabel('x mm')
        ylabel('y mm')
        axis([000,1000,-150,850])
        title([num2str(i*10),' ms'])
        axis square
        legend(Names,'Location','northeast')
        drawnow
    end

%     close(fig)
end

function angle_comp(Angles,Names,LineT,LineW,Title)
figure; clf;

% loop over conditions
for i = 1:length(Angles)
    subplot(3,1,1) % hip
    plot((0:length(Angles{i})-1)/100,rad2deg(Angles{i}(:,1)),LineT{i},'LineWidth',LineW(i))
    hold on
%     plot((Data{ind(i)}.et_ind(j)-1)/100, rad2deg(Angles{ind(i)}.Trial{j}(Data{ind(i)}.et_ind(j),1)),marker2{j})
    subplot(3,1,2) % knee
    plot((0:length(Angles{i})-1)/100,rad2deg(Angles{i}(:,2)),LineT{i},'LineWidth',LineW(i))
    hold on
%     plot((Data{ind(i)}.et_ind(j)-1)/100, rad2deg(Angles{ind(i)}.Trial{j}(Data{ind(i)}.et_ind(j),2)),marker2{j})
    subplot(3,1,3) % ankle - or TC
    plot((0:length(Angles{i})-1)/100,rad2deg(Angles{i}(:,3)),LineT{i},'LineWidth',LineW(i))
    hold on
%     plot((Data{ind(i)}.et_ind(j)-1)/100, rad2deg(Angles{ind(i)}.Trial{j}(Data{ind(i)}.et_ind(j),3)),marker2{j})
end
subplot(3,1,1)
xlabel('time (sec)')
ylabel('hip angle (deg)')
title(Title)
grid on;
grid minor;


subplot(3,1,2)
xlabel('time (sec)')
ylabel('knee angle (deg)')
legend(Names,'Position',[0.6713    0.5695    0.2351    0.0980])
grid on;
grid minor;
ylim([-20,80]);


subplot(3,1,3)
xlabel('time (sec)')
ylabel('ankle angle (deg)')
grid on;
grid minor;
ylim([-10,30])

% set(gcf,'Position',[1000 100 560,800])
set(gcf,'Position',[1000 100 464,662])
    

end

function [Forces, Moments,ma] = calc_forces(Data,ind)
%% Fill in Spring values
stiff = 4.100; % N/mm
F0 = 24.38; % N

% the length the spring was set to for each test condition
Sn(	2	,	2	) = 	25.4837	;
Sn(	2	,	4	) = 	31.3844	;
Sn(	2	,	6	) = 	37.2336	;
Sn(	4	,	2	) = 	37.9258	;
Sn(	4	,	4	) = 	43.35	;
Sn(	4	,	6	) = 	43.35	;
Sn(	6	,	2	) = 	43.35	;
Sn(	6	,	4	) = 	43.35	;
Sn(	6	,	6	) = 	43.35	;

%% Preallocate things
Forces = cell(size(Data));
Moments = cell(size(Data));
ma.k = cell(size(Data));
ma.a = cell(size(Data));

%% Calculate the forces and lengths and moments
for i = 1:length(ind)
    for j = 1:length(Data{ind(i)}.Trial)
        for k = 1:length(Data{ind(i)}.Trial{j})
            % Calculate Spring lengths
            l_sys{i}.Trial{j}(k) =  dist(Data{ind(i)}.Trial{j}(k,:,6),Data{ind(i)}.Trial{j}(k,:,7));
            l_0 = l_sys{i}.Trial{j}(1) - Sn(ind(i));
            l_s{i}.Trial{j}(k) = l_sys{i}.Trial{j}(k) - l_0;
            
            % Calculate spring force
            if l_s{i}.Trial{j}(k)>0
               Forces{ind(i)}.Trial{j}(k) = stiff*l_s{i}.Trial{j}(k) + F0;
            else
               Forces{ind(i)}.Trial{j}(k) =0;
            end
            
%             % calculate spring LoA
%             LoA = Data{ind(i)}.Trial{j}(k,:,6) - Data{ind(i)}.Trial{j}(k,:,7);
%             LoAn = LoA/dist([0,0,0],LoA);
            
            ma.k{ind(i)}.Trial{j}(k) = perpendicular([Data{ind(i)}.Trial{j}(k,2:3,6),0],[Data{ind(i)}.Trial{j}(k,2:3,7),0],[Data{ind(i)}.Trial{j}(k,2:3,2),0]);
            ma.a{ind(i)}.Trial{j}(k) = -perpendicular([Data{ind(i)}.Trial{j}(k,2:3,6),0],[Data{ind(i)}.Trial{j}(k,2:3,7),0],[Data{ind(i)}.Trial{j}(k,2:3,3),0]);

            Moments{ind(i)}.Trial{j}.Knee(k) = ma.k{ind(i)}.Trial{j}(k)*Forces{ind(i)}.Trial{j}(k)/1000;
            Moments{ind(i)}.Trial{j}.Ankle(k) = ma.a{ind(i)}.Trial{j}(k)*Forces{ind(i)}.Trial{j}(k)/1000;
        end        
    end
end
end

function plotting_moments(Angles,Data,ind,Moments,Forces,MA)
marker = {'-','--','.-','-.','.--'};
color = {'b','r','g','m','c'};
    for i = 1:length(ind) % loop over conditions
        figure; clf;
        for j = 1:length(Angles{ind(i)}.Trial) % loop over trials
            
            subplot(3,1,1) % Angles
            plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,rad2deg(Angles{ind(i)}.Trial{j}(:,2)),[marker{1},color{j}])
            hold on
            plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,rad2deg(Angles{ind(i)}.Trial{j}(:,3)),[marker{2},color{j}])
            
            subplot(3,1,2) % Moments
            plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,Moments{ind(i)}.Trial{j}.Knee,[marker{1},color{j}])
            hold on
            plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,Moments{ind(i)}.Trial{j}.Ankle,[marker{2},color{j}])

            subplot(3,1,3) % Forces and MAs
%             yyaxis right
            plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,Forces{ind(i)}.Trial{j},[marker{3},color{j}])
            hold on
%             yyaxis left
%             plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,MA.k{ind(i)}.Trial{j},[marker{1},color{j}])
%             plot((0:length(Angles{ind(i)}.Trial{j})-1)/100,MA.a{ind(i)}.Trial{j},[marker{2},color{j}])
        end
        subplot(3,1,1)
        xlabel('time (sec)')
        ylabel('joint angle (deg)')
        title(['Actuator Moments. Condition: Knee ',num2str(Data{ind(i)}.knee),', Ankle ',num2str(Data{ind(i)}.ankle)])
        grid on
        grid minor
        x = get(gca,'Xlim');
        plot(x,[0,0],'k')
        set(gca,'Xlim',x)
        legend('knee','ankle')
        
        subplot(3,1,2)
        xlabel('time (sec)')
        ylabel('joint moments (Nm)')
        grid on
        grid minor
        x = get(gca,'Xlim');
        plot(x,[0,0],'k')
        set(gca,'Xlim',x)
        legend('knee','ankle')
        
        subplot(3,1,3)
        xlabel('time (sec)')
%         yyaxis left
%         ylabel('moment arm (mm)')
        x = get(gca,'Xlim');
        plot(x,[0,0],'k')
%          legend('knee arm','ankle arm')
%         yyaxis right
%         legend('Force')
        ylabel('actuator force (N)')
        grid on
        grid minor
        set(gca,'Xlim',x)

        set(gcf,'Position',[1000 100 560,800])
    end

end

function plotting_metrics(Metrics,Names)
kf = figure; clf; hold on
df = figure; clf; hold on
sf = figure; clf; hold on
mf = figure; clf; hold on

leng = length(Metrics(1,1,:));

if  leng == 3
    cond = {'K2, A4','K4, A4','K6, A4'};
    cond2 = {'error','error2','error3'};
    loop = 4;
else
    cond = {'K2, A2','K2, A4','K2, A6'...
            'K4, A2','K4, A4','K4, A6'...
            'K6, A2','K6, A4','K6, A6'};
    cond2 = {'K1, A2','K1, A4','K1, A6'...
            'K4, A2','K4, A4','K4, A6'...
            'K6, A2','K6, A4','K6, A6'};
    loop = (1:3)*2;
end
iter = 0;
% put all values into single matrix

for i = (1:3)*2 % loop over knees
    KneFlex = cell(1,leng);
    DorsiFlex = cell(1,leng);
    SwingDur = cell(1,leng);
    MTC = cell(1,leng);
    organizer = cell(1,leng);
    aplabel = cell(1,leng);
    for j = loop % loop over ankles
        iter = iter+1;
        for k = 1:length(Metrics(1,1,:)) % loop over conditions
            if ~isempty(Metrics{i,j,k})
                KneFlex{k}(1,end+1:end+length(Metrics{i,j,k}.PeakKneeFlex.data)) = Metrics{i,j,k}.PeakKneeFlex.data;
                DorsiFlex{k}(1,end+1:end+length(Metrics{i,j,k}.PeakDorsiFlex.data)) = Metrics{i,j,k}.PeakDorsiFlex.data;
                SwingDur{k}(1,end+1:end+length(Metrics{i,j,k}.SwingDur.data)) = Metrics{i,j,k}.SwingDur.data;
                MTC{k}(1,end+1:end+length(Metrics{i,j,k}.MTC.data)) = Metrics{i,j,k}.MTC.data;
                organizer{k}(1,end+1:end+length(Metrics{i,j,k}.PeakKneeFlex.data)) = cond(iter);
                aplabel{k}(1,end+1:end+length(Metrics{i,j,k}.PeakKneeFlex.data)) = Names(k);
                matsize(k) = length(KneFlex{k});
            elseif ~isempty(Metrics{i-1,j,k})
                KneFlex{k}(1,end+1:end+length(Metrics{i-1,j,k}.PeakKneeFlex.data)) = Metrics{i-1,j,k}.PeakKneeFlex.data;
                DorsiFlex{k}(1,end+1:end+length(Metrics{i-1,j,k}.PeakDorsiFlex.data)) = Metrics{i-1,j,k}.PeakDorsiFlex.data;
                SwingDur{k}(1,end+1:end+length(Metrics{i-1,j,k}.SwingDur.data)) = Metrics{i-1,j,k}.SwingDur.data;
                MTC{k}(1,end+1:end+length(Metrics{i-1,j,k}.MTC.data)) = Metrics{i-1,j,k}.MTC.data;
                organizer{k}(1,end+1:end+length(Metrics{i-1,j,k}.PeakKneeFlex.data)) = cond2(iter);
                aplabel{k}(1,end+1:end+length(Metrics{i-1,j,k}.PeakKneeFlex.data)) = Names(k);
                matsize(k) = length(KneFlex{k});
            end
        end
    end
%     % Pad the smaller matrices
%     [longest,long_ind] = max(matsize);
%     for k = 1:length(Metrics(1,1,:))
%         if k ~=long_ind
%             KneFlex{k}(end+1:longest,1) = -1;
%             DorsiFlex{k}(end+1:longest,1) = -1;
%             SwingDur{k}(end+1:longest,1) = -1;
%             MTC{k}(end+1:longest,1) = -1;
%             organizer{k}(end+1:longest,1) = cond(iter);
%         end
%     end

%     aplabel = repmat(Names,longest,1);
    if leng ~= 3
        order = {[cond{iter-2},',',Names{1}],[cond2{iter-2},',',Names{2}],[cond{iter-1},',',Names{1}],[cond2{iter-1},',',Names{2}],[cond{iter},',',Names{1}],[cond2{iter},',',Names{2}]};
    else
        order = cond;
    end
    % Plot Peak Knee Flex
    figure(kf)
    if leng ~= 3
        subplot(3,1,i/2)
    end
    boxplot([KneFlex{:}],{[organizer{:}],[aplabel{:}]},'factorgap',10,'grouporder',order)
    y = get(gca,'ylim');
    set(gca,'ylim',[0,y(2)])
    ylabel('knee angle (deg)')
    grid on
    grid minor
    if i == 2
        title('Peak Knee Flexion')
    end
    set(gcf,'Position',[589.0000 91.5660 464.0000 650])

    % Plot Peak Dorsi Flex
    figure(df)
    subplot(3,1,i/2)
    boxplot([DorsiFlex{:}],{[organizer{:}],[aplabel{:}]},'factorgap',10,'grouporder',order)
    y = get(gca,'ylim');
    set(gca,'ylim',[0,y(2)])
    ylabel('ankle angle (deg)')
    grid on
    grid minor
    if i == 2
        title('Peak Dorsiflexion')
    end
    set(gcf,'Position',[589.0000 91.5660 464.0000 650])

    % Plot Swing Duration
    figure(sf)
    subplot(3,1,i/2)
    boxplot([SwingDur{:}]*10,{[organizer{:}],[aplabel{:}]},'factorgap',10,'grouporder',order)
    y = get(gca,'ylim');
    set(gca,'ylim',[0,y(2)])
    ylabel('duration (ms)')
    grid on
    grid minor
    if i == 2
        title('Swing Phase Duration')
    end
    set(gcf,'Position',[589.0000 91.5660 464.0000 650])

    % Plot MTC
    figure(mf)
    subplot(3,1,i/2)
    boxplot([MTC{:}],{[organizer{:}],[aplabel{:}]},'factorgap',10,'grouporder',order)
    y = get(gca,'ylim');
%     set(gca,'ylim',[0,y(2)])
    ylabel('clearance (mm)')
    grid on
    grid minor
    if i == 2
        title('Minimum Toe Clearance')
    end
    set(gcf,'Position',[589.0000 91.5660 464.0000 650])

    
    
end

% set(gcf,'Position',[589.0000 91.5660 560.0000 784.4340])

% % Pad the smaller matrices
% [longest,long_ind] = max(matsize);
% for k = 1:length(Metrics(1,1,:))
%     if k ~=long_ind
%         KneFlex{k}(end+1:longest) = 0;
%         organizer{k}(end+1:longest) = 0;
%     end
% end
% 
% % Actually plot that box
% aplabel = repmat('ap',longest,1);
% subplot(3,1,1)
% boxplot([KneFlex{1};KneFlex{2}],{[organizer{1};organizer{2}],aplabel(:)},'factorgap',10)
end


function plotting_metrics_together(Metrics,Names)

% preallocate
malabel = cell(0);
aplabel = cell(0);
PKF = [];
PDF = [];
MTC = [];
SD = [];
order = cell(0);
maorder = cell(0);
biggroup = cell(0);

sizes = size(Metrics);

% gather data into single matrix with labels
for kne = 1:sizes(1)
    for ank = 1:sizes(2)
        for cond = 1:sizes(3)
            % check to see if data exists
            if ~isempty(Metrics{kne,ank,cond})
                num = length(Metrics{kne,ank,cond}.PeakKneeFlex.data);
                PKF(end+1:end+num) = Metrics{kne,ank,cond}.PeakKneeFlex.data;
                PDF(end+1:end+num) = Metrics{kne,ank,cond}.PeakDorsiFlex.data;
                MTC(end+1:end+num) = Metrics{kne,ank,cond}.MTC.data;
                SD(end+1:end+num) = Metrics{kne,ank,cond}.SwingDur.data;
                
                malabel(end+1:end+num) = {['K',num2str(kne),' A',num2str(ank)]};
                maorder(end+1:end+num) = {['K',num2str(max([kne,2])),' A',num2str(ank)]};
                aplabel(end+1:end+num) = Names(cond);
                biggroup(end+1:end+num) = {num2str(max([2,kne]))};
                order(end+1) = {[maorder{end},',',aplabel{end}]};
            end
        end        
    end    
end
    
%order(1:6) = order([1,4,2,5,3,6]);

% Make boxplots
figure
% subplot(1,2,1)
boxplot(PKF,{biggroup,maorder,aplabel},'factorgap',[10,10],'Labels',{malabel,aplabel}...
    ,'Orientation','vertical','BoxStyle','outline','FactorSeparator',1)
    x = get(gca,'xlim');
    set(gca,'xlim',[0,x(2)])
    y = get(gca,'ylim');
    set(gca,'ylim',[0,y(2)])
    ylabel('knee angle (deg)')
    grid on
    grid minor
    title('Peak Knee Flexion')
    set(gcf,'Position',[-1700.0    0330.6     900    225]);
    %set(gca,'Units','centimeters');set(gca,'Position',[ 3.6673    2.4909    7.0299   18.4551])
    

figure
% subplot(1,4,2)
boxplot(PDF,{biggroup,maorder,aplabel},'factorgap',[10,10],'Labels',{malabel,aplabel}...
    ,'Orientation','vertical','BoxStyle','outline','FactorSeparator',1)
    x = get(gca,'xlim');
    set(gca,'xlim',[0,x(2)])
    y = get(gca,'ylim');
    set(gca,'ylim',[0,y(2)])
    ylabel('ankle angle (deg)')
    grid on
    grid minor
    title('Peak Dorsiflexion')
%     set(gca,'YTickLabel',{''})
    set(gcf,'Position',[-1700.0    0330.6     900    225]);

figure
% subplot(1,4,3)
boxplot(MTC,{biggroup,maorder,aplabel},'factorgap',[10,10],'Labels',{malabel,aplabel}...
    ,'Orientation','vertical','BoxStyle','outline','FactorSeparator',1)
    ylabel('clearance (mm)')
%     y = get(gca,'ylim');
%     set(gca,'ylim',[0,y(2)])
    grid on
    grid minor
    title('Minimum Toe Clearance')
%     set(gca,'YTickLabel',{''})
    set(gcf,'Position',[-1700.0    0330.6    900    225]);

figure
% subplot(1,4,4)
boxplot(SD*10,{biggroup,maorder,aplabel},'factorgap',[10,10],'Labels',{malabel,aplabel}...
    ,'Orientation','vertical','BoxStyle','outline','FactorSeparator',1)
    x = get(gca,'xlim');
    set(gca,'xlim',[0,x(2)])
    y = get(gca,'ylim');
    set(gca,'ylim',[0,y(2)])
    ylabel('duration (ms)')
    grid on
    grid minor
    title('Swing Phase Duration')
%     set(gca,'YTickLabel',{''})
    set(gcf,'Position',[-1700.0    0330.6     900    225]);
stop = 1;

end

function plotting_metrics2(Metrics,Names)
kf = figure; clf; hold on
df = figure; clf; hold on
sf = figure; clf; hold on
mf = figure; clf; hold on

leng = length(Metrics(1,1,:));

    cond = {'K2, A4','K4, A4','K6, A4'};
    cond2 = {'error','error2','error3'};
%     ap = {'Passive','Active','Active2'};
    loop = 4;

iter = 0;
% put all values into single matrix

for i = (1:3)*2 % loop over knees
    for j = loop % loop over ankles
        iter = iter+1;
        KneFlex = cell(1,leng);
        DorsiFlex = cell(1,leng);
        SwingDur = cell(1,leng);
        MTC = cell(1,leng);
        organizer = cell(1,leng);
        aplabel = cell(1,leng);
        for k = 1:length(Metrics(1,1,:)) % loop over conditions
            if ~isempty(Metrics{i,j,k})
                KneFlex{k}(1,end+1:end+length(Metrics{i,j,k}.PeakKneeFlex.data)) = Metrics{i,j,k}.PeakKneeFlex.data;
                DorsiFlex{k}(1,end+1:end+length(Metrics{i,j,k}.PeakDorsiFlex.data)) = Metrics{i,j,k}.PeakDorsiFlex.data;
                SwingDur{k}(1,end+1:end+length(Metrics{i,j,k}.SwingDur.data)) = Metrics{i,j,k}.SwingDur.data;
                MTC{k}(1,end+1:end+length(Metrics{i,j,k}.MTC.data)) = Metrics{i,j,k}.MTC.data;
                organizer{k}(1,end+1:end+length(Metrics{i,j,k}.PeakKneeFlex.data)) = cond(iter);
                aplabel{k}(1,end+1:end+length(Metrics{i,j,k}.PeakKneeFlex.data)) = Names(k);
                matsize(k) = length(KneFlex{k});
          end
        end



%     aplabel = repmat(Names,longest,1);
        order = {};
        for ii = iter
            for jj = 1:leng
                order{end+1} = [cond{ii},',',Names{jj}];
            end            
        end

        % Plot Peak Knee Flex
        figure(kf)
        subplot(3,1,i/2)
        KneFlex = [KneFlex{:}];
        organizer = [organizer{:}];
        aplabel = [aplabel{:}];
        boxplot(KneFlex(:),{organizer(:),aplabel(:)},'factorgap',10,'grouporder',order)
        y = get(gca,'ylim');
        set(gca,'ylim',[0,90])
        ylabel('knee angle (deg)')
        grid on
        grid minor
        if i == 2
            title('Peak Knee Flexion')
        end
        set(gcf,'Position',[589.0000 91.5660 560.0000 784.4340])

        % Plot Peak Dorsi Flex
        figure(df)
        subplot(3,1,i/2)
        DorsiFlex = [DorsiFlex{:}];
        boxplot(DorsiFlex(:),{organizer(:),aplabel(:)},'factorgap',10,'grouporder',order)
        y = get(gca,'ylim');
        set(gca,'ylim',[0,y(2)])
        ylabel('ankle angle (deg)')
        grid on
        grid minor
        if i == 2
            title('Peak Dorsiflexion')
        end
        set(gcf,'Position',[589.0000 91.5660 560.0000 784.4340])

        % Plot Swing Duration
        figure(sf)
        subplot(3,1,i/2)
        SwingDur = [SwingDur{:}];
        boxplot(SwingDur(:)*10,{organizer(:),aplabel(:)},'factorgap',10,'grouporder',order)
        y = get(gca,'ylim');
        set(gca,'ylim',[0,550])
        ylabel('duration (ms)')
        grid on
        grid minor
        if i == 2
            title('Swing Phase Duration')
        end
        set(gcf,'Position',[589.0000 91.5660 560.0000 784.4340])

        % Plot MTC
        figure(mf)
        subplot(3,1,i/2)
        MTC = [MTC{:}];
        boxplot(MTC(:),{organizer(:),aplabel(:)},'factorgap',10,'grouporder',order)
        y = get(gca,'ylim');
        set(gca,'ylim',[min(0,y(1)),y(2)])
        ylabel('clearance (mm)')
        grid on
        grid minor
        if i == 2
            title('Minimum Toe Clearance')
        end
        set(gcf,'Position',[589.0000 91.5660 560.0000 784.4340])

    end 
end
    


% set(gcf,'Position',[589.0000 91.5660 560.0000 784.4340])

% % Pad the smaller matrices
% [longest,long_ind] = max(matsize);
% for k = 1:length(Metrics(1,1,:))
%     if k ~=long_ind
%         KneFlex{k}(end+1:longest) = 0;
%         organizer{k}(end+1:longest) = 0;
%     end
% end
% 
% % Actually plot that box
% aplabel = repmat('ap',longest,1);
% subplot(3,1,1)
% boxplot([KneFlex{1};KneFlex{2}],{[organizer{1};organizer{2}],aplabel(:)},'factorgap',10)
end

function plotting_TC(Angles,Data,Metrics,ind)
marker = {'.','--','-';'.-','-.','o-'}';
color = {'b','r','g','c','m','k'};
for k = 1:length(Data(1,1,:))
    for i = 1:length(ind{k}) % loop over conditions
        figure(ind{k}(i)); %clf;
        [kne,ank] = ind2sub(size(Data),ind{k}(i));

        for j = 1:length(Data{kne,ank,k}.Trial) % loop over trials
            subplot(2,1,1) % angles
            plot((0:length(Angles{kne,ank,k}.Trial{j})-1)/100,rad2deg(Angles{kne,ank,k}.Trial{j}(:,1)),[marker{1,k},color{j}])
            hold on
            plot((0:length(Angles{kne,ank,k}.Trial{j})-1)/100,rad2deg(Angles{kne,ank,k}.Trial{j}(:,2)),[marker{2,k},color{j}])
            plot((0:length(Angles{kne,ank,k}.Trial{j})-1)/100,rad2deg(Angles{kne,ank,k}.Trial{j}(:,3)),[marker{3,k},color{j}])

            subplot(2,1,2) % TC
            plot((0:length(Data{kne,ank,k}.Trial{j})-1)/100,Data{kne,ank,k}.Trial{j}(:,3,5),[marker{3,k},color{j}])
            hold on
            plot((Metrics{kne,ank,k}.MTC.ind(j)-1)/100,Metrics{kne,ank,k}.MTC.data(j),['x',color{j}])
        end
        subplot(2,1,1)
        xlabel('time (sec)')
        ylabel('angle (deg)')
        title(['Knee ',num2str(Data{kne,ank,k}.knee),', Ankle ',num2str(Data{kne,ank,k}.ankle)])
        plot((0:length(Angles{kne,ank,k}.Mean)-1)/100,rad2deg(Angles{kne,ank,k}.Mean(:,1)),[marker{1,k},color{end}],'Linewidth',2)
        plot((0:length(Angles{kne,ank,k}.Mean)-1)/100,rad2deg(Angles{kne,ank,k}.Mean(:,2)),[marker{2,k},color{end}],'Linewidth',2)
        plot((0:length(Angles{kne,ank,k}.Mean)-1)/100,rad2deg(Angles{kne,ank,k}.Mean(:,3)),[marker{3,k},color{end}],'Linewidth',2)
        legend('Hip','Knee','Akle')

        subplot(2,1,2)
        xlabel('time (sec)')
        ylabel('Toe Clearance (mm)')
        plot((0:length(Data{kne,ank,k}.Mean)-1)/100,Data{kne,ank,k}.Mean(:,3,5),[marker{3,k},color{end}])
%         plot(Data{ind(i)}.Mean-1)/100,Metrics{ind(i)}.Mean.MTC.ind = 


%         trial_names = {'Trial 1','Trial 2','Trial 3','Trial 4','Trial 5'};
        set(gcf,'Position',[1000 100 560,800])
    end
end
end

function [tab] = output_metrics(Metrics)
iter = 1;
place = [];
mat=[];
num = length(Metrics(1,1,:));
% [PMetrics{k,a}.PeakKneeFlex.data',PMetrics{k,a}.PeakDorsiFlex.data',PMetrics{k,a}.MTC.data',PMetrics{k,a}.SwingDur.data']
for kne = 1:length(Metrics(:,1,1)) % loop over knee conditions
    for ank = 1:length(Metrics(1,:,1)) % loop over ankle conditions
        if ~isempty([Metrics{kne,ank,:}]) % if there are metrics
            for k = 1:length(Metrics(kne,ank,:)) % for active and passive
                if ~isempty([Metrics{kne,ank,k}])
                    tab_chunk = [Metrics{kne,ank,k}.PeakKneeFlex.data',Metrics{kne,ank,k}.PeakDorsiFlex.data',Metrics{kne,ank,k}.MTC.data',Metrics{kne,ank,k}.SwingDur.data'*10];
                    l = length(tab_chunk(:,1));
                    if length(place) == iter % if we've cycled through this conditon already
                        if pre< l % if we need to add any new rows
                            mat(end+1:end+(l-pre),:) = nan(l-pre,3+num*4);
                            mat(end-(l-pre)+1:end,1:3) = [ones(l-pre,2).*[kne,ank],[pre+1:l]'];
                            pre=l;
                        end
                        mat(end-pre+1:end-pre+l,3+(k-1)*4+(1:4)) = tab_chunk;
                    else % if we haven' been through before
                        mat(end+1:end+l,:) = [ones(l,2).*[kne,ank],[1:l]',nan(l,(k-1)*4),tab_chunk,nan(l,(num-k)*4)];
                        pre = l;
                    end
                    
                    place(iter) = length(mat(:,1));
                    pre = max(pre,l);
                end
            end
            % add means
            mat(end+1,:) = [kne,ank,nan,mean(mat(end-pre+1:end,4:end),1,'omitnan')];
        end
        iter = iter+1;
        clear pre
    end    
end
if length(mat)==15 % Low friction case
    Vars = {'knee','ankle','trial','KFAp','DFAp','MTCp','Durp','KFAa','DFAa','MTCa','Dura','KFAa2','DFAa2','MTCa2','Dura2'}; % repmat({'KFAa','DFAa','MTCa','Dura'},1,num)
    tab = array2table(mat','RowNames',Vars);
else % regular case
    Vars = {'knee','ankle','trial','KFAa','DFAa','MTCa','Dura','KFAp','DFAp','MTCp','Durp'}; % repmat({'KFAa','DFAa','MTCa','Dura'},1,num)
    tab = array2table(mat,'VariableNames',Vars);
end
    
% tab = array2table(mat,'VariableNames',Vars);
end
