function [locsTurns,FilteredData,FootContacts] = ControlsDetermineLocationTurnsFunc(inputData,FS,ApplyRealignment,plotit,ResultStruct)
% Description: Determine the location of turns, plot for visual inspection

% Input: Acc Data (not yet realigned)

%Realign sensor data to VT-ML-AP frame

if ApplyRealignment % apply relignment as described in Rispens S, Pijnappels M, van Schooten K, Beek PJ, Daffertshofer A, van Die?n JH (2014).
    data = inputData(:,[3,2,1]); % NOT THE SAME AS IN THE CLBP DATA; reorder data to 1 = V; 2 = ML; 3 = AP
    % Consistency of gait characteristics as determined from acceleration data collected at different trunk locations. Gait Posture 2014;40(1):187-92.
    [RealignedAcc, ~] = RealignSensorSignalHRAmp(data, FS);
    dataAcc = RealignedAcc;
else
    data = inputData; % Realignment performed in previous stage
    dataAcc = inputData;
end

% Filter data
[B,A] = butter(2,20/(FS/2),'low'); % Filters data very strongly which is needed to determine turns correctly
dataStepDetection = filtfilt(B,A,dataAcc);
VTAcc = data(:,1);
APAcc = data(:,3);

%% Determine Steps -
% USE THE AP ACCELERATION: WIEBREN ZIJLSTRA: ASSESSMENT OF SPATIO-TEMPORAL PARAMTERS DURING UNCONSTRAINED WALKING (2004)

% In order to run the step detection script we first need to run an autocorrelation function;
StrideTimeRange = [0.2 4.0];       % Range to search for stride time (seconds)

[ResultStruct] = AutocorrStrides(dataStepDetection,FS, StrideTimeRange,ResultStruct); % first guess of stridetimesamples
    
% StrideTimeSamples is needed as an input for the stepcountFunc;
StrideTimeSamples = ResultStruct.StrideTimeSamples;

[PksAndLocsCorrected] = StepcountFunc(data,StrideTimeSamples,FS);
FootContacts = PksAndLocsCorrected; % (1:2:end,2); previous version 
numStepsOption2_filt = numel(FootContacts);                                                               % counts number of steps;

%% Determine Turns
% To convert the XYZ acceleration vectors at each point in time into scalar values,
% calculate the magnitude of each vector. This way, you can detect large changes in overall acceleration,
% such as steps taken while walking, regardless of device orientation.
magfilt = sqrt(sum((dataStepDetection(:,1).^2) + (dataStepDetection(:,2).^2) + (dataStepDetection(:,3).^2), 2));
magNoGfilt = magfilt - mean(magfilt);
minPeakHeight2 = 1*std(magNoGfilt); % used to be 2                                               % based on visual inspection, parameter tuning was performed on *X*standard deviation from MInPeak (used to be 1 SD)
[pks, locs] = findpeaks(magNoGfilt, 'MINPEAKHEIGHT', minPeakHeight2,'MINPEAKDISTANCE',0.3*FS);   % for step detection
numStepsOption2_filt = numel(pks);                                                               % counts number of locs for turn detection

diffLocs = diff(locs);          % calculates difference in location magnitude differences
avg_diffLocs = mean(diffLocs);  % average distance between steps
std_diffLocs = std(diffLocs);   % standard deviation of distance between steps

figure;
findpeaks(diffLocs, 'MINPEAKHEIGHT', avg_diffLocs, 'MINPEAKDISTANCE',0.2*FS);                                    % these values have been chosen based on visual inspection of the signal
line([1 length(diffLocs)],[avg_diffLocs avg_diffLocs])
[pks_diffLocs, locs_diffLocs] = findpeaks(diffLocs, 'MINPEAKHEIGHT', avg_diffLocs,'MINPEAKDISTANCE',0.2*FS);     % values were initially 5
locsTurns =  [locs(locs_diffLocs),  locs(locs_diffLocs+1)];

%magNoGfilt_copy = magNoGfilt;
VTAcc_copy = data(:,1);
APAcc_copy = data(:,3);
for k = 1: size(locsTurns,1);
    VTAcc_copy(locsTurns(k,1):locsTurns(k,2)) = NaN;
    APAcc_copy(locsTurns(k,1):locsTurns(k,2)) = NaN;
end

% Visualising signal;
if plotit
    figure()
    subplot(2,1,1)
    hold on;
    plot(VTAcc,'b')
    plot(VTAcc_copy, 'r');
    title('Inside Straight: Filtered data VT with turns highlighted in blue')
    line([6000,12000,18000,24000,30000,36000;6000,12000,18000,24000,30000,36000],[5,5,5,5,5,5;10,10,10,10,10,10],'LineWidth',2,'Linestyle','--','color','k')
    hold on;
    subplot(2,1,2)
    plot(APAcc,'g')
    plot(APAcc_copy, 'r');
    title('Inside Straight: Filtered data AP with turns highlighted in blue')
    line([6000,12000,18000,24000,30000,36000;6000,12000,18000,24000,30000,36000],[0,0,0,0,0,0;10,10,10,10,10,10],'LineWidth',2,'Linestyle','--','color','k')
    hold off;
end

% Check if number of turns * 20 m are making sense based on total
% distance measured by researcher.
disp(['Number of turns detected = ' num2str(size(locsTurns,1))])
%disp(['Total distance measured by researcher was = ' num2str(Distance)])

FilteredData = dataAcc;
end