%% Analyze EMG Data
% This code creates an average step of the EMG data by the data provided for each subject separately. The code can be run in whole, but there are some things that need to be adjusted depending on the loaded data. It cuts the EMG in steps and creates an average of it and saves it at the end at the defined location.
%
%% Load Data
% In this part of the Script the EMG data and the GRF data will be loaded. You have to choose what data of which subject you want to use. For each subject there is data from the Slow Walking (0.788m/s) as well as for Preferred Walking Speed (PWS) except for subject 3. Here, during the experiment something went wrong and the data was not usable.

clear all;
close all;

%
% Subject1:
% SlowWalking:

%% EMG DATA
% WithoutExo = readmatrix("Sub1_SlowWalking_EMG_NE.csv");
% WithoutControl = readmatrix("Sub1_SlowWalking_EMG_NC.csv");
% RL_control = readmatrix("Sub1_SlowWalking_EMG_RL.csv");
% emp_control = readmatrix("Sub1_SlowWalking_EMG_Emp.csv");

%% GRF DATA
% Grf_withoutControl = load("Sub1_SlowWalking_GRF_NC.mat","GRF").GRF;
% grf_withoutControl_time = load("Sub1_SlowWalking_GRF_NC.mat","time").time;
 
% Grf_withoutExo = load("Sub1_SlowWalking_GRF_NE.mat","GRF").GRF;
% grf_withoutExo_time = load("Sub1_SlowWalking_GRF_NE.mat","time").time;
 
% Grf_rlControl = load("Sub1_SlowWalking_GRF_RL.mat","GRF").GRF;
% grf_rlControl_time = load("Sub1_SlowWalking_GRF_RL.mat","time").time;
 
% Grf_empDeveloped = load("Sub1_SlowWalking_GRF_Emp.mat","GRF").GRF;
% grf_empDeveloped_time = load("Sub1_SlowWalking_GRF_Emp.mat","time").time;

% PWS:

%% EMG DATA
% WithoutExo = readmatrix("Sub1_PWS_EMG_NE.csv");
% WithoutControl = readmatrix("Sub1_PWS_EMG_NC.csv");
% RL_control = readmatrix("Sub1_PWS_EMG_RL.csv");
% emp_control = readmatrix("Sub1_PWS_EMG_Emp.csv");

%% GRF DATA
% Grf_withoutControl = load("Sub1_PWS_GRF_NC.mat","GRF").GRF;
% grf_withoutControl_time = load("Sub1_PWS_GRF_NC.mat","time").time;
 
% Grf_withoutExo = load("Sub1_PWS_GRF_NE.mat","GRF").GRF;
% grf_withoutExo_time = load("Sub1_PWS_GRF_NE.mat","time").time;
 
% Grf_rlControl = load("Sub1_PWS_GRF_RL.mat","GRF").GRF;
% grf_rlControl_time = load("Sub1_PWS_GRF_RL.mat","time").time;
 
% Grf_empDeveloped = load("Sub1_PWS_GRF_Emp.mat","GRF").GRF;
% grf_empDeveloped_time = load("Sub1_PWS_GRF_Emp.mat","time").time;

%
% Subject 2:
% SlowWalking:

%% EMG DATA
% WithoutExo = readmatrix("Sub2_SlowWalking_EMG_NE.csv");
% WithoutControl = readmatrix("Sub2_SlowWalking_EMG_NC.csv");
% RL_control = readmatrix("Sub2_SlowWalking_EMG_RL.csv");
% emp_control = readmatrix("Sub2_SlowWalking_EMG_Emp.csv");

%% GRF DATA
% Grf_withoutControl = load("Sub2_SlowWalking_GRF_NC.mat","GRF").GRF;
% grf_withoutControl_time = load("Sub2_SlowWalking_GRF_NC.mat","time").time;
 
% Grf_withoutExo = load("Sub2_SlowWalking_GRF_NE.mat","GRF").GRF;
% grf_withoutExo_time = load("Sub2_SlowWalking_GRF_NE.mat","time").time;
 
% Grf_rlControl = load("Sub2_SlowWalking_GRF_RL.mat","GRF").GRF;
% grf_rlControl_time = load("Sub2_SlowWalking_GRF_RL.mat","time").time;
 
% Grf_empDeveloped = load("Sub2_SlowWalking_GRF_Emp.mat","GRF").GRF;
% grf_empDeveloped_time = load("Sub2_SlowWalking_GRF_Emp.mat","time").time;


% PWS:

%% EMG DATA
% WithoutExo = readmatrix("Sub2_PWS_EMG_NE.csv");
% WithoutControl = readmatrix("Sub2_PWS_EMG_NC.csv");
% RL_control = readmatrix("Sub2_PWS_EMG_RL.csv");
% emp_control = readmatrix("Sub2_PWS_EMG_Emp.csv");

%% GRF DATA
% Grf_withoutControl = load("Sub2_PWS_GRF_NC.mat","GRF").GRF;
% grf_withoutControl_time = load("Sub2_PWS_GRF_NC.mat","time").time;
 
% Grf_withoutExo = load("Sub2_PWS_GRF_NE.mat","GRF").GRF;
% grf_withoutExo_time = load("Sub2_PWS_GRF_NE.mat","time").time;
 
% Grf_rlControl = load("Sub2_PWS_GRF_RL.mat","GRF").GRF;
% grf_rlControl_time = load("Sub2_PWS_GRF_RL.mat","time").time;
 
% Grf_empDeveloped = load("Sub2_PWS_GRF_Emp.mat","GRF").GRF;
% grf_empDeveloped_time = load("Sub2_PWS_GRF_Emp.mat","time").time;

%
% Subject 3:
% SlowWalking:

%% EMG DATA
WithoutExo = readmatrix("Sub3_SlowWalking_EMG_NE.csv");
WithoutControl = readmatrix("Sub3_SlowWalking_EMG_NC.csv");
RL_control = readmatrix("Sub3_SlowWalking_EMG_RL.csv");
emp_control = readmatrix("Sub3_SlowWalking_EMG_Emp.csv");

%% GRF DATA
Grf_withoutControl = load("Sub3_SlowWalking_GRF_NC.mat","GRF").GRF;
grf_withoutControl_time = load("Sub3_SlowWalking_GRF_NC.mat","time").time;

Grf_withoutExo = load("Sub3_SlowWalking_GRF_NE.mat","GRF").GRF;
grf_withoutExo_time = load("Sub3_SlowWalking_GRF_NE.mat","time").time;

Grf_rlControl = load("Sub3_SlowWalking_GRF_RL.mat","GRF").GRF;
grf_rlControl_time = load("Sub3_SlowWalking_GRF_RL.mat","time").time;

Grf_empDeveloped = load("Sub3_SlowWalking_GRF_Emp.mat","GRF").GRF;
grf_empDeveloped_time = load("Sub3_SlowWalking_GRF_Emp.mat","time").time;

%
% Subject 4
% SlowWalking:

%% EMG DAtA
% WithoutExo = readmatrix("Sub4_SlowWalking_EMG_NE.csv");
% WithoutControl = readmatrix("Sub4_SlowWalking_EMG_NC.csv");
% RL_control = readmatrix("Sub4_SlowWalking_EMG_RL.csv");
% emp_control = readmatrix("Sub4_SlowWalking_EMG_Emp.csv");

%% GRF DATA
% Grf_withoutControl = load("Sub4_SlowWalking_GRF_NC.mat","GRF").GRF;
% grf_withoutControl_time = load("Sub4_SlowWalking_GRF_NC.mat","time").time;
 
% Grf_withoutExo = load("Sub4_SlowWalking_GRF_NE.mat","GRF").GRF;
% grf_withoutExo_time = load("Sub4_SlowWalking_GRF_NE.mat","time").time;
 
% Grf_rlControl = load("Sub4_SlowWalking_GRF_RL.mat","GRF").GRF;
% grf_rlControl_time = load("Sub4_SlowWalking_GRF_RL.mat","time").time;
 
% Grf_empDeveloped = load("Sub4_SlowWalking_GRF_Emp.mat","GRF").GRF;
% grf_empDeveloped_time = load("Sub4_SlowWalking_GRF_Emp.mat","time").time;

% PWS:

%%EMG DATA
% WithoutExo = readmatrix("Sub4_PWS_EMG_NE.csv");
% WithoutControl = readmatrix("Sub4_PWS_EMG_NC.csv");
% RL_control = readmatrix("Sub4_PWS_EMG_RL.csv");
% emp_control = readmatrix("Sub4_PWS_EMG_Emp.csv");

%% GRF DATA
% Grf_withoutControl = load("Sub4_PWS_GRF_NC.mat","GRF").GRF;
% grf_withoutControl_time = load("Sub4_PWS_GRF_NC.mat","time").time;
 
% Grf_withoutExo = load("Sub4_PWS_GRF_NE.mat","GRF").GRF;
% grf_withoutExo_time = load("Sub4_PWS_GRF_NE.mat","time").time;
 
% Grf_rlControl = load("Sub4_PWS_GRF_RL.mat","GRF").GRF;
% grf_rlControl_time = load("Sub4_PWS_GRF_RL.mat","time").time;
 
% Grf_empDeveloped = load("Sub4_PWS_GRF_Emp.mat","GRF").GRF;
% grf_empDeveloped_time = load("Sub4_PWS_GRF_Emp.mat","time").time;

%
%% Get EMG Data Processed for reference Data (withoutExo)

%delete nan entries
nan_val = isnan(WithoutExo(:,1));
WithoutExo(nan_val,:) = [];

emgtime_ref = WithoutExo(:,1);

ref_RF = WithoutExo(1:end,2);
ref_Vastus = WithoutExo(1:end,4);
ref_Gastro = WithoutExo(1:end,6);
ref_Soleus = WithoutExo(1:end,8);
ref_BF = WithoutExo(1:end,10);

[withoutExo_proRF, withoutExo_rectRF,withoutExo_lowRF] = processEMG_Outlier(ref_RF,1080);
[withoutExo_proVastus, withoutExo_rectVastus,withoutExo_lowVastus] = processEMG_Outlier(ref_Vastus,1080);
[withoutExo_proGastro, withoutExo_rectGastro,withoutExo_lowGastro] = processEMG_Outlier(ref_Gastro,1080);
[withoutExo_proSoleus, withoutExo_rectSoleus,withoutExo_lowSoleus] = processEMG_Outlier(ref_Soleus,1080);
[withoutExo_proBF, withoutExo_rectBF,withoutExo_lowBF] = processEMG_Outlier(ref_BF,1080);

plot(emgtime_ref,withoutExo_proVastus);
plot(emgtime_ref, withoutExo_proRF);
plot(emgtime_ref,withoutExo_proBF);
plot(emgtime_ref,withoutExo_proGastro);
plot(emgtime_ref, withoutExo_proSoleus);

%

grf_ref = Grf_withoutExo;

ref_lowBF= withoutExo_lowBF;
ref_lowRF= withoutExo_lowRF;
ref_lowVastus= withoutExo_lowVastus;
ref_lowGastro = withoutExo_lowGastro;
ref_lowSoleus= withoutExo_lowSoleus;

ref_proBF= withoutExo_proBF;
ref_proRF= withoutExo_proRF;
ref_proVastus= withoutExo_proVastus;
ref_proGastro = withoutExo_proGastro;
ref_proSoleus= withoutExo_proSoleus;

%
%% Get EMG Data for alle other Cases

%RL control

%delete nan entries 
nan_val = isnan(RL_control(:,1));
RL_control(nan_val,:) = [];

start_emg = 1;
RL_control = RL_control(start_emg:end,:);

emgtime_rl = RL_control(:,1)-RL_control(1,1);

rl_RF = RL_control(1:end,2);
rl_Vastus = RL_control(1:end,4);
rl_Gastro = RL_control(1:end,6);
rl_Soleus = RL_control(1:end,8);
rl_BF = RL_control(1:end,10);

plot(emgtime_rl,rl_RF);

[rl_proRF, rl_rectRF,rl_lowRF] = processEMG_Outlier_withReference(rl_RF,1080,ref_lowRF);
[rl_proVastus, rl_rectVastus,rl_lowVatus] = processEMG_Outlier_withReference(rl_Vastus,1080,ref_lowVastus);
[rl_proGastro, rl_rectGastro,rl_lowGastro] = processEMG_Outlier_withReference(rl_Gastro,1080,ref_lowGastro);
[rl_proSoleus, rl_rectSoleus,rl_lowSoleus] = processEMG_Outlier_withReference(rl_Soleus,1080,ref_lowSoleus);
[rl_proBF, rl_rectBF,rl_lowBF] = processEMG_Outlier_withReference(rl_BF,1080,ref_lowBF);

plot(emgtime_rl, rl_proVastus)

%

%% Emp-Developed Control

%delete nan entries 
nan_val = isnan(emp_control(:,1));
emp_control(nan_val,:) = [];

start_emg = 1;

emp_control = emp_control(start_emg:end,:);

emgtime_emp = emp_control(:,1)-emp_control(1,1);

emp_RF = emp_control(1:end,2);
emp_Vastus = emp_control(1:end,4);
emp_Gastro = emp_control(1:end,6);
emp_Soleus = emp_control(1:end,8);
emp_BF = emp_control(1:end,10);

plot(emgtime_emp,emp_RF);

[emp_proRF, emp_rectRF,emp_lowRF] = processEMG_Outlier_withReference(emp_RF,1080,ref_lowRF);
[emp_proVastus, emp_rectVastus,emp_lowVatus] = processEMG_Outlier_withReference(emp_Vastus,1080,ref_lowVastus);
[emp_proGastro, emp_rectGastro,emp_lowGastro] = processEMG_Outlier_withReference(emp_Gastro,1080,ref_lowGastro);
[emp_proSoleus, emp_rectSoleus,emp_lowSoleus] = processEMG_Outlier_withReference(emp_Soleus,1080,ref_lowSoleus);
[emp_proBF, emp_rectBF,emp_lowBF] = processEMG_Outlier_withReference(emp_BF,1080,ref_lowBF);

plot(emgtime_emp,emp_proVastus);

%

% WithoutControl

%delete nan entries 
nan_val = isnan(WithoutControl(:,1));
WithoutControl(nan_val,:) = [];

start_emg = 1;
WithoutControl = WithoutControl(start_emg:end,:);
emgtime_withoutControl = WithoutControl(:,1)-WithoutControl(1,1);

withoutControl_RF = WithoutControl(1:end,2);
withoutControl_Vastus = WithoutControl(1:end,4);
withoutControl_Gastro = WithoutControl(1:end,6);
withoutControl_Soleus = WithoutControl(1:end,8);
withoutControl_BF = WithoutControl(1:end,10);

plot(emgtime_withoutControl,withoutControl_RF);

[withoutControl_proRF, withoutControl_rectRF,withoutControl_lowRF] = processEMG_Outlier_withReference(withoutControl_RF,1080,ref_lowRF);
[withoutControl_proVastus, withoutControl_rectVastus,withoutControl_lowVatus] = processEMG_Outlier_withReference(withoutControl_Vastus,1080,ref_lowVastus);
[withoutControl_proGastro, withoutControl_rectGastro,withoutControl_lowGastro] = processEMG_Outlier_withReference(withoutControl_Gastro,1080,ref_lowGastro);
[withoutControl_proSoleus, withoutControl_rectSoleus,withoutControl_lowSoleus] = processEMG_Outlier_withReference(withoutControl_Soleus,1080,ref_lowSoleus);
[withoutControl_proBF, withoutControl_rectBF,withoutControl_lowBF] = processEMG_Outlier_withReference(withoutControl_BF,1080,ref_lowBF);

plot(emgtime_withoutControl,withoutControl_proRF);

%
%% Find the Beginning of the Steps
% Here, the beginning of the steps in the GRF data is found. A new step is found at touch down. Because touch down is defined with a constant rise of the GRF value, the chosen threshold for each dataset was chosen depending on the data.

%--------------------Reference-----------------------

grf_ref_new = grf_ref;
time_grf_ref_new = grf_withoutExo_time;

time_start = 1;

grf_ref_new = grf_ref_new(time_start:end);
time_grf_ref_new = time_grf_ref_new(time_start:end)-time_grf_ref_new(time_start);

stepStart_ref = zeros(size(grf_ref_new));
starttimegrf = find(time_grf_ref_new == 0,1);
y = starttimegrf;

%sub2 comfort 100
%sub2 250
%sub4 250
%sub4 comfort 50
%sub1 comfort 50
%sub1 50
%sub3 250
threshold = 250;

while y<(length(grf_ref_new)-7)
    if grf_ref_new(y)<threshold && grf_ref_new(y+1)<threshold && grf_ref_new(y+3)<threshold &&  y<(length(grf_ref_new)-3)
        while grf_ref_new(y)<threshold && grf_ref_new(y+1)<threshold && grf_ref_new(y+3)<threshold &&  y<(length(grf_ref_new)-3)
            y =y+1;
        end
    else
        if (grf_ref_new(y+1)>threshold && grf_ref_new(y+2)>threshold && (sum(grf_ref_new(y:y+10))/10)>threshold)
            stepStart_ref(y)=1;
            while(grf_ref_new(y+1)>threshold || (grf_ref_new(y+1)<threshold && grf_ref_new(y+7)>threshold)) && y<(length(grf_ref_new)-7)
                y=y+1;
            end
        else
            y=y+1;
        end
    end
end

plot(time_grf_ref_new,grf_ref_new);
hold on
plot(emgtime_ref, ref_proBF*100);
xline(time_grf_ref_new(find(stepStart_ref==1)));
hold off

%

%---------------------AiControl------------------------------------------

grf_rl_new = Grf_rlControl;
time_grf_rl_new = grf_rlControl_time;

y = 1;
stepStart_rl = zeros(size(grf_rl_new));

% sub2 comfort 50
%sub2 50
%sub4 250
%sub4 comfort 50
%sub1 comfort 50
%sub1 50
%sub3 250
threshold = 250;

while y<(length(grf_rl_new)-7)
    if grf_rl_new(y)<threshold && grf_rl_new(y+1)<threshold && grf_rl_new(y+3)<threshold &&  y<(length(grf_rl_new)-3)
        while grf_rl_new(y)<threshold && grf_rl_new(y+1)<threshold && grf_rl_new(y+3)<threshold &&  y<(length(grf_rl_new)-3)
            y =y+1;
        end
    else
        if (grf_rl_new(y+1)>threshold && grf_rl_new(y+2)>threshold && (sum(grf_rl_new(y:y+10))/10)>threshold)
            stepStart_rl(y)=1;
            while(grf_rl_new(y+1)>threshold || (grf_rl_new(y+1)<threshold && grf_rl_new(y+7)>threshold)) && y<(length(grf_rl_new)-7)
                y=y+1;
            end
        else
            y=y+1;
        end
    end
end
% startindex_rl=1;

plot(time_grf_rl_new,grf_rl_new);
hold on
plot(emgtime_rl, rl_proBF*100);
xline(time_grf_rl_new(find(stepStart_rl==1)));
hold off

%

%-----------------------Emp-developed----------------------------------

grf_emp_new = Grf_empDeveloped;
time_grf_emp_new = grf_empDeveloped_time;

stepStart_emp = zeros(size(grf_emp_new));

starttimegrf = find(time_grf_emp_new == 0,1);
y = starttimegrf;

%sub2 comfort threshold 50
%sub2 250
%sub4 250
%sub4 comfort 50
%sub1 comfort 50
%sub1 50
%sub3 250
threshold = 250;

while y<(length(grf_emp_new)-7)
    if grf_emp_new(y)<threshold && grf_emp_new(y+1)<threshold && grf_emp_new(y+3)<threshold &&  y<(length(grf_emp_new)-3)
        while grf_emp_new(y)<threshold && grf_emp_new(y+1)<threshold && grf_emp_new(y+3)<threshold &&  y<(length(grf_emp_new)-3)
            y =y+1;
        end
    else
        if (grf_emp_new(y+1)>threshold && grf_emp_new(y+2)>threshold && (sum(grf_emp_new(y:y+10))/10)>threshold)
            stepStart_emp(y)=1;
            while(grf_emp_new(y+1)>threshold || (grf_emp_new(y+1)<threshold && grf_emp_new(y+7)>threshold)) && y<(length(grf_emp_new)-7)
                y=y+1;
            end
        else
            y=y+1;
        end
    end
end

plot(time_grf_emp_new,grf_emp_new);
hold on
plot(emgtime_emp, emp_proBF*100);
xline(time_grf_emp_new(find(stepStart_emp==1)));
hold off

%

%-----------------------WithoutControl----------------------------------

grf_withoutControl_new= Grf_withoutControl;
time_grf_withoutControl_new = grf_withoutControl_time;

stepStart_withoutControl = zeros(size(grf_withoutControl_new));
starttimegrf = find(time_grf_withoutControl_new == 0,1);
y = starttimegrf;

%sub2 comfort threshold 50
%sub2 250
%sub4 250
%sub4 comfort 100
%sub1 comfort 50
%sub1 50
%sub3 250
threshold = 250;

while y<(length(grf_withoutControl_new)-7)
    if grf_withoutControl_new(y)<threshold && grf_withoutControl_new(y+1)<threshold && grf_withoutControl_new(y+3)<threshold &&  y<(length(grf_withoutControl_new)-3)
        while grf_withoutControl_new(y)<threshold && grf_withoutControl_new(y+1)<threshold && grf_withoutControl_new(y+3)<threshold &&  y<(length(grf_withoutControl_new)-3)
            y =y+1;
        end
    else
        if (grf_withoutControl_new(y+1)>threshold && grf_withoutControl_new(y+2)>threshold && (sum(grf_withoutControl_new(y:y+10))/10)>threshold)
            stepStart_withoutControl(y)=1;
            while(grf_withoutControl_new(y+1)>threshold || (grf_withoutControl_new(y+1)<threshold && grf_withoutControl_new(y+7)>250)) && y<(length(grf_withoutControl_new)-7)
                y=y+1;
            end
        else
            y=y+1;
        end
    end
end


plot(time_grf_withoutControl_new,grf_withoutControl_new);
hold on
plot(emgtime_withoutControl, withoutControl_proBF*100);
xline(time_grf_withoutControl_new(find(stepStart_withoutControl==1)));
hold off

%
%% Interpolating EMG Data to use the StepStarts properly
% To be able to use the beginning ot he steps within the EMG data, interpolation has to be used to get the data into the same timing as the GRF data.

%--------------Reference---------------------------------

new_time_emg_ref = (0:1/240:emgtime_ref(end))';

ref_new_RF = interp1(emgtime_ref,ref_proRF(1:end),new_time_emg_ref);
ref_new_Soleus = interp1(emgtime_ref, ref_proSoleus(1:end), new_time_emg_ref);
ref_new_BF = interp1(emgtime_ref,ref_proBF(1:end), new_time_emg_ref);
ref_new_Vastus = interp1(emgtime_ref,ref_proVastus(1:end),new_time_emg_ref);
ref_new_Gastro = interp1(emgtime_ref, ref_proGastro(1:end), new_time_emg_ref);

stepStartTime_ref = time_grf_ref_new(find(stepStart_ref==1));

stepStarts_emg_ref = zeros(size(find(stepStartTime_ref<=new_time_emg_ref(end))));

i = 1;
while stepStartTime_ref(i)<=new_time_emg_ref(end)
    stepStarts_emg_ref(i)=find(abs(new_time_emg_ref - stepStartTime_ref(i))<0.00005);
    i=i+1;

    if i>length(stepStartTime_ref)
        break;
    end
end

plot(new_time_emg_ref, ref_new_Vastus);
hold on
xline(new_time_emg_ref(stepStarts_emg_ref));
hold off

%

%--------------RL---------------------------------

new_time_emg_rl =(0:1/240:emgtime_rl(end))';

rl_new_RF = interp1(emgtime_rl,rl_proRF(1:end),new_time_emg_rl);
rl_new_Soleus = interp1(emgtime_rl, rl_proSoleus(1:end), new_time_emg_rl);
rl_new_BF = interp1(emgtime_rl,rl_proBF(1:end), new_time_emg_rl);
rl_new_Vastus = interp1(emgtime_rl,rl_proVastus(1:end),new_time_emg_rl);
rl_new_Gastro = interp1(emgtime_rl, rl_proGastro(1:end), new_time_emg_rl);

stepStartTime_rl = time_grf_rl_new(find(stepStart_rl==1));

stepStarts_emg_rl = zeros(size(find(stepStartTime_rl<=new_time_emg_rl(end))));

i = 1;
while stepStartTime_rl(i)<=new_time_emg_rl(end)
    stepStarts_emg_rl(i)=find(abs(new_time_emg_rl - stepStartTime_rl(i))<0.00005);
    i=i+1;

    if i>length(stepStartTime_rl)
        break;
    end
end

plot(new_time_emg_rl, rl_new_Vastus);
hold on
xline(new_time_emg_rl(stepStarts_emg_rl));
hold off

%

%--------------Emp-Developed---------------------------------

new_time_emg_emp = (0:1/240:emgtime_emp(end))';

emp_new_RF = interp1(emgtime_emp,emp_proRF(1:end),new_time_emg_emp);
emp_new_Soleus = interp1(emgtime_emp, emp_proSoleus(1:end), new_time_emg_emp);
emp_new_BF = interp1(emgtime_emp,emp_proBF(1:end), new_time_emg_emp);
emp_new_Vastus = interp1(emgtime_emp,emp_proVastus(1:end),new_time_emg_emp);
emp_new_Gastro = interp1(emgtime_emp, emp_proGastro(1:end), new_time_emg_emp);

stepStartTime_emp = time_grf_emp_new(find(stepStart_emp==1));

stepStarts_emg_emp = zeros(size(find(stepStartTime_emp<=new_time_emg_emp(end))));

i = 1;
while stepStartTime_emp(i)<=new_time_emg_emp(end)
    stepStarts_emg_emp(i)=find(abs(new_time_emg_emp - stepStartTime_emp(i))<0.00005);
    i=i+1;

    if i>length(stepStartTime_emp)
        break;
    end
end

plot(new_time_emg_emp, emp_new_RF);
hold on
xline(new_time_emg_emp(stepStarts_emg_emp));
hold off

%

%--------------WithoutControl---------------------------------

new_time_emg_withoutControl = (0:1/240:emgtime_withoutControl(end))';

withoutControl_new_RF = interp1(emgtime_withoutControl,withoutControl_proRF(1:end),new_time_emg_withoutControl);
withoutControl_new_Soleus = interp1(emgtime_withoutControl, withoutControl_proSoleus(1:end), new_time_emg_withoutControl);
withoutControl_new_BF = interp1(emgtime_withoutControl,withoutControl_proBF(1:end), new_time_emg_withoutControl);
withoutControl_new_Vastus = interp1(emgtime_withoutControl,withoutControl_proVastus(1:end),new_time_emg_withoutControl);
withoutControl_new_Gastro = interp1(emgtime_withoutControl, withoutControl_proGastro(1:end), new_time_emg_withoutControl);

stepStartTime_withoutControl = time_grf_withoutControl_new(find(stepStart_withoutControl==1));

stepStarts_emg_withoutControl = zeros(size(find(stepStartTime_withoutControl<=new_time_emg_withoutControl(end))));

i = 1;
while stepStartTime_withoutControl(i)<=new_time_emg_withoutControl(end)
    stepStarts_emg_withoutControl(i)=find(abs(new_time_emg_withoutControl - stepStartTime_withoutControl(i))<0.00005);
    i=i+1;

    if i>length(stepStartTime_withoutControl)
        break;
    end
end

plot(new_time_emg_withoutControl, withoutControl_new_Vastus);
hold on
xline(new_time_emg_withoutControl(stepStarts_emg_withoutControl));
hold off

%
%% Cutting EMG Data into separated Steps
% Here, the EMG data is cut into separated steps based on the found beginnings of the steps. For that, the data is first cut, then adjusted to have the same beginning points regarding the hight of the value to be able to align them easier and be able to create a better average at the end.
% Depending on the quality of the data, not all steps are considered in this process. Some measurements required that just a small part of the data could be used due to falsly data (very high EMG values that fall completely out of range).
% To create the graphs for the different muscles, the muscles need to be adjusted by hand for every case. Change the line where the arrow  points to (line 540, 641, 742 and 811).

%-----------------Reference--------------------------------
duration = [];
timesteps = [];
steps = [];

current_newMuscle_ref = ref_new_BF; % <------------- adjust to change the muscle 

%sub2 comfort 10 100
%sub2 10 10 
%sub4 10 10
%sub4 comfort 10 150
%sub1 comfort 10 10 
%sub1 10 10
%sub3 10 10
stepStarts_emg_ref_ = stepStarts_emg_ref(10:end-10);

startingPoints_stance_ref = current_newMuscle_ref(stepStarts_emg_ref_);
    
plot(new_time_emg_ref, current_newMuscle_ref);
hold on 
plot(new_time_emg_ref(stepStarts_emg_ref_), startingPoints_stance_ref);
hold off

use = ones(size(stepStarts_emg_ref_));

%if outliners shall be filtered out
std_startingPoints = std(use'.*startingPoints_stance_ref);

meanval_startingPoints = mean(use'.*startingPoints_stance_ref);
outliners = find(abs(std_startingPoints-startingPoints_stance_ref)>(meanval_startingPoints+(1.5*std_startingPoints)));

startingPoints_stance_new_ref = startingPoints_stance_ref;
startingPoints_stance_new_ref(outliners) = 0;

meanval_new = mean(use'.*startingPoints_stance_new_ref);

%aligning the starting points of each step 
diff_mean_startingpoints_stance = use'.*startingPoints_stance_new_ref(:) - meanval_new;

correct_startingPoints = interp1(new_time_emg_ref(stepStarts_emg_ref_), diff_mean_startingpoints_stance,new_time_emg_ref);

corrected_graphs_ref = current_newMuscle_ref - correct_startingPoints;

values_corrected_startingPoints = corrected_graphs_ref(stepStarts_emg_ref_);

plot(new_time_emg_ref, corrected_graphs_ref);
hold on 
plot(new_time_emg_ref(stepStarts_emg_ref_), values_corrected_startingPoints);
hold off

%normalizing the amplitudes
steps = [];
minmax=[];

for index = 1:size(startingPoints_stance_new_ref)-1
    start = stepStarts_emg_ref_(index);
    endpoint = stepStarts_emg_ref_(index+1);
    steps{index}=corrected_graphs_ref(start:endpoint);
    minmax(index,1,:)=max(steps{index});
    minmax(index,2,:)=min(steps{index});
end

mean_maxvalues = mean(minmax(:,1,:));
mean_minvalues = mean(minmax(:,2,:));

newgraphs = [];
newgraphs_cell = [];

for index=1:size(startingPoints_stance_new_ref,1)-1
    newgraphs_cell{index} = steps{index}-(minmax(index,1) + minmax(index,2))/2;
    newgraphs_cell{index} = newgraphs_cell{index} / (minmax(index,1) - minmax(index,2));
    newgraphs_cell{index} = newgraphs_cell{index} * (mean_maxvalues - mean_minvalues);
    newgraphs_cell{index} = newgraphs_cell{index} + (mean_minvalues + mean_maxvalues)/2;   
    
    newgraphs = [newgraphs; newgraphs_cell{index}(:)];
    %sind doppelt da start = endpoint vom nächsten
    if index ~= size(startingPoints_stance_new_ref,1)-1
        newgraphs(end) = [];
    end
end

% %normalizing duration of step
for index=1:size(startingPoints_stance_ref,1)-1
    start = stepStarts_emg_ref_(index);
    endpoint = stepStarts_emg_ref_(index+1);
    timesteps{index} = new_time_emg_ref(start:endpoint,1);
    steps{index} = corrected_graphs_ref(start:endpoint,1);
    duration(index) = new_time_emg_ref(stepStarts_emg_ref_(index+1),1)-new_time_emg_ref(stepStarts_emg_ref_(index),1);
end

mean_duration_stance = round(mean(duration),3);
factor_stance = mean_duration_stance./ duration;
time_corrected_steps_stance = [];
t_new=(0:0.0005:mean_duration_stance);
time_corrected_steps_interpol_stance_ref = [];

for index=1:size(startingPoints_stance_ref,1)-1
    diff_to_zero{index} = timesteps{index}-timesteps{index}(1);
    time_corrected_steps_stance{index} = diff_to_zero{index}.*factor_stance(index);
    time_corrected_steps_interpol_stance_ref(index,:) = interp1(time_corrected_steps_stance{index},steps{index},t_new,'linear','extrap');
end

%

%-----------------RL--------------------------------
duration = [];
timesteps = [];
steps = [];

current_newMuscle_rl = rl_new_BF;   % <------------- adjust to change the muscle 

%sub2 comfort 20 10
%sub2 10 0
%sub4 10 10
%sub4 comfort 10 100 
%sub1 comfort 10 70
%sub1 10 0 
%sub3 10 0
stepStarts_emg_rl_ = stepStarts_emg_rl(10:end-0);

startingPoints_stance_rl = current_newMuscle_rl(stepStarts_emg_rl_);

plot(new_time_emg_rl, current_newMuscle_rl);
hold on 
plot(new_time_emg_rl(stepStarts_emg_rl_), startingPoints_stance_rl);
hold off

use = ones(size(stepStarts_emg_rl_));

%if outliners shall be filtered out
std_startingPoints = std(use'.*startingPoints_stance_rl);

meanval_startingPoints = mean(use'.*startingPoints_stance_rl);
outliners = find(abs(std_startingPoints-startingPoints_stance_rl)>(meanval_startingPoints+(1.5*std_startingPoints)));

startingPoints_stance_new_rl = startingPoints_stance_rl;
% startingPoints_stance_new_rl(outliners) = 0;

meanval_new = mean(use'.*startingPoints_stance_new_rl);

%using mean value of starting points to align steps
diff_mean_startingpoints_stance = use'.*startingPoints_stance_new_rl(:) - meanval_new;

correct_startingPoints = interp1(new_time_emg_rl(stepStarts_emg_rl_), diff_mean_startingpoints_stance,new_time_emg_rl);

corrected_graphs_rl = current_newMuscle_rl - correct_startingPoints;

values_corrected_startingPoints = corrected_graphs_rl(stepStarts_emg_rl_);

plot(new_time_emg_rl, corrected_graphs_rl);
hold on 
plot(new_time_emg_rl(stepStarts_emg_rl_), values_corrected_startingPoints);
hold off

%normalizing amplitude of steps
steps = [];
minmax=[];

for index = 1:size(startingPoints_stance_new_rl)-1
    start = stepStarts_emg_rl_(index);
    endpoint = stepStarts_emg_rl_(index+1);
    steps{index}=corrected_graphs_rl(start:endpoint);
    minmax(index,1,:)=max(steps{index});
    minmax(index,2,:)=min(steps{index});
end

mean_maxvalues = mean(minmax(:,1,:));
mean_minvalues = mean(minmax(:,2,:));

newgraphs = [];
newgraphs_cell = [];

for index=1:size(startingPoints_stance_new_rl,1)-1
    newgraphs_cell{index} = steps{index}-(minmax(index,1) + minmax(index,2))/2;
    newgraphs_cell{index} = newgraphs_cell{index} / (minmax(index,1) - minmax(index,2));
    newgraphs_cell{index} = newgraphs_cell{index} * (mean_maxvalues - mean_minvalues);
    newgraphs_cell{index} = newgraphs_cell{index} + (mean_minvalues + mean_maxvalues)/2;   
    
    newgraphs = [newgraphs; newgraphs_cell{index}(:)];
    %sind doppelt da start = endpoint vom nächsten
    if index ~= size(startingPoints_stance_new_rl,1)-1
        newgraphs(end) = [];
    end
end

%normalizing duration of steps
for index=1:size(startingPoints_stance_rl,1)-1
    start = stepStarts_emg_rl_(index);
    endpoint = stepStarts_emg_rl_(index+1);
    timesteps{index} = new_time_emg_rl(start:endpoint,1);
    steps{index} = corrected_graphs_rl(start:endpoint,1);
    duration(index) = new_time_emg_rl(stepStarts_emg_rl_(index+1),1)-new_time_emg_rl(stepStarts_emg_rl_(index),1);
end

mean_duration_stance = round(mean(duration),3);
factor_stance = mean_duration_stance./ duration;
time_corrected_steps_stance = [];
t_new_rl=(0:0.0005:mean_duration_stance);
time_corrected_steps_interpol_stance_rl = [];

for index=1:size(startingPoints_stance_rl,1)-1
    diff_to_zero{index} = timesteps{index}-timesteps{index}(1);
    time_corrected_steps_stance{index} = diff_to_zero{index}.*factor_stance(index);
    time_corrected_steps_interpol_stance_rl(index,:) = interp1(time_corrected_steps_stance{index},steps{index},t_new_rl,'linear','extrap');
end

%

%-----------------Emp-Developed--------------------------------
duration = [];
timesteps = [];
steps = [];

current_newMuscle_emp = emp_new_BF; % <------------- adjust to change the muscle 

%sub2 comfort 20 30 
%sub2 GAS 32:end-110, all other 10 0
%sub4 10 0
%sub4 comfort Vastus 10:25, BF 80:end-150  all others 10:end- 150 
%sub1 comfort 10 0
%sub1 10 0
%sub3 10 0
stepStarts_emg_emp_ = stepStarts_emg_emp(10:end-0);

startingPoints_stance_emp = current_newMuscle_emp(stepStarts_emg_emp_);

plot(new_time_emg_emp, current_newMuscle_emp);
hold on 
plot(new_time_emg_emp(stepStarts_emg_emp_), startingPoints_stance_emp);
hold off


use = ones(size(stepStarts_emg_emp_));

%if outliners shall be filtered out
std_startingPoints = std(use'.*startingPoints_stance_emp);

meanval_startingPoints = mean(use'.*startingPoints_stance_emp);
% outliners = find(abs(std_startingPoints-startingPoints_stance_emp)>(meanval_startingPoints+(1.5*std_startingPoints)));

startingPoints_stance_new_emp = startingPoints_stance_emp;
% startingPoints_stance_new_emp(outliners) = 0;

meanval_new = mean(use'.*startingPoints_stance_new_emp);

diff_mean_startingpoints_stance = use'.*startingPoints_stance_new_emp(:) - meanval_new;

correct_startingPoints = interp1(new_time_emg_emp(stepStarts_emg_emp_), diff_mean_startingpoints_stance,new_time_emg_emp);

corrected_graphs_emp = current_newMuscle_emp- correct_startingPoints;

values_corrected_startingPoints = corrected_graphs_emp(stepStarts_emg_emp_);

plot(new_time_emg_emp, corrected_graphs_emp);
hold on 
plot(new_time_emg_emp(stepStarts_emg_emp_), values_corrected_startingPoints);
hold off

for index=1:size(startingPoints_stance_emp,1)-1
    start = stepStarts_emg_emp_(index);
    endpoint = stepStarts_emg_emp_(index+1);
    timesteps{index} = new_time_emg_emp(start:endpoint,1);
    steps{index} = corrected_graphs_emp(start:endpoint,1);
    duration(index) = new_time_emg_emp(stepStarts_emg_emp_(index+1),1)-new_time_emg_emp(stepStarts_emg_emp_(index),1);
end

mean_duration_stance = round(mean(duration),3);
factor_stance = mean_duration_stance./ duration;
time_corrected_steps_stance = [];
t_new_emp=(0:0.0005:mean_duration_stance);
time_corrected_steps_interpol_stance_emp = [];

for index=1:size(startingPoints_stance_emp,1)-1
    diff_to_zero{index} = timesteps{index}-timesteps{index}(1);
    time_corrected_steps_stance{index} = diff_to_zero{index}.*factor_stance(index);
    time_corrected_steps_interpol_stance_emp(index,:) = interp1(time_corrected_steps_stance{index},steps{index},t_new_emp,'linear','extrap');
end

%

%-----------------WithoutControl--------------------------------
duration = [];
timesteps = [];
steps = [];

current_newMuscle_withoutControl = withoutControl_new_BF;   % <------------- adjust to change the muscle 

%sub2 comfort 10 10
%sub2 10 10
%sub4 10 10
%sub4 comfort 10 10
%sub1 comfort 10 10
%sub1 10 10
%sub3 10 10
stepStarts_emg_withoutControl_ = stepStarts_emg_withoutControl(10:end-0);

startingPoints_stance_withoutControl = current_newMuscle_withoutControl(stepStarts_emg_withoutControl_);

plot(new_time_emg_withoutControl, current_newMuscle_withoutControl);
hold on 
plot(new_time_emg_withoutControl(stepStarts_emg_withoutControl_), startingPoints_stance_withoutControl);
hold off


use = ones(size(stepStarts_emg_withoutControl_));

%if outliners shall be filtered out
std_startingPoints = std(use'.*startingPoints_stance_withoutControl);

meanval_startingPoints = mean(use'.*startingPoints_stance_withoutControl);
outliners = find(abs(std_startingPoints-startingPoints_stance_withoutControl)>(meanval_startingPoints+(1.5*std_startingPoints)));

startingPoints_stance_new_withoutControl = startingPoints_stance_withoutControl;
startingPoints_stance_new_withoutControl(outliners) = 0;

meanval_new = mean(use'.*startingPoints_stance_new_withoutControl);

diff_mean_startingpoints_stance = use'.*startingPoints_stance_new_withoutControl(:) - meanval_new;

correct_startingPoints = interp1(new_time_emg_withoutControl(stepStarts_emg_withoutControl_), diff_mean_startingpoints_stance,new_time_emg_withoutControl);

corrected_graphs_withoutControl = current_newMuscle_withoutControl - correct_startingPoints;

values_corrected_startingPoints = corrected_graphs_withoutControl(stepStarts_emg_withoutControl_);

plot(new_time_emg_withoutControl, corrected_graphs_withoutControl);
hold on 
plot(new_time_emg_withoutControl(stepStarts_emg_withoutControl_), values_corrected_startingPoints);
hold off

for index=1:size(startingPoints_stance_withoutControl,1)-1
    start = stepStarts_emg_withoutControl_(index);
    endpoint = stepStarts_emg_withoutControl_(index+1);
    timesteps{index} = new_time_emg_withoutControl(start:endpoint,1);
    steps{index} = corrected_graphs_withoutControl(start:endpoint,1);
    duration(index) = new_time_emg_withoutControl(stepStarts_emg_withoutControl_(index+1),1)-new_time_emg_withoutControl(stepStarts_emg_withoutControl_(index),1);
end

mean_duration_stance = round(mean(duration),3);
factor_stance = mean_duration_stance./ duration;
time_corrected_steps_stance = [];
t_new_withoutControl=(0:0.0005:mean_duration_stance);
time_corrected_steps_interpol_stance_withoutControl = [];

for index=1:size(startingPoints_stance_withoutControl,1)-1
    diff_to_zero{index} = timesteps{index}-timesteps{index}(1);
    time_corrected_steps_stance{index} = diff_to_zero{index}.*factor_stance(index);
    time_corrected_steps_interpol_stance_withoutControl(index,:) = interp1(time_corrected_steps_stance{index},steps{index},t_new_withoutControl,'linear','extrap');
end

%
% These plots show all steps layered on top of each other.

plot(t_new,time_corrected_steps_interpol_stance_ref);
hold on
title('WithoutExo');
hold off

plot(t_new_rl,time_corrected_steps_interpol_stance_rl);
hold on
title('RL');
hold off

plot(t_new_emp,time_corrected_steps_interpol_stance_emp);
hold on
title('Emp');
hold off

plot(t_new_withoutControl,time_corrected_steps_interpol_stance_withoutControl);
hold on 
title('WithoutControl')
hold off

%
%% Taking Average over all measurements of muscle activity
% Now, the average step is created based on the previously selected and layered steps.

average_ref = mean(time_corrected_steps_interpol_stance_ref);
average_rl = mean(time_corrected_steps_interpol_stance_rl);
average_emp = mean(time_corrected_steps_interpol_stance_emp);
average_withoutControl = mean(time_corrected_steps_interpol_stance_withoutControl);

plot(t_new, average_ref);
hold on
plot(t_new_withoutControl, average_withoutControl);
plot(t_new_rl, average_rl);
plot(t_new_emp, average_emp);
xlabel("Time[s]");
ylabel("")
hold off
legend ('NE','NC','RL','Emp')

%
%% Normalizing the three graphs to be able to compare them => without Exo is reference
%
% Here, the data is normalized to the NE case. This way it is easy to see, what the different controls and ne NC caused and how the data changed. First, the bias was taken out of the data and afterwards, the normalization was done.

%remove bias
min_nc = min(average_withoutControl);
min_ne = min(average_ref);
min_rl = min(average_rl);
min_emp = min(average_emp);

average_withoutControl = average_withoutControl-min_nc;
average_ref = average_ref - min_ne;
average_rl = average_rl - min_rl;
average_emp = average_emp - min_emp;

minLow = min(average_ref);
norm_final_ref = (average_ref - minLow)./ (max(average_ref) - minLow);

% minLow = min(average_rl);
norm_final_rl = (average_rl - minLow)./ (max(average_ref) - minLow);

% minLow = min(average_emp);
norm_final_emp = (average_emp - minLow)./ (max(average_ref) - minLow);

% minLow = min(average_withoutControl);
norm_final_withoutControl = (average_withoutControl - minLow)./ (max(average_ref) - minLow);

plot(t_new, norm_final_ref);
hold on
plot(t_new_withoutControl, norm_final_withoutControl);
plot(t_new_rl, norm_final_rl);
plot(t_new_emp, norm_final_emp);
axis([0 1.4 -0.05 inf]);
xlabel("Time[s]");
ylabel("Muscle Activation")
hold off
legend ('NE','NC','RL','Emp')

%
%% Correcting the start and ending to be equal
% Due to the cutting of the data and layering the data does not always start and end at the same height. Since it is considered a regular pattern, which repeats with every new step and also to make it easier to compare, the start and ending points of the curves are aligned.

%Reference
mean_height = mean([norm_final_ref(1) norm_final_ref(end)]);
diff_corrected_mean = [norm_final_ref(1) norm_final_ref(end)] - mean_height;

interpol_graphs_final_ref = interp1(t_new([1 end]),diff_corrected_mean,t_new);
corrected_graphs_final_ref = norm_final_ref - interpol_graphs_final_ref;

%emp-developed
mean_height=[];
mean_height = mean([norm_final_emp(1) norm_final_emp(end)]);
diff_corrected_mean = [norm_final_emp(1) norm_final_emp(end)] - mean_height;

interpol_graphs_final_emp = interp1(t_new_emp([1 end]),diff_corrected_mean,t_new_emp);
corrected_graphs_final_emp = norm_final_emp - interpol_graphs_final_emp;

%RL
mean_height=[];
mean_height = mean([norm_final_rl(1) norm_final_rl(end)]);
diff_corrected_mean = [norm_final_rl(1) norm_final_rl(end)] - mean_height;

interpol_graphs_final_rl = interp1(t_new_rl([1 end]),diff_corrected_mean,t_new_rl);
corrected_graphs_final_rl = norm_final_rl - interpol_graphs_final_rl;

%withoutControl
mean_height=[];
mean_height = mean([norm_final_withoutControl(1) norm_final_withoutControl(end)]);
diff_corrected_mean = [norm_final_withoutControl(1) norm_final_withoutControl(end)] - mean_height;

interpol_graphs_final_withoutControl = interp1(t_new_withoutControl([1 end]),diff_corrected_mean,t_new_withoutControl);
corrected_graphs_final_withoutControl = norm_final_withoutControl - interpol_graphs_final_withoutControl;

plot(t_new, corrected_graphs_final_ref);
hold on
plot(t_new_withoutControl, corrected_graphs_final_withoutControl);
plot(t_new_rl, corrected_graphs_final_rl);
plot(t_new_emp, corrected_graphs_final_emp);
xlabel("Time[s]");
ylabel("")
hold off
legend ('NE','NC','RL','Emp')

%
%% Save Results
% Here the data is just stored in the defined location.

empDev = 'DataAnalysis\sub3\SlowWalking\BF\empDev';
rlcontrol = 'DataAnalysis\sub3\SlowWalking\BF\rlControl';
withoutExo = 'DataAnalysis\sub3\SlowWalking\BF\withoutExo';
withoutControl = 'DataAnalysis\sub3\SlowWalking\BF\withoutControl';

save(empDev, "corrected_graphs_final_emp");
save(rlcontrol, "corrected_graphs_final_rl");
save(withoutExo, "corrected_graphs_final_ref");
save(withoutControl, "corrected_graphs_final_withoutControl");