%% Function to determine when the foot is stationary on the floor.
% inputs:
% acc - Nx3 matrix with acceleration values.
% gyr - Nx# matrix with gyroscope values.

% outputs:
% tStart - indexes of initial contacts.
% tEnd - indexes of toe-off.
% contactVector - vector with indicators of contact.

function  [tStart, tEnd, contactVector] = zvt(acc,gyr)

% Tranform gyroscope data in degrees/second.
gyrDeg=gyr*(180/pi); 

% Obtain the magnitudes of acceleration and angular velocity.
mgyr=sqrt(sum(gyrDeg.^2,2));
macc=sqrt(sum(acc.^2,2));

% Initialize N, k and v.
N = length(mgyr);
contactVector = zeros(1,length(mgyr));
varianceWindow = zeros(1,length(mgyr));

% loop over all samples starting at 11 for the first window.
for i=11:N
    % Take a snapshot of the acceleration data and average it.
    maccWindow=macc(i-10:i-1); 
    avgmaccWindow=sum(maccWindow)/10;

    % Calculate the variance within that window.
    varianceWindow(i-10)=sqrt(sum((maccWindow-avgmaccWindow).^2)/10);

    % If the variance is small, the gyroscope signal is small and the
    % acceleration magnitude is close to gravity the foot should be
    % stationary. For detailed explanation of the conditions see the
    % publication: https://doi.org/10.1186/s12984-021-00816-4
    if varianceWindow(i-10)<0.5 && mgyr(i-10)<50 && (9<macc(i-10) && macc(i-10)<11)
        contactVector(i-10)=0;
    else
        contactVector(i-10)=1;
    end
end

% Differentiate the contact vector to find indices of the contacts and
% double check that the duration of contact is sufficient (10 samples).
x=diff(contactVector);
dx=find(abs(x)==1);

% Correct the contact windows shorter than 10 samples to non-contact.
for i=1:length(dx)-1
    if (dx(i+1)-dx(i))<10
        contactVector(dx(i):dx(i+1))=contactVector(dx(i)-1);
    end
end

% Recalculate the moments of contact to get indices.
dx=(diff(contactVector));
tStart=find(dx==1);
tEnd=find(dx==-1);
end