/* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc */
void InitilizeAccForEBDFootCalc_CalcBrakingCurves_Pkg_internalOperators(
    /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::target */Target_real_T_TargetManagement_types *target,
    /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::aSafe */ASafe_T_CalcBrakingCurves_types *aSafe,
    /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::BCACCForEBDFoot */BCAccumulator_type_CalcBrakingCurves_Pkg *BCACCForEBDFoot)
{
    /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::_L9 */
    static L_internal_real_Type_SDM_Types_Pkg _L9;
    /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::_L15 */
    static kcg_real _L15;

    (*BCACCForEBDFoot).distance = (*target).distance;
    _L15 = /* 2 */ dV_ebi_for_V_SDM_Types_Pkg((*target).speed) + (*target).speed;
    (*BCACCForEBDFoot).speed = _L15;
    /* 3 */
    getDistanceStepFromDistance_CalcBrakingCurves_Pkg_Step_Function_operators(
        (*target).distance,
        &(*aSafe).distance_definition,
        &_L9,
        &(*BCACCForEBDFoot).distanceIndex);
    /* 3 */
    getSpeedStepFromSpeed_CalcBrakingCurves_Pkg_Step_Function_operators(
        _L15,
        &(*aSafe).speed_definition,
        &_L9,
        &(*BCACCForEBDFoot).speedIndex);
    /* 2 */
    makeEmptyParabolaCurve_CalcBrakingCurves_types(&(*BCACCForEBDFoot).BC);
}
/* SDM_Types_Pkg::CalcSpeeds */
void CalcSpeeds_SDM_Types_Pkg(
  /* SDM_Types_Pkg::CalcSpeeds::odometry */ Odometry_real_T_SDM_Types_Pkg *odometry,
  /* SDM_Types_Pkg::CalcSpeeds::v_MRSP */ V_internal_real_Type_SDM_Types_Pkg v_MRSP,
  /* SDM_Types_Pkg::CalcSpeeds::v_release */ V_internal_real_Type_SDM_Types_Pkg v_release,
  /* SDM_Types_Pkg::CalcSpeeds::v_target */ V_internal_real_Type_SDM_Types_Pkg v_target,
  /* SDM_Types_Pkg::CalcSpeeds::mrdt */ Target_real_T_TargetManagement_types *mrdt,
  /* SDM_Types_Pkg::CalcSpeeds::v_p_ebd */ V_internal_real_Type_SDM_Types_Pkg v_p_ebd,
  /* SDM_Types_Pkg::CalcSpeeds::v_p_sbd */ V_internal_real_Type_SDM_Types_Pkg v_p_sbd,
  /* SDM_Types_Pkg::CalcSpeeds::v_p_ebd_valid */ kcg_bool v_p_ebd_valid,
  /* SDM_Types_Pkg::CalcSpeeds::v_sbi_mrdt */ V_internal_real_Type_SDM_Types_Pkg v_sbi_mrdt,
  /* SDM_Types_Pkg::CalcSpeeds::speeds */ Speeds_T_SDM_Types_Pkg *speeds)
{
  (*speeds).V_est = (*odometry).speed.v_safeNominal;
  (*speeds).V_MRSP = v_MRSP;
  (*speeds).V_release = v_release;
  (*speeds).V_target = v_target;
  (*speeds).v_sbi_mrdt = v_sbi_mrdt;
  (*speeds).v_p_mrdt = /* 1 */
    P_SpeedRelatedToMRDT_SDM_Types_Pkg(
      v_p_ebd,
      v_p_sbd,
      v_target,
      v_p_ebd_valid);
  (*speeds).v_FLOI = /* 1 */
    FloiSpeed_SDM_Types_Pkg(
      v_sbi_mrdt,
      v_target,
      v_MRSP,
      v_release,
      (*mrdt).targetType);
  (*speeds).dV_warning_V_MRSP = /* 1 */ dV_warning_for_V_SDM_Types_Pkg(v_MRSP);
  (*speeds).dV_warning_V_target = /* 2 */
    dV_warning_for_V_SDM_Types_Pkg(v_target);
  (*speeds).dV_sbi_V_MRSP = /* 1 */ dV_sbi_for_V_SDM_Types_Pkg(v_MRSP);
  (*speeds).dV_sbi_V_target = /* 2 */ dV_sbi_for_V_SDM_Types_Pkg(v_target);
  (*speeds).dV_ebi_V_MRSP = /* 1 */ dV_ebi_for_V_SDM_Types_Pkg(v_MRSP);
  (*speeds).dV_ebi_V_target = /* 2 */ dV_ebi_for_V_SDM_Types_Pkg(v_target);
}
/* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc */
void InitilizeAccForEBDFootCalc_CalcBrakingCurves_Pkg_internalOperators(
  /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::target */ Target_real_T_TargetManagement_types *target,
  /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::aSafe */ ASafe_T_CalcBrakingCurves_types *aSafe,
  /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::BCACCForEBDFoot */ BCAccumulator_type_CalcBrakingCurves_Pkg *BCACCForEBDFoot)
{
  kcg_int i;
  /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::_L4 */ ParabolaCurveValid_T_CalcBrakingCurves_types _L4;
  /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::_L5 */ array_real_25 _L5;
  /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::_L11 */ kcg_real _L11;
  /* CalcBrakingCurves_Pkg::internalOperators::InitilizeAccForEBDFootCalc::_L15 */ kcg_real _L15;
  
  (*BCACCForEBDFoot).distance = (*target).distance;
  _L15 = /* 2 */ dV_ebi_for_V_SDM_Types_Pkg((*target).speed) + (*target).speed;
  (*BCACCForEBDFoot).speed = _L15;
  /* 3 */
  getDistanceStepFromDistance_CalcBrakingCurves_Pkg_Step_Function_operators(
    (*target).distance,
    &(*aSafe).distance_definition,
    &_L11,
    &(*BCACCForEBDFoot).distanceIndex);
  /* 3 */
  getSpeedStepFromSpeed_CalcBrakingCurves_Pkg_Step_Function_operators(
    _L15,
    &(*aSafe).speed_definition,
    &_L11,
    &(*BCACCForEBDFoot).speedIndex);
  _L11 = - 1.0;
  for (i = 0; i < 25; i++) {
    _L5[i] = _L11;
    _L4[i] = kcg_false;
  }
  kcg_copy_ParabolaCurveSpeeds_T_CalcBrakingCurves_types(
    &(*BCACCForEBDFoot).BC.speeds,
    &_L5);
  kcg_copy_ParabolaCurveAccelerations_T_CalcBrakingCurves_types(
    &(*BCACCForEBDFoot).BC.accelerations,
    &(*BCACCForEBDFoot).BC.speeds);
  kcg_copy_ParabolaCurveDistances_T_CalcBrakingCurves_types(
    &(*BCACCForEBDFoot).BC.distances,
    &_L5);
  kcg_copy_ParabolaCurveValid_T_CalcBrakingCurves_types(
    &(*BCACCForEBDFoot).BC.valid,
    &_L4);
}