calcNcpCost.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function takes the necessary inputs and produces the results of IK,
% ID, and MuscleAnalysis so the values can be used as inputs for
% MuscleTendonPersonalization.
%
% (struct, struct) -> (None)
% Prepares raw data for MuscleTendonPersonalization


function cost = calcNcpCost(activations, inputs, params, values)

error = [];
% Split activations into subsets ahead of cost computation
if isfield(inputs, 'mtpActivationsColumnNames')
    [activationsWithMtpData, activationsWithoutMtpData] = ...
        makeMtpActivatonSubset(activations, ...
        inputs.mtpActivationsColumnNames, inputs.muscleTendonColumnNames);
else
    activationsWithoutMtpData = activations;
end
for term = 1:length(params.costTerms)
    costTerm = params.costTerms{term};
    if costTerm.isEnabled
        switch costTerm.type
            case "moment_tracking"
                [normalizedFiberLengths, normalizedFiberVelocities] = ...
                    calcNormalizedMuscleFiberLengthsAndVelocities( ...
                    inputs, inputs.optimalFiberLengthScaleFactors, ...
                    inputs.tendonSlackLengthScaleFactors);
                muscleJointMoments = calcMuscleJointMoments(inputs, ...
                    activations, normalizedFiberLengths, ...
                    normalizedFiberVelocities);
                rawCost = muscleJointMoments - ...
                    inputs.inverseDynamicsMoments;
            case "activation_tracking"
                if isfield(inputs, 'mtpActivations')
                    rawCost = activationsWithMtpData - inputs.mtpActivations;
                else
                    rawCost = 0;
                end
            case "activation_minimization"
                errorCenter = valueOrAlternate(costTerm, "errorCenter", 0);
                rawCost = reshape(activationsWithoutMtpData, [], 1) - errorCenter;
            case "grouped_activations"
                rawCost = calcGroupedActivationCost(activations, ...
                    inputs, params);
            case "grouped_fiber_lengths"
                rawCost = calcGroupedNormalizedFiberLengthCost( ...
                    activations, inputs, params);
            case "bilateral_symmetry"
                if length(inputs.synergyGroups) ~= 2
                    throw(MException('', ['Bilateral symmetry cost ' ...
                        'requires exactly two synergy groups.']))
                end
                weights = findSynergyWeightsByGroup(values, inputs);
                rawCost = weights(1, :, :) - weights(2, :, :);
            otherwise
                throw(MException('', ['Cost term type ' costTerm.type ...
                    ' does not exist for this tool.']))
        end
        error = [error; (rawCost(:) / costTerm.maxAllowableError) / ...
            sqrt(numel(rawCost))];
    end
end

cost = error' * error;
end