generateCostTermStruct.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function returns all of the cost term calculation methods including
% user_defined and existing cost term values. Tools use this function for
% the discrete and continuous cost calculations.
%
% inputs:
% costTermType - one of ["discrete", "continuous"]
% toolName - one of ["TrackingOptimization", "TreatmentOptimization", ...
%   "DesignOptimization"]
%
% (string, string) -> (struct of function handles, Array of string)
%


function [costTermCalculations, allowedTypes] = ...
    generateCostTermStruct(costTermType, toolName)
allowedTypes = getAllowedTypes(costTermType, toolName);
costTermCalculations = getCostTermCalculations(costTermType);
end

function allowedTypes = getAllowedTypes(costTermType, toolName)
if strcmp(costTermType, "continuous")
    switch toolName
        case "TrackingOptimization"
            allowedTypes = [ ...
                "coordinate_tracking", ...
                "inverse_dynamics_load_tracking", ...
                "external_force_tracking", ...
                "external_moment_tracking", ...
                "muscle_activation_tracking", ...
                "joint_jerk_minimization", ...
                ];
        case "VerificationOptimization"
            allowedTypes = [ ...
                "coordinate_tracking", ...
                "controller_tracking", ...
                "joint_jerk_minimization", ...
            ];
        case "DesignOptimization"
            allowedTypes = [ ...
                "coordinate_tracking", ...
                "controller_tracking", ...
                "joint_jerk_minimization", ...
                "metabolic_cost_minimization" ...
                "propulsive_force_maximization" ...
                "propulsive_force_minimization" ...
                "breaking_force_maximization" ...
                "breaking_force_minimization" ...
                "step_length_maximization" ... 
                "step_length_asymmetry_minimization" ...
                "single_support_time_maximization" ...
                "single_support_time_goal" ...
                "step_time_asymmetry_minimization" ...
                "joint_power_minimization" ...
                "trailing_limb_angle_minimization" ...
                "trailing_limb_angle_maximization" ...
                "muscle_activation_minimization" ...
                "muscle_activation_maximization" ...
                "center_mass_velocity_x_minimization" ...
                "center_mass_velocity_y_minimization" ...
                "center_mass_velocity_z_minimization" ...
                "kinematic_symmetry" ...
                "walking_speed_goal" ...
                "external_torque_control_minimization" ...
                "user_defined", ...
                ];
        otherwise
            throw(MException('', ['Tool name' toolName 'is not valid']))
    end
elseif strcmp(costTermType, "discrete")
    switch toolName
        case "TrackingOptimization"
            allowedTypes = [ ...

            ];
        case "VerificationOptimization"
            allowedTypes = [ ...

            ];
        case "DesignOptimization"
            allowedTypes = [ ...
                "synergy_vector_tracking" ...
                "belt_speed_goal" ...
                "user_defined" ...
                ];
        otherwise
            throw(MException('', ['Tool name' toolName 'is not valid']))
    end
else
    throw(MException('', ['Cost term type ' costTermType ...
        ' is not valid, must be continuous or discrete']))
end
end

function costTermCalculations = getCostTermCalculations(costTermType)

costTermCalculations.coordinate_tracking = @(values, modeledValues, auxdata, costTerm) ...
    calcTrackingCoordinateIntegrand( ...
    auxdata, ...
    values.time, ...
    values.statePositions, ...
    costTerm.coordinate ...
    );

costTermCalculations.controller_tracking = @(values, modeledValues, auxdata, costTerm) ...
    calcTrackingControllerIntegrand( ...
    auxdata, ...
    values, ...
    values.time, ...
    costTerm.controller ...
    );

costTermCalculations.joint_jerk_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcMinimizingJointJerkIntegrand( ...
    values.controlJerks, ...
    auxdata, ...
    costTerm.coordinate ...
    );

costTermCalculations.metabolic_cost_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcMinimizingMetabolicCost( ...
    values, ...
    modeledValues, ...
    auxdata);

costTermCalculations.inverse_dynamics_load_tracking = @(values, modeledValues, auxdata, costTerm) ...
    calcTrackingInverseDynamicLoadsIntegrand( ...
    auxdata, ...
    values.time, ...
    modeledValues.inverseDynamicMoments, ...
    costTerm.load ...
    );

costTermCalculations.external_force_tracking = @(values, modeledValues, auxdata, costTerm) ...
    calcTrackingExternalForcesIntegrand( ...
    auxdata, ...
    modeledValues.groundReactionsLab.forces, ...
    values.time, ...
    costTerm.force);

costTermCalculations.external_moment_tracking = @(values, modeledValues, auxdata, costTerm) ...
    calcTrackingExternalMomentsIntegrand( ...
    auxdata, ...
    modeledValues.groundReactionsLab.moments, ...
    values.time, ...
    costTerm.moment);

costTermCalculations.muscle_activation_tracking = @(values, modeledValues, auxdata, costTerm) ...
    calcTrackingMuscleActivationIntegrand( ...
    modeledValues.muscleActivations, ...
    values.time, ...
    auxdata, ...
    costTerm.muscle);

costTermCalculations.propulsive_force_maximization = @(values, modeledValues, auxdata, costTerm) ...
    calcMaximizingPropulsiveForceIntegrand( ...
    modeledValues, ...
    auxdata, ...
    costTerm);

costTermCalculations.propulsive_force_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcMinimizingPropulsiveForceIntegrand( ...
    modeledValues, ...
    auxdata, ...
    costTerm);

costTermCalculations.breaking_force_maximization = @(values, modeledValues, auxdata, costTerm) ...
    calcMaximizingBreakingForceIntegrand( ...
    modeledValues, ...
    auxdata, ...
    costTerm);

costTermCalculations.breaking_force_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcMinimizingBreakingForceIntegrand( ...
    modeledValues, ...
    auxdata, ...
    costTerm);

costTermCalculations.step_length_maximization = @(values, modeledValues, auxdata, costTerm) ...
    calcMaximizingStepLengthIntegrand( ...
    values, ...
    modeledValues, ...
    auxdata, ...
    costTerm);

costTermCalculations.step_length_asymmetry_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcStepLengthAsymmetryIntegrand( ...
    values, ...
    modeledValues, ...
    auxdata, ...
    costTerm);

costTermCalculations.single_support_time_maximization = @(values, modeledValues, auxdata, costTerm) ...
    calcMaximizingSingleSupportTimeIntegrand( ...
    values, ...
    modeledValues, ...
    auxdata, ...
    costTerm);


costTermCalculations.single_support_time_goal = @(values, modeledValues, auxdata, costTerm) ...
    calcGoalSingleSupportTimeIntegrand( ...
    values, ...
    modeledValues, ...
    auxdata, ...
    costTerm);

costTermCalculations.step_time_asymmetry_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcStepTimeAsymmetryIntegrand( ...
    values, ...
    modeledValues, ...
    auxdata, ...
    costTerm);

costTermCalculations.joint_power_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcMinimizingJointPowerIntegrand( ...
    values.stateVelocities, ...
    modeledValues.inverseDynamicMoments, ...
    auxdata, ...
    costTerm.load ...
    );

costTermCalculations.trailing_limb_angle_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcMinimizingTrailingLimbAngleIntegrand( ...
    values, ...
    modeledValues, ...
    auxdata, ...
    costTerm ...
    );

costTermCalculations.trailing_limb_angle_maximization = @(values, modeledValues, auxdata, costTerm) ...
    calcMaximizingTrailingLimbAngleIntegrand( ...
    values, ...
    modeledValues, ...
    auxdata, ...
    costTerm ...
    );

costTermCalculations.muscle_activation_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcMinimizingMuscleActivationIntegrand( ...
    modeledValues.muscleActivations, ...
    auxdata, ...
    costTerm.muscle ...
    );

costTermCalculations.muscle_activation_maximization = @(values, modeledValues, auxdata, costTerm) ...
    calcMaximizingMuscleActivationIntegrand( ...
    modeledValues.muscleActivations, ...
    auxdata, ...
    costTerm.muscle ...
    );

costTermCalculations.center_mass_velocity_x_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcMinimizingMassCenterVelocityXIntegrand( ...
    values, ...
    auxdata);

costTermCalculations.center_mass_velocity_y_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcMinimizingMassCenterVelocityYIntegrand( ...
    values, ...
    auxdata);

costTermCalculations.center_mass_velocity_z_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcMinimizingMassCenterVelocityZIntegrand( ...
    values, ...
    auxdata);

costTermCalculations.kinematic_symmetry = @(values, modeledValues, auxdata, costTerm) ...
    calcKinematicSymmetryIntegrand( ...
    values.statePositions, ...
    auxdata, ...
    costTerm);

costTermCalculations.walking_speed_goal = @(values, modeledValues, auxdata, costTerm) ...
    calcGoalWalkingSpeedIntegrand( ...
    values, ...
    modeledValues, ...
    auxdata, ...
    costTerm);

costTermCalculations.external_torque_control_minimization = @(values, modeledValues, auxdata, costTerm) ...
    calcMinimizingExternalTorqueControl( ...
    values.externalTorqueControls, ...
    auxdata, ...
    costTerm.coordinate);
    
costTermCalculations.synergy_vector_tracking = @(values, modeledValues, auxdata, costTerm) ...
    calcTrackingSynergyVectorsDiscrete( ...
    values.synergyWeights, ...
    auxdata, ...
    costTerm);

costTermCalculations.belt_speed_goal = @(values, modeledValues, auxdata, costTerm) ...
    calcGoalBeltSpeedDiscrete( ...
    values, ...
    auxdata, ...
    costTerm);

costTermCalculations.user_defined = @(values, modeledValues, auxdata, costTerm) ...
    userDefinedFunction(values, ...
    modeledValues, ...
    auxdata, ...
    costTerm, ...
    costTermType);
end

function output =  ...
    userDefinedFunction(values, modeledValues, auxdata, costTerm, costTermType)
output = [];
if strcmp(costTerm.cost_term_type, costTermType)
    fn = str2func(costTerm.function_name);
    output = fn(values, modeledValues, auxdata, costTerm);
end
end