% % Initialize
close all
clear all 
clc

% % Locate and load file directories
datapath = uigetdir('','Select directory with files containing trials');
addpath(datapath)
files = dir(datapath);
fileNames = cell(1,0);

for f = 1:length(files)
    if contains(files(f).name, "Participant")
       fileNames{length(fileNames)+1} = files(f).name; 
    end
end

for i = 1:length(fileNames)
    % Define loop variables
    indexStartNewJump = 1;
    indexEndTakeoff = -1;
    indexStartLanding = 1;
    takeoffForce = [];
    takeoffTime = [];
    landingForce = [];
    landingTime = [];
    peakValues = [];
    peakTimes = [];
    minPowers = [];
    minPowerTimes = [];
    zeroTimes = [];
    zeroValues = [];
    participant = append("Participant ", int2str(i));
    fileIndex = find(contains(fileNames, participant));
    trialRef = "";
    if fileIndex == -1 
        break
    end
    
    originalFileName  = fileNames{fileIndex};
    splitFileName = split(originalFileName, ".csv");
    
    % read header data
    f = fopen(originalFileName);
    columns = textscan(f, '%s%s%s', 'Delimiter',  ',', 'CollectOutput', true);
    mat = columns{1};


    timeIndex = find(mat(:,1) == "Time");
    headers = mat(timeIndex, :);
    weightIndex = find(mat(:,1) == "Weight");
    weight = mat(weightIndex, 2);
    weight = str2double(weight{1});
    freqIndex = find(mat(:,1) == "Frequency");
    freq = mat(freqIndex, 2);
    freq = str2double(freq{1});
    athleteIdIndex = find(mat(:,1) == "AthleteId");
    athleteId = mat(athleteIdIndex, 2);
    dateIndex = find(mat(:,1) == "Recording Date");
    date = mat(dateIndex, 2);
    dataSourceIndex = find(mat(:,1) == "Data Source");
    dataSource = mat(dataSourceIndex, 2);
    fclose(f);

    % quiet standing period (1sec)
    startData = readtable("startTimes.csv");
    endData = readtable("endTimes.csv");
    if contains(datapath, 'Trial 1')
        startTime = startData(strcmp(startData.participant, participant), :).cmj1;
        endTime = endData (strcmp(endData.participant, participant), :).cmj1;
        trialRef = "Trial 1";
    else
        startTime = startData(strcmp(startData.participant, participant), :).cmj2;
        endTime = endData (strcmp(endData.participant, participant), :).cmj2;
        trialRef = "Trial 2";
    end

    outputFileName = sprintf( 'Data/output/CMJ 1x20/%s/%s_output.xlsx', trialRef, splitFileName{1} );
    
    % set tableData headers
    idx = strncmpi(headers,'',1);
    headers(idx) = {'null'};

    % configure tabledata
    opts = detectImportOptions(originalFileName);
    opts.PreserveVariableNames = true;
    opts.Delimiter = ',';
    opts.VariableNamesLine = timeIndex;
    opts.VariableNames = headers;
    opts.SelectedVariableNames = ["Time", "Left", "Right"];

    tableData = readtable(originalFileName, opts);
    tableData = rmmissing(tableData);

    tableData = rmmissing(tableData);
    tableData.Properties.VariableNames = ["time", "left", "right"];
    tableData  = tableData((tableData.time > startTime & tableData.time < endTime ), : );

    % calc force
    netForce = (tableData.right + tableData.left);

    % calc acceleration
    netAcceleration = netForce / weight;
    netAcceleration = detrend(netAcceleration);

    % high-pass filter acceleration data
    N = 2;
    fc = 0.25; % cut-off freq [Hz]
    [B,A] = butter(N,2*fc/freq,'high');
    netAcceleration = filter(B,A,netAcceleration);

    % calculate velocity
    netVelocity = cumtrapz(tableData.time, netAcceleration);

    % calculate power
    netPower = netForce .* netVelocity;

    % find phases of each repetition
    % 1) establish baseline quiet bodyweight data
    startData = tableData( (tableData.time > startTime) & (tableData.time < startTime+1), : );

    % find quiet time weight and sd
    quietWeight = mean(netForce(1:freq));
    quietWeightSd = std(netForce(1:freq));

    

    while indexStartNewJump > 0 
        indexEndTakeoffPrev = indexEndTakeoff;
        indexStartLandingPrev = indexStartLanding;

        % take-off phase
        for to = indexStartLanding:(length(netForce) - 0.1 * freq)
           if (netForce(to) < 50) && (netForce(to + 0.1*freq) < 50)
               indexEndTakeoff = to;
               takeoffForce = [takeoffForce; netForce(indexEndTakeoff)];
               takeoffTime = [takeoffTime; tableData.time(indexEndTakeoff)];
               break
           end
        end

        % landing phase
        for sl = indexEndTakeoff:length(netForce)
           if (netForce(sl) > 50) && (netForce(sl+0.1*freq) > 50)
               indexStartLanding = sl;
               landingForce = [landingForce, netForce(indexStartLanding)];
               landingTime = [landingTime, tableData.time(indexStartLanding)];
               break
           end
        end

        if indexEndTakeoff == indexEndTakeoffPrev
           break 
        end
    end

    % reduce to first 20
    if length(takeoffForce) > 20 &&  length(landingForce) > 20 
        takeoffForce = takeoffForce(1:20);
        takeoffTime = takeoffTime(1:20);
        landingForce = landingForce(1:20);
        landingTime = landingTime(1:20);
    end

    % find peaks between landing (n) and take-off (n+1)
        for tof = 1:length(takeoffTime)-1
            landingFrame = floor( (landingTime(tof) * freq) - (startTime * freq) );
            takeoffFrame = floor( (takeoffTime(tof+1) * freq) - (startTime * freq) );

            if takeoffFrame + 700 < length(tableData.time)
            % if takeoffFrame - landingFrame > 700
                [peak, loc] = findpeaks( netPower(landingFrame:takeoffFrame), 'MinPeakDistance', 700 );
                peakValues = [peakValues; peak];
                for ti = 1:length(loc)
                    peakTimes = [peakTimes; tableData.time( loc(ti) + landingFrame )];    
                end     
            end
        end

     % find minimum power between takeoff peak and landing peak
        for p = 1:length(peakTimes)-1
            if rem(p,2) ~= 0 
                % closest value
                [ ~, landingFrame ] = min( abs( tableData.time-peakTimes(p) ) );
                [ ~, takeoffFrame ] = min( abs( tableData.time-peakTimes(p+1) ) );
                [minVal, minIndex] = min(netPower(landingFrame:takeoffFrame));
                minPowers = [minPowers, minVal];
                minPowerTimes = [minPowerTimes, tableData.time(landingFrame + minIndex)]; 
            end
        end

    % find zero-crossing point between minPower and peakValue
    for mpt = 1:length(peakTimes)
        if rem(mpt, 2) == 0 
             %closest value frame
            [ ~, minFrame ] = min( abs( tableData.time-minPowerTimes(mpt/2) ) );
            [ ~, peakFrame ] = min( abs( tableData.time-peakTimes(mpt) ) );

            % closest value zero-crossing
            [ zeroValue, zeroFrame ] = min( abs( netPower(minFrame:peakFrame)-0 ) );
            zeroFrame = zeroFrame + minFrame;

            zeroTimes = [zeroTimes, tableData.time(zeroFrame)];
            zeroValues = [zeroValues, zeroValue];
        end
    end

    % propulsion phase = zero-crossing -> takeoff (start flight)
    propPowers = {};
    propTimes = {};
    patchIdxs = [];
    for zt = 1:length(zeroTimes)
        % closest time
        [ ~, zeroFrame ] = min( abs( tableData.time-zeroTimes(zt) ) );
        [ ~, takeoffFrame ] = min( abs( tableData.time-takeoffTime(zt+1) ) );
        propPower = netPower(zeroFrame:takeoffFrame);
        propTime = tableData.time(zeroFrame:takeoffFrame);

        propTimes{1,zt+1} = num2cell(propTime);
        propPowers{1,zt+1} = num2cell(propPower);
    end


    % first jump
    origTakeoffTime = takeoffTime(1);
    [ ~, origTakeoffFrame ] = min( abs( tableData.time-origTakeoffTime ) );
    [origMax, origMaxFrame] = max( netPower(1:origTakeoffFrame) );
    peakValues = [origMax; peakValues];
    peakTimes = [tableData.time(origMaxFrame); peakTimes];

    [origMin, origMinFrame] = min( netPower(1:origTakeoffFrame) );
    minPowers = [origMin, minPowers];
    minPowerTimes =  [tableData.time(origMinFrame), minPowerTimes];

    [origZeroPower, origZeroFrame] = min( abs( netPower(origMinFrame:origMaxFrame)-0 ) );
    zeroTimes = [tableData.time(origZeroFrame+origMinFrame), zeroTimes];
    zeroValues = [origZeroPower, zeroValues];

    [ ~, origTakeoffFrame ] = min( abs( tableData.time-takeoffTime(1) ) );
    origPropPower = netPower(origZeroFrame+origMinFrame:origTakeoffFrame);
    origPropTime = tableData.time(origZeroFrame+origMinFrame:origTakeoffFrame);
    propTimes{1,1} = num2cell(origPropTime);
    propPowers{1,1} = num2cell(origPropPower);

    % calculate ave. power and ave peak power
    avePeakPower = 0;
    avePower = 0;
    maxes = [];
    maxPeakPower = 0;
    for powerInd = 1:length(propPowers)
        avePower = avePower + mean(cell2mat( propPowers{1,1}) );
        powers = cell2mat(propPowers{1,powerInd});
        max_ = max( powers(:,1) );
        if max_ > maxPeakPower
            maxPeakPower = max_;
        end
        maxes = [maxes, max_];
    end
    avePower = avePower / powerInd;
    avePeakPower = mean(maxes);

    writematrix(["Weight", weight], outputFileName);
    writematrix(["Frequency", freq], outputFileName, "WriteMode", "append");
    writematrix(["Recording date", date], outputFileName, "WriteMode", "append");
    writematrix(["Data source", dataSource], outputFileName, "WriteMode", "append");
    writematrix(["Athlete Id", athleteId], outputFileName, "WriteMode", "append");
    writematrix([" "], outputFileName, "WriteMode", "append");
    writematrix(["Ave prop power (W)", avePower], outputFileName, "WriteMode", "append");
    writematrix(["Ave max prop power (W)", avePeakPower], outputFileName, "WriteMode", "append");
    writematrix(["Max prop power (W)", maxPeakPower], outputFileName, "WriteMode", "append");
    writematrix([" "], outputFileName, "WriteMode", "append");
    writematrix(["Time", "Net force (N)", "Net power (W)"], outputFileName, "WriteMode", "append");
    writematrix([tableData.time, netForce, netPower], outputFileName, "WriteMode", "append");
    %}

    % remove every second (helper) max from plot
    peakTimes(2:2:end) = [];
    peakValues(2:2:end) = [];


    % plot results
    figure()
    sgtitle(outputFileName, 'Interpreter', 'none');
    set(gcf,'Position',[100 100 1250 750])

    subplot(2,1,1)
    hold on
    plot(takeoffTime, takeoffForce, '*', 'Color', [0.8500 0.3250 0.0980])
    plot(landingTime, landingForce, '*', 'Color', [0.9290 0.6940 0.1250])
    plot(tableData.time, netForce, 'Color', [0 0.4470 0.7410])
    hold off
    title("combinedForce")
    legend('Takeoff', 'Landing')
    ylabel("Force (N)")
    xlabel("Time (s)")

    subplot(2,1,2)
    hold on
    plot(peakTimes, peakValues, '*', 'Color', [0.8500 0.3250 0.0980])
    plot(zeroTimes, zeroValues, '*', 'Color', [0.4940 0.1840 0.5560])
    for pt = 1:length(propTimes)
        plot(cell2mat(propTimes{:,pt}), cell2mat(propPowers{:,pt}), 'LineWidth', 1.5, 'Color', '#77AC30')            
    end
    plot(tableData.time, netPower, 'Color', [0 0.4470 0.7410])
    hold off
    ylabel("Power (W)")
    xlabel("Time (s)")
    title("netPower");
    legend('Max power', 'Beginning propulsion', 'Propulsion phase', 'Location', 'southeast')
    
    plotFileName = sprintf( 'Data/output/CMJ 1x20/%s/%s_plot.eps', trialRef, splitFileName{1} );
    saveas(gcf,plotFileName,'epsc')
end