calcGoalSingleSupportTimeIntegrand.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function tracks the difference between the predicted single spport
% phase and the error center for the specified foot. On average, single 
% support phase is around 0.37 to 0.39 of the gait cycle. 
%
% (struct, struct, struct, struct) -> (Array of number)


function cost = calcGoalSingleSupportTimeIntegrand(values, ...
    modeledValues, params, costTerm)

errorCenter = valueOrAlternate(costTerm, "error_center", 0.38);
for i = 1:length(params.contactSurfaces)
    if params.contactSurfaces{i}.isLeftFoot == costTerm.is_left_foot
        if i == 1
            singleSupportTime = calcSingleSupportTime( ...
                modeledValues.groundReactionsLab.forces{i + 1}(:, 2), ...
                values.time);
        else
            singleSupportTime = calcSingleSupportTime( ...
                modeledValues.groundReactionsLab.forces{i - 1}(:, 2), ...
                values.time);
        end
    end
end
percentSingleSupportTime = singleSupportTime / values.time(end);
cost = calcTrackingCostArrayTerm(percentSingleSupportTime * ...
    ones(length(values.time), 1), errorCenter * ...
    ones(length(values.time), 1), 1);
end