%% Core Advantage - Metric VBT
%% Set up for participant

%Set the data directory name
dataDir = 'E:\';

%Set the participants ID
participantID = input('What is the participants ID: ','s');

%Open participants folder
cd([dataDir, participantID]);

%Get all .c3d file names
c3d = dir();
for aa = 1:size(c3d,1)
    subTrials{aa} = c3d(aa).name;
end
clear aa c3d

%Get list of squat trials
SquatTrials = {};
for tt = 1:length(subTrials)
    if contains(subTrials{tt},'Squat')
        SquatTrials{end+1} = subTrials{tt};
    end
end
%Sort list
temp = sort(SquatTrials); %Sort the trials in alphabetical order
X = temp(2:end); %Get all but the first cell (1RM)
X(end+1) = temp(1); %Add 1RM to the end
SquatTrials = X; %Allocate back to trial list
clear temp X

%Get list of bench press trials
BenchTrials = {};
for tt = 1:length(subTrials)
    if contains(subTrials{tt},'Bench')
        BenchTrials{end+1} = subTrials{tt};
    end
end
%Sort list
temp = sort(BenchTrials); %Sort the trials in alphabetical order
X = temp(2:end); %Get all but the first cell (1RM)
X(end+1) = temp(1); %Add 1RM to the end
BenchTrials = X; %Allocate back to trial list
clear temp X

%Clean up
clear tt c3d subTrials
clc;

%
%% Process squat trials

%Loop through squat trials
for tt = 1:length(SquatTrials)
    
    %Get trial and set trial name
    trialData = btk_loadc3d(SquatTrials{tt}); %Load in the participants c3d squat files
    trialName = (SquatTrials{tt}); trialName = trialName(1:end-4); %Set trial name
    
    %Filter raw marker data  -4th order, frequency 10
    filtFreq = 10; %Set filter frequency
    fs = 200; %Set the sampling rate
    filtOrder = 4; %Set order
    [b,a] = butter(filtOrder, filtFreq/(fs/2), 'low'); %Create the low-pass Butterworth filter
    trialData.marker_data.Markers.RBar = filtfilt(b,a,(trialData.marker_data.Markers.RBar)); %Filter RBar data
    trialData.marker_data.Markers.LBar = filtfilt(b,a,(trialData.marker_data.Markers.LBar)); %Filter LBar data
    clear a b filtFreq fs filtOrder
    clc;
    
    %Calculate mid-point of bar by averaging left and right marker positions
    trialData.marker_data.Markers.MidBar =  (trialData.marker_data.Markers.RBar + trialData.marker_data.Markers.LBar) / 2;
    
    %Find start and end points for each repetition from min/max marker height
    
    %Find start indices using max point of markers in vertical
    %direction only (Invert data so that bottom of squat is a peak
    %to use findpeaks function)
    neg = (trialData.marker_data.Markers.MidBar(1:end,3)) * -1; %Invert vertical marker position
    [bottomHeight,startLocationsMid] = findpeaks(neg, 'MinPeakHeight', -1300); %Get peaks and locations of peaks
    startLocationsMid = num2cell(startLocationsMid); %Convert double to cell array
    bottomHeight = num2cell(bottomHeight); %Convert double to cell array
    for ll = 1:(length(startLocationsMid))-2 %Loop through the locations for each peak
        point1 = startLocationsMid(ll); %Grab the current location
        point2 = startLocationsMid(ll+1); %Grab the location after the current one
        if ((point2{1} - point1{1}) < 190) %If the number of frames between 2 consecutive timepoints in < 300
            startLocationsMid(ll+1,:) = []; %Then remove the location of the second peak as this is a false rep
            bottomHeight(ll+1,:) = []; %And remove the location of the second peak
        end
    end
    clear ll point1 point2 bottomHeight
    
    %Calculate the instantaneous difference between markers in the
    %vertical direction
    for nn = 1:(length(neg))-1 %Loop through the inverted marker positions
        negDiff(nn,1) = neg(nn+1) - neg(nn); %Calculate the difference in marker positions between each frame
    end
    clear nn
    
    %Find all instances where negDiff crosses zero - indicating a
    %change in direction (top of the squat)
    x = find(negDiff > 0); %Find instances where negDiff is greater than 0
    %Loop through
    for ll = 1:length(startLocationsMid) %For each startLocation
        startLocationMid = startLocationsMid{ll}; %Get the frame number for the startLocation
        stopInd = find(x > startLocationMid); %Find the first instance that 0 appears after the startLocation frame number
        stopInd = stopInd(1); %Get the index for the stop frame
        stopLocationsMid(ll,1) = x(stopInd); %Get the stop frame and store in stopLocations
    end
    stopLocationsMid = num2cell(stopLocationsMid); %Convert double to cell array
    clear x ll startLocationMid stopInd negDiff neg
    clc;
    
    %Calculate vertical RoM (cm)
    repNumber = 0;
    MidBarV = (trialData.marker_data.Markers.MidBar(1:end,3));
    MidBarV = MidBarV/10; %Convert from mm to cm
    for ll = 1:length(startLocationsMid)
        repNumber = repNumber + 1;
        startLocationMid = cell2mat(startLocationsMid(repNumber));
        stopLocationMid = cell2mat(stopLocationsMid(repNumber));
        output.Squat_MidBar_RoM.(trialName)(repNumber,1) = MidBarV(stopLocationMid) - MidBarV(startLocationMid);
    end
    clear ll repNumber startLocationMid stopLocationMid MidBarV
    
    %Calculate average velocity (m/s)
    
        %Get time
        time = trialData.marker_data.Time;
        
        %Calculate average vertical velocity
        repNumber = 0;
        for ll = 1:length(startLocationsMid)
            repNumber = repNumber + 1;
            startTimeMid = time(startLocationsMid{ll,1});
            stopTimeMid = time(stopLocationsMid{ll,1});
            timeChange = stopTimeMid - startTimeMid;
            output.Squat_MidBar_Velocity_Vert.(trialName)(repNumber,1) = (output.Squat_MidBar_RoM.(trialName)(repNumber,1) /100) / (timeChange);
        end
        clear ll repNumber startLocationMid stopLocationMid startTimeMid stopTimeMid timeChange
        
        %Calculate average anterior-posterior velocity
        repNumber = 0;
        MidBarAP = trialData.marker_data.Markers.MidBar(1:end,2) /1000; %Get anterior-posterior marker data and convert from mm to m
        for ll = 1:length(startLocationsMid)
            repNumber = repNumber + 1;
            AP_RoM = MidBarAP(stopLocationsMid{ll,1}) - MidBarAP(startLocationsMid{ll,1});
            startTimeMid = time(startLocationsMid{ll,1});
            stopTimeMid = time(stopLocationsMid{ll,1});
            timeChange = stopTimeMid - startTimeMid;
            output.Squat_MidBar_Velocity_AP.(trialName)(repNumber,1) = (AP_RoM) / (timeChange);
        end
        clear ll repNumber startLocationMid stopLocationMid AP_RoM startTimeMid stopTimeMid timeChange MidBarAP
        
        %Calculate resultant velocities
        repNumber = 0;
        for ll = 1:length(startLocationsMid)
            repNumber = repNumber + 1;
            output.Squat_MidBar_Velocity_Resultant.(trialName)(repNumber,1) = hypot(output.Squat_MidBar_Velocity_Vert.(trialName)(repNumber,1),output.Squat_MidBar_Velocity_AP.(trialName)(repNumber,1));
        end
        clear ll repNumber
    
    
    %Clean up after rep
    clear trialData trialName startLocationsMid stopLocationsMid
    
end
clear tt time 

%
%% Process bench press trials

%Loop through bench press trials
for tt = 1:length(BenchTrials)
    
    %Get trial and set trial name
    trialData = btk_loadc3d(BenchTrials{tt}); %Load in the participants c3d bench press files
    trialName = (BenchTrials{tt}); trialName = trialName(1:end-4); %Set trial name
    
    %Filter raw marker data  -4th order, frequency 10
    filtFreq = 10; %Set filter frequency
    fs = 200; %Set the sampling rate
    filtOrder = 4; %Set order
    [b,a] = butter(filtOrder, filtFreq/(fs/2), 'low'); %Create the low-pass Butterworth filter
    trialData.marker_data.Markers.RBar = filtfilt(b,a,(trialData.marker_data.Markers.RBar)); %Filter RBar data
    trialData.marker_data.Markers.LBar = filtfilt(b,a,(trialData.marker_data.Markers.LBar)); %Filter LBar data
    clear a b filtFreq fs filtOrder
    clc;
        
    %Calculate mid-point of bar by averaging left and right marker positions
    trialData.marker_data.Markers.MidBar =  (trialData.marker_data.Markers.RBar + trialData.marker_data.Markers.LBar) / 2;
    
    %Find start and end points for each repetition from min/max marker height
    
    %Find start indices using max point of markers in vertical
    %direction only (Invert data so that bottom of bench press is a peak
    %to use findpeaks function
    neg = (trialData.marker_data.Markers.MidBar(1:end,3)) * -1; %Invert vertical marker position
    [bottomHeight,startLocationsMid] = findpeaks(neg, 'MinPeakHeight', -800); %Get peaks and locations of peaks
    startLocationsMid = num2cell(startLocationsMid); %Convert double to cell array
    bottomHeight = num2cell(bottomHeight); %Convert double to cell array      
    for ll = 1:(length(startLocationsMid))-6 %Loop through the locations for each peak
        point1 = startLocationsMid(ll); %Grab the current location
        point2 = startLocationsMid(ll+1); %Grab the location after the current one
        if ((point2{1} - point1{1}) < 150) %If the number of frames between 2 consecutive timepoints in < 300            
            startLocationsMid(ll,:) = []; %Then remove the location of the first peak as this is a false rep
            bottomHeight(ll,:) = []; %And remove the location of the first peak
        end
    end
    clear ll point1 point2 bottomHeight
    
    %Calculate the instantaneous difference between markers in the
    %vertical direction
    for nn = 1:(length(neg))-1 %Loop through the inverted marker positions
        negDiff(nn,1) = neg(nn+1) - neg(nn); %Calculate the difference in marker positions between each frame
    end
    clear nn
    
    %Find all instances where negDiff crosses zero - indicating a
    %change in direction (top of the bench press)
    x = find(negDiff > 0); %Find instances where negDiff is greater than 0
    %Loop through
    for ll = 1:length(startLocationsMid) %For each startLocation
        startLocationMid = startLocationsMid{ll}; %Get the frame number for the startLocation
        stopInd = find(x > startLocationMid); %Find the first instance that 0 appears after the startLocation frame number
        stopInd = stopInd(1); %Get the index for the stop frame
        stopLocationsMid(ll,1) = x(stopInd); %Get the stop frame and store in stopLocations
    end
    stopLocationsMid = num2cell(stopLocationsMid); %Convert double to cell array
    clear x ll startLocationMid stopInd negDiff neg
    clc;
    
    %Calculate vertical RoM (cm)
    repNumber = 0;
    MidBarV = (trialData.marker_data.Markers.MidBar(1:end,3));
    MidBarV = MidBarV/10; %Convert from mm to cm
    for ll = 1:length(startLocationsMid)
        repNumber = repNumber + 1;
        startLocationMid = cell2mat(startLocationsMid(repNumber));
        stopLocationMid = cell2mat(stopLocationsMid(repNumber));
        output.Bench_MidBar_RoM.(trialName)(repNumber,1) = MidBarV(stopLocationMid) - MidBarV(startLocationMid);
    end
    clear ll repNumber startLocationMid stopLocationMid MidBarV
    
    %Calculate average velocity (m/s)
    
        %Get time
        time = trialData.marker_data.Time;
        
        %Calculate average vertical velocity
        repNumber = 0;
        for ll = 1:length(startLocationsMid)
            repNumber = repNumber + 1;
            startTimeMid = time(startLocationsMid{ll,1});
            stopTimeMid = time(stopLocationsMid{ll,1});
            timeChange = stopTimeMid - startTimeMid;
            output.Bench_MidBar_Velocity_Vert.(trialName)(repNumber,1) = (output.Bench_MidBar_RoM.(trialName)(repNumber,1) /100) / (timeChange);
        end
        clear ll repNumber startLocationMid stopLocationMid startTimeMid stopTimeMid timeChange
        
        %Calculate average anterior-posterior velocity
        repNumber = 0;
        MidBarAP = trialData.marker_data.Markers.MidBar(1:end,2) /1000; %Get anterior-posterior marker data and convert from mm to m
        for ll = 1:length(startLocationsMid)
            repNumber = repNumber + 1;
            AP_RoM = MidBarAP(stopLocationsMid{ll,1}) - MidBarAP(startLocationsMid{ll,1});
            startTimeMid = time(startLocationsMid{ll,1});
            stopTimeMid = time(stopLocationsMid{ll,1});
            timeChange = stopTimeMid - startTimeMid;
            output.Bench_MidBar_Velocity_AP.(trialName)(repNumber,1) = (AP_RoM) / (timeChange);
        end
        clear ll repNumber startLocationMid stopLocationMid AP_RoM startTimeMid stopTimeMid timeChange MidBarAP
        
        %Calculate resultant velocities
        repNumber = 0;
        for ll = 1:length(startLocationsMid)
            repNumber = repNumber + 1;
            output.Bench_MidBar_Velocity_Resultant.(trialName)(repNumber,1) = hypot(output.Bench_MidBar_Velocity_Vert.(trialName)(repNumber,1),output.Bench_MidBar_Velocity_AP.(trialName)(repNumber,1));
        end
        clear ll repNumber
    
    
    %Clean up after rep
    clear trialData trialName startLocationsMid stopLocationsMid
    
end
clear tt time

%
%% Compile data and save participants output

%Put squat and bench rep data into one column

    %Create empty variables to append to
    finalOutput.Squat_Rbar_RoM = [];
    finalOutput.Squat_Rbar_Velocity_Resultant = [];
    finalOutput.Squat_MidBar_RoM = [];
    finalOutput.Squat_MidBar_Velocity_Resultant = [];
    finalOutput.Bench_Rbar_RoM = [];
    finalOutput.Bench_Rbar_Velocity_Resultant = [];
    finalOutput.Bench_MidBar_RoM = [];
    finalOutput.Bench_MidBar_Velocity_Resultant = [];
    
    %Get list of variables to loop through
    SquatVars = {'Squat_MidBar_RoM';'Squat_MidBar_Velocity_Resultant'};
    
    %Loop through SquatTrials
    for tt = 1:length(SquatTrials)
        
        %Loop through list of variables
        for vv = 1:length(SquatVars)
            
            %Get results (i.e., rep numbers) to loop through
            repData = num2cell(output.(SquatVars{vv}).(SquatTrials{tt}(1:end-4)));
            
            %Loop through individual reps
            for rr = 1:length(repData)
                
                %Add data to squat variable
                finalOutput.(SquatVars{vv})(end+1,1) = (repData{rr});
                                
            end %End individual rep loop
            
        end %End variable loop
                
    end %End loop of squat trials
    clear tt vv rr repData SquatVars
    
    
    %Get list of variables to loop through
    BenchVars = {'Bench_MidBar_RoM';'Bench_MidBar_Velocity_Resultant'};
    
    %Loop through SquatTrials
    for tt = 1:length(BenchTrials)
        
        %Loop through list of variables
        for vv = 1:length(BenchVars)
            
            %Get results (i.e., rep numbers) to loop through
            repData = num2cell(output.(BenchVars{vv}).(BenchTrials{tt}(1:end-4)));
            
            %Loop through individual reps
            for rr = 1:length(repData)
                
                %Add data to squat variable
                finalOutput.(BenchVars{vv})(end+1,1) = (repData{rr});
                                
            end %End individual rep loop
            
        end %End variable loop
                
    end %End loop of squat trials
    clear tt vv rr repData BenchVars

%Save output
save([(participantID),'_output.mat']);