#include "workshop_orchestrator_v2/plan_builder.hpp"

namespace workshop_orchestrator_v2
{

using calibration_vehicle_profile_interfaces::msg::CalibrationAbilityType;
using calibration_vehicle_profile_interfaces::msg::CameraMountType;
using calibration_vehicle_profile_interfaces::msg::SensorProfile;
using calibration_vehicle_profile_interfaces::msg::SensorType;
using calibration_workshop_orchestration_interfaces::msg::StageExecutionPolicy;

namespace
{

void add_metadata_if_missing(StagePlan & stage, const std::string & key, const std::string & value)
{
  for (const auto & kv : stage.metadata) {
    if (kv.key == key) {
      return;
    }
  }
  calibration_common_interfaces::msg::KeyValuePair kv;
  kv.key = key;
  kv.value = value;
  stage.metadata.push_back(kv);
}

const SensorProfile * find_default_sensor(
  const VehicleProfile & profile,
  bool require_intrinsic,
  bool require_extrinsic)
{
  for (const auto & sensor : profile.sensors) {
    if (!sensor.enabled || !sensor.selected_for_this_session) {
      continue;
    }
    if (require_intrinsic &&
      (!sensor.needs_intrinsic_calibration || !sensor.supports_intrinsic_calibration))
    {
      continue;
    }
    if (require_extrinsic &&
      (!sensor.needs_extrinsic_calibration || !sensor.supports_extrinsic_calibration))
    {
      continue;
    }
    return &sensor;
  }

  for (const auto & sensor : profile.sensors) {
    if (!sensor.enabled) {
      continue;
    }
    if (require_intrinsic && !sensor.supports_intrinsic_calibration) {
      continue;
    }
    if (require_extrinsic && !sensor.supports_extrinsic_calibration) {
      continue;
    }
    return &sensor;
  }

  return nullptr;
}

std::string resolve_intrinsic_subtype(const SensorProfile & sensor)
{
  if (sensor.sensor_type.value == SensorType::IMU) {
    return "imu_intrinsic";
  }
  if (sensor.camera_mount_type.value == CameraMountType::DOWNWARD_MOUNTED ||
    sensor.sensor_type.value == SensorType::DOWNWARD_CAMERA)
  {
    return "downward_camera_intrinsic";
  }
  return "front_camera_intrinsic";
}

std::string resolve_extrinsic_subtype(const SensorProfile & sensor)
{
  if (sensor.sensor_type.value == SensorType::IMU) {
    return "imu_extrinsic";
  }
  if (sensor.sensor_type.value == SensorType::LIDAR_2D) {
    return "lidar_2d_extrinsic";
  }
  if (sensor.sensor_type.value == SensorType::LIDAR_3D) {
    return "lidar_3d_extrinsic";
  }
  if (sensor.camera_mount_type.value == CameraMountType::DOWNWARD_MOUNTED ||
    sensor.sensor_type.value == SensorType::DOWNWARD_CAMERA)
  {
    return "downward_camera_extrinsic";
  }
  return "front_camera_extrinsic";
}

std::string resolve_hand_eye_subtype(const SensorProfile & sensor)
{
  if (sensor.camera_mount_type.value == CameraMountType::EYE_TO_HAND) {
    return "eye_to_hand";
  }
  return "eye_in_hand";
}

void apply_default_external_metadata(StagePlan & stage)
{
  add_metadata_if_missing(stage, metadata_keys::EXTERNAL_STATIC_SAMPLE_COUNT, "10");
  add_metadata_if_missing(stage, metadata_keys::EXTERNAL_DYNAMIC_SAMPLE_COUNT, "20");
  add_metadata_if_missing(stage, metadata_keys::EXTERNAL_REQUIRE_SHORT_MOTION_SEGMENT, "true");
  add_metadata_if_missing(stage, metadata_keys::EXTERNAL_MAX_POSITION_STDDEV_M, "0.05");
  add_metadata_if_missing(stage, metadata_keys::EXTERNAL_MAX_YAW_STDDEV_RAD, "0.05");
  add_metadata_if_missing(stage, metadata_keys::EXTERNAL_MAX_TRACKING_LOSS_RATIO, "0.05");
  add_metadata_if_missing(stage, metadata_keys::EXTERNAL_MAX_TIME_SYNC_OFFSET_MS, "50.0");
  add_metadata_if_missing(stage, metadata_keys::EXTERNAL_TIMEOUT_SEC, "30.0");
}

void apply_default_chassis_metadata(StagePlan & stage)
{
  add_metadata_if_missing(stage, metadata_keys::CHASSIS_PRIMITIVE_TYPE, "straight_line");
  add_metadata_if_missing(stage, metadata_keys::CHASSIS_STRAIGHT_LINE_DISTANCE_M, "2.0");
  add_metadata_if_missing(stage, metadata_keys::CHASSIS_STRAIGHT_LINE_SPEED_MS, "0.5");
  add_metadata_if_missing(stage, metadata_keys::COMMON_BRAKE_WHEN_FINISHED, "true");
  add_metadata_if_missing(stage, metadata_keys::COMMON_TIMEOUT_SEC, "60.0");
}

void apply_default_control_metadata(StagePlan & stage)
{
  add_metadata_if_missing(stage, metadata_keys::CONTROL_TASK_TYPE, "trajectory_tracking");
  add_metadata_if_missing(stage, metadata_keys::CONTROL_STOP_AT_END, "true");
  add_metadata_if_missing(stage, metadata_keys::CONTROL_TIMEOUT_SEC, "60.0");
  add_metadata_if_missing(stage, std::string(metadata_keys::CONTROL_TRAJECTORY_PREFIX) + "0_x_m", "0.0");
  add_metadata_if_missing(stage, std::string(metadata_keys::CONTROL_TRAJECTORY_PREFIX) + "0_y_m", "0.0");
  add_metadata_if_missing(stage, std::string(metadata_keys::CONTROL_TRAJECTORY_PREFIX) + "0_yaw_rad", "0.0");
  add_metadata_if_missing(stage, std::string(metadata_keys::CONTROL_TRAJECTORY_PREFIX) + "0_speed_ms", "0.3");
  add_metadata_if_missing(stage, std::string(metadata_keys::CONTROL_TRAJECTORY_PREFIX) + "1_x_m", "2.0");
  add_metadata_if_missing(stage, std::string(metadata_keys::CONTROL_TRAJECTORY_PREFIX) + "1_y_m", "0.0");
  add_metadata_if_missing(stage, std::string(metadata_keys::CONTROL_TRAJECTORY_PREFIX) + "1_yaw_rad", "0.0");
  add_metadata_if_missing(stage, std::string(metadata_keys::CONTROL_TRAJECTORY_PREFIX) + "1_speed_ms", "0.3");
}

void apply_default_sensor_intrinsic_metadata(StagePlan & stage, const VehicleProfile & profile)
{
  const auto * sensor = find_default_sensor(profile, true, false);
  if (!sensor) {
    return;
  }

  add_metadata_if_missing(stage, metadata_keys::SENSOR_ID, sensor->sensor_id);
  const auto subtype = resolve_intrinsic_subtype(*sensor);
  add_metadata_if_missing(stage, metadata_keys::SENSOR_TASK_SUBTYPE, subtype);

  if (subtype == "imu_intrinsic") {
    add_metadata_if_missing(stage, metadata_keys::IMU_INTRINSIC_REQUIRED_STATIC_SEGMENT_COUNT, "5");
    add_metadata_if_missing(stage, metadata_keys::IMU_INTRINSIC_REQUIRED_MOTION_SEGMENT_COUNT, "5");
    add_metadata_if_missing(stage, metadata_keys::IMU_INTRINSIC_TIMEOUT_SEC, "120.0");
    return;
  }

  add_metadata_if_missing(stage, metadata_keys::CAMERA_INTRINSIC_REQUIRED_IMAGE_COUNT, "20");
  add_metadata_if_missing(stage, metadata_keys::CAMERA_INTRINSIC_TIMEOUT_SEC, "120.0");
}

void apply_default_sensor_extrinsic_metadata(StagePlan & stage, const VehicleProfile & profile)
{
  const auto * sensor = find_default_sensor(profile, false, true);
  if (!sensor) {
    return;
  }

  add_metadata_if_missing(stage, metadata_keys::SENSOR_ID, sensor->sensor_id);
  add_metadata_if_missing(stage, metadata_keys::SENSOR_TASK_SUBTYPE, resolve_extrinsic_subtype(*sensor));
  add_metadata_if_missing(stage, metadata_keys::SENSOR_EXTRINSIC_BASE_FRAME_ID,
    profile.base_link_frame.empty() ? "base_link" : profile.base_link_frame);
  add_metadata_if_missing(stage, metadata_keys::SENSOR_EXTRINSIC_REQUIRED_SAMPLE_COUNT, "10");
  add_metadata_if_missing(stage, metadata_keys::SENSOR_EXTRINSIC_TIMEOUT_SEC, "180.0");
}

void apply_default_hand_eye_metadata(StagePlan & stage, const VehicleProfile & profile)
{
  if (!profile.arm_profile.has_mechanical_arm || profile.arm_profile.arm_id.empty()) {
    return;
  }

  const auto * sensor = find_default_sensor(profile, false, true);
  if (!sensor) {
    return;
  }

  add_metadata_if_missing(stage, metadata_keys::SENSOR_ID, sensor->sensor_id);
  add_metadata_if_missing(stage, metadata_keys::SENSOR_TASK_SUBTYPE, resolve_hand_eye_subtype(*sensor));
  add_metadata_if_missing(stage, metadata_keys::HAND_EYE_ARM_ID, profile.arm_profile.arm_id);
  add_metadata_if_missing(stage, metadata_keys::HAND_EYE_REQUIRED_POSE_COUNT, "12");
  add_metadata_if_missing(stage, metadata_keys::HAND_EYE_TIMEOUT_SEC, "180.0");
}

}  // namespace

bool PlanBuilder::has_capability(const VehicleProfile & profile, uint8_t ability_type) const
{
  for (const auto & cap : profile.capabilities) {
    if (cap.ability_type.value == ability_type && cap.supported) {
      return true;
    }
  }
  return false;
}

const RequestedCalibrationTask * PlanBuilder::find_requested_task(
  const WorkshopSessionConfig & config,
  uint8_t stage_type) const
{
  for (const auto & task : config.requested_tasks) {
    if (task.stage_type.value == stage_type) {
      return &task;
    }
  }
  return nullptr;
}

const RequestedCalibrationTask * PlanBuilder::find_requested_task_by_code(
  const WorkshopSessionConfig & config,
  const std::string & task_code) const
{
  if (task_code.empty()) {
    return nullptr;
  }
  for (const auto & task : config.requested_tasks) {
    if (task.task_code == task_code) {
      return &task;
    }
  }
  return nullptr;
}

bool PlanBuilder::should_include_stage(
  const VehicleProfile & profile,
  const WorkshopSessionConfig & config,
  uint8_t stage_type,
  uint8_t ability_type) const
{
  // 如果 requested_tasks 非空，以用户显式配置为准。
  if (!config.requested_tasks.empty()) {
    const auto * task = find_requested_task(config, stage_type);
    // 用户没有列出这个阶段 → 不加入。
    if (!task) {
      return false;
    }
    // 用户显式禁用 → 不加入。
    if (!task->enabled) {
      return false;
    }
    // 用户启用了，但车辆不支持 → 看执行策略。
    if (ability_type != 0 && !has_capability(profile, ability_type)) {
      return task->execution_policy.value == StageExecutionPolicy::SKIP_IF_UNSUPPORTED;
    }
    return true;
  }

  // requested_tasks 为空：按 capabilities 全量生成。
  if (ability_type == 0) {
    return true; // 无需 capability 检查的阶段（如外部真值校核）始终加入。
  }
  return has_capability(profile, ability_type);
}

void PlanBuilder::apply_task_config(StagePlan & stage, const RequestedCalibrationTask * task_cfg) const
{
  if (!task_cfg) {
    // 没有用户配置时，默认 REQUIRED。
    stage.execution_policy.value = StageExecutionPolicy::REQUIRED;
    return;
  }
  stage.execution_policy = task_cfg->execution_policy;
  stage.requires_approval_before_commit = task_cfg->require_manual_approval;
  // 把 task_params 合并到 stage.metadata。
  for (const auto & kv : task_cfg->task_params) {
    stage.metadata.push_back(kv);
  }
}

void PlanBuilder::apply_task_identity(StagePlan & stage, const RequestedCalibrationTask & task) const
{
  if (!task.task_code.empty()) {
    calibration_common_interfaces::msg::KeyValuePair kv;
    kv.key = metadata_keys::TASK_CODE;
    kv.value = task.task_code;
    stage.metadata.push_back(kv);
  }
  if (!task.target_id.empty()) {
    calibration_common_interfaces::msg::KeyValuePair kv;
    kv.key = metadata_keys::TARGET_ID;
    kv.value = task.target_id;
    stage.metadata.push_back(kv);
    stage.target_resource_ids.push_back(task.target_id);
  }
}

std::string PlanBuilder::make_task_suffix(const std::string & task_code) const
{
  if (task_code.empty()) {
    return std::string();
  }
  std::string suffix = "_";
  suffix.reserve(task_code.size() + 1);
  for (char ch : task_code) {
    if ((ch >= 'a' && ch <= 'z') || (ch >= 'A' && ch <= 'Z') || (ch >= '0' && ch <= '9')) {
      suffix.push_back(ch);
    } else {
      suffix.push_back('_');
    }
  }
  return suffix;
}

StagePlan PlanBuilder::make_stage_for_task(
  int order_index,
  const VehicleProfile & profile,
  const RequestedCalibrationTask & task) const
{
  if (task.stage_type.value == WorkflowStageType::CHASSIS_CALIBRATION_STAGE) {
    return make_chassis_stage(order_index, profile, &task);
  }
  if (task.stage_type.value == WorkflowStageType::CONTROL_CALIBRATION_STAGE) {
    return make_control_stage(order_index, profile, &task);
  }
  if (task.stage_type.value == WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE) {
    return make_sensor_intrinsic_stage(order_index, profile, &task);
  }
  if (task.stage_type.value == WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE) {
    return make_sensor_extrinsic_stage(order_index, profile, &task);
  }
  if (task.stage_type.value == WorkflowStageType::HAND_EYE_CALIBRATION_STAGE) {
    return make_hand_eye_stage(order_index, profile, &task);
  }
  if (task.stage_type.value == WorkflowStageType::EXTERNAL_REFERENCE_READY_CHECK_STAGE) {
    return make_external_reference_stage(order_index, profile, &task);
  }

  StagePlan stage;
  stage.stage_id = "stage_unknown" + make_task_suffix(task.task_code);
  stage.stage_type = make_stage_type(WorkflowStageType::WORKFLOW_STAGE_UNSPECIFIED);
  stage.module_type = make_module_type(CalibrationModuleType::CALIBRATION_MODULE_UNSPECIFIED);
  stage.display_name = task.task_code.empty() ? "未知任务" : task.task_code;
  stage.order_index = order_index;
  stage.executor_endpoint_name = std::string();
  stage.description = "未知的任务模板。";
  stage.auto_generated = true;
  apply_task_config(stage, &task);
  apply_task_identity(stage, task);
  return stage;
}

std::vector<StagePlan> PlanBuilder::build_minimal_plan(
  const VehicleProfile & profile,
  const WorkshopSessionConfig & config) const
{
  std::vector<StagePlan> stages;
  int order = 0;

  if (!config.requested_tasks.empty()) {
    for (const auto & task : config.requested_tasks) {
      if (!task.enabled) {
        continue;
      }
      if (task.stage_type.value != WorkflowStageType::WORKFLOW_STAGE_UNSPECIFIED) {
        if (task.stage_type.value == WorkflowStageType::CHASSIS_CALIBRATION_STAGE &&
            !has_capability(profile, CalibrationAbilityType::CHASSIS_CALIBRATION)) {
          if (task.execution_policy.value != StageExecutionPolicy::SKIP_IF_UNSUPPORTED) {
            continue;
          }
        }
        if (task.stage_type.value == WorkflowStageType::CONTROL_CALIBRATION_STAGE &&
            !has_capability(profile, CalibrationAbilityType::CONTROL_CALIBRATION)) {
          if (task.execution_policy.value != StageExecutionPolicy::SKIP_IF_UNSUPPORTED) {
            continue;
          }
        }
        if ((task.stage_type.value == WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE ||
             task.stage_type.value == WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE) &&
            !has_capability(profile, CalibrationAbilityType::SENSOR_CALIBRATION)) {
          if (task.execution_policy.value != StageExecutionPolicy::SKIP_IF_UNSUPPORTED) {
            continue;
          }
        }
        if (task.stage_type.value == WorkflowStageType::HAND_EYE_CALIBRATION_STAGE &&
            !has_capability(profile, CalibrationAbilityType::HAND_EYE_CALIBRATION)) {
          if (task.execution_policy.value != StageExecutionPolicy::SKIP_IF_UNSUPPORTED) {
            continue;
          }
        }
        stages.push_back(make_stage_for_task(order++, profile, task));
      }
    }
    return stages;
  }

  // 外部真值校核：默认依赖 external localization 能力；若 requested_tasks 显式要求则仍可纳入。
  if (should_include_stage(profile, config,
        WorkflowStageType::EXTERNAL_REFERENCE_READY_CHECK_STAGE,
        CalibrationAbilityType::EXTERNAL_LOCALIZATION_CALIBRATION)) {
    const auto * task_cfg = find_requested_task(config,
      WorkflowStageType::EXTERNAL_REFERENCE_READY_CHECK_STAGE);
    stages.push_back(make_external_reference_stage(order++, profile, task_cfg));
  }

  // 底盘标定。
  if (should_include_stage(profile, config,
        WorkflowStageType::CHASSIS_CALIBRATION_STAGE,
        CalibrationAbilityType::CHASSIS_CALIBRATION)) {
    const auto * task_cfg = find_requested_task(config,
      WorkflowStageType::CHASSIS_CALIBRATION_STAGE);
    stages.push_back(make_chassis_stage(order++, profile, task_cfg));
  }

  // 运控参数标定。
  if (should_include_stage(profile, config,
        WorkflowStageType::CONTROL_CALIBRATION_STAGE,
        CalibrationAbilityType::CONTROL_CALIBRATION)) {
    const auto * task_cfg = find_requested_task(config,
      WorkflowStageType::CONTROL_CALIBRATION_STAGE);
    stages.push_back(make_control_stage(order++, profile, task_cfg));
  }

  // 传感器内参标定。
  if (should_include_stage(profile, config,
        WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE,
        CalibrationAbilityType::SENSOR_CALIBRATION)) {
    const auto * task_cfg = find_requested_task(config,
      WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE);
    stages.push_back(make_sensor_intrinsic_stage(order++, profile, task_cfg));
  }

  // 传感器外参标定。
  if (should_include_stage(profile, config,
        WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE,
        CalibrationAbilityType::SENSOR_CALIBRATION)) {
    const auto * task_cfg = find_requested_task(config,
      WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE);
    stages.push_back(make_sensor_extrinsic_stage(order++, profile, task_cfg));
  }

  // 手眼标定。
  if (should_include_stage(profile, config,
        WorkflowStageType::HAND_EYE_CALIBRATION_STAGE,
        CalibrationAbilityType::HAND_EYE_CALIBRATION)) {
    const auto * task_cfg = find_requested_task(config,
      WorkflowStageType::HAND_EYE_CALIBRATION_STAGE);
    stages.push_back(make_hand_eye_stage(order++, profile, task_cfg));
  }

  return stages;
}

StagePlan PlanBuilder::make_external_reference_stage(
  int order_index,
  const VehicleProfile & profile,
  const RequestedCalibrationTask * task_cfg) const
{
  StagePlan stage;
  stage.stage_id = "stage_external_reference";
  stage.stage_type = make_stage_type(WorkflowStageType::EXTERNAL_REFERENCE_READY_CHECK_STAGE);
  stage.module_type = make_module_type(CalibrationModuleType::EXTERNAL_LOCALIZATION_MODULE);
  stage.display_name = "外部真值校核";
  stage.order_index = order_index;
  stage.executor_endpoint_name = "/external_localization/execute_task";
  stage.description = "验证并接入车间外部定位真值参考源。需要 session.config 提供 localization_source_id/workcell_zone_id；stage.metadata 需要提供 external.static_sample_count、external.dynamic_sample_count、external.max_position_stddev_m、external.max_yaw_stddev_rad、external.max_tracking_loss_ratio、external.max_time_sync_offset_ms、external.timeout_sec。";
  stage.retry_limit = 0;
  stage.auto_generated = true;
  apply_default_external_metadata(stage);
  apply_task_config(stage, task_cfg);
  if (task_cfg) {
    stage.stage_id += make_task_suffix(task_cfg->task_code);
    apply_task_identity(stage, *task_cfg);
  }
  return stage;
}

StagePlan PlanBuilder::make_chassis_stage(
  int order_index,
  const VehicleProfile & profile,
  const RequestedCalibrationTask * task_cfg) const
{
  StagePlan stage;
  stage.stage_id = "stage_chassis_calibration";
  stage.stage_type = make_stage_type(WorkflowStageType::CHASSIS_CALIBRATION_STAGE);
  stage.module_type = make_module_type(CalibrationModuleType::CHASSIS_MODULE);
  stage.display_name = "底盘标定";
  stage.order_index = order_index;
  stage.executor_endpoint_name = "/chassis/execute_motion_primitive";
  stage.description = "调用独立底盘标定包执行动作原语任务。stage.metadata 需要提供 primitive_type；支持 straight_line、arc、in_place_rotation、steering_sweep、lateral_translation、diagonal_motion、module_alignment、coordinated_steering。各动作分别消费对应 metadata 参数，并通用支持 brake_when_finished、timeout_sec。";
  stage.retry_limit = 1;
  stage.auto_generated = true;
  apply_default_chassis_metadata(stage);
  apply_task_config(stage, task_cfg);
  if (task_cfg) {
    stage.stage_id += make_task_suffix(task_cfg->task_code);
    apply_task_identity(stage, *task_cfg);
  }
  return stage;
}

StagePlan PlanBuilder::make_control_stage(
  int order_index,
  const VehicleProfile & profile,
  const RequestedCalibrationTask * task_cfg) const
{
  StagePlan stage;
  stage.stage_id = "stage_control_calibration";
  stage.stage_type = make_stage_type(WorkflowStageType::CONTROL_CALIBRATION_STAGE);
  stage.module_type = make_module_type(CalibrationModuleType::CONTROL_MODULE);
  stage.display_name = "运控参数调优";
  stage.order_index = order_index;
  stage.executor_endpoint_name = "/control/execute_controller_evaluation";
  stage.description = "调用独立运控标定包执行控制评估任务。stage.metadata 需要提供 control.task_type；支持 trajectory_tracking、velocity_step、accel_decel、stop_accuracy。trajectory_tracking 需提供 traj_pt_{i}_{x_m|y_m|yaw_rad|speed_ms}，其余任务需提供各自目标速度、加速度、停车目标与 timeout 等参数。";
  stage.retry_limit = 0;
  stage.auto_generated = true;
  apply_default_control_metadata(stage);
  apply_task_config(stage, task_cfg);
  if (task_cfg) {
    stage.stage_id += make_task_suffix(task_cfg->task_code);
    apply_task_identity(stage, *task_cfg);
  }
  return stage;
}

StagePlan PlanBuilder::make_sensor_intrinsic_stage(
  int order_index,
  const VehicleProfile & profile,
  const RequestedCalibrationTask * task_cfg) const
{
  StagePlan stage;
  stage.stage_id = "stage_sensor_intrinsic_calibration";
  stage.stage_type = make_stage_type(WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE);
  stage.module_type = make_module_type(CalibrationModuleType::SENSOR_INTRINSIC_MODULE);
  stage.display_name = "传感器内参标定";
  stage.order_index = order_index;
  stage.executor_endpoint_name = "/sensor_calibration/execute_task";
  stage.description = "启动独立传感器标定包执行内参标定任务。stage.metadata 需要提供 sensor.sensor_id、sensor.task_subtype；相机内参任务需提供 camera_intrinsic.required_image_count，可选 target_board_id/timeout_sec；IMU 内参任务需提供 imu_intrinsic.required_static_segment_count、imu_intrinsic.required_motion_segment_count，可选 imu_intrinsic.timeout_sec。";
  stage.retry_limit = 0;
  stage.auto_generated = true;
  apply_default_sensor_intrinsic_metadata(stage, profile);
  apply_task_config(stage, task_cfg);
  if (task_cfg) {
    stage.stage_id += make_task_suffix(task_cfg->task_code);
    apply_task_identity(stage, *task_cfg);
  }
  return stage;
}

StagePlan PlanBuilder::make_sensor_extrinsic_stage(
  int order_index,
  const VehicleProfile & profile,
  const RequestedCalibrationTask * task_cfg) const
{
  StagePlan stage;
  stage.stage_id = "stage_sensor_extrinsic_calibration";
  stage.stage_type = make_stage_type(WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE);
  stage.module_type = make_module_type(CalibrationModuleType::SENSOR_EXTRINSIC_MODULE);
  stage.display_name = "传感器外参标定";
  stage.order_index = order_index;
  stage.executor_endpoint_name = "/sensor_calibration/execute_task";
  stage.description = "启动独立传感器标定包执行外参标定任务。stage.metadata 需要提供 sensor.sensor_id、sensor_extrinsic.base_frame_id、sensor_extrinsic.required_sample_count；可选 sensor_extrinsic.timeout_sec。";
  stage.retry_limit = 0;
  stage.auto_generated = true;
  apply_default_sensor_extrinsic_metadata(stage, profile);
  apply_task_config(stage, task_cfg);
  if (task_cfg) {
    stage.stage_id += make_task_suffix(task_cfg->task_code);
    apply_task_identity(stage, *task_cfg);
  }
  return stage;
}

StagePlan PlanBuilder::make_hand_eye_stage(
  int order_index,
  const VehicleProfile & profile,
  const RequestedCalibrationTask * task_cfg) const
{
  StagePlan stage;
  stage.stage_id = "stage_hand_eye_calibration";
  stage.stage_type = make_stage_type(WorkflowStageType::HAND_EYE_CALIBRATION_STAGE);
  stage.module_type = make_module_type(CalibrationModuleType::HAND_EYE_MODULE);
  stage.display_name = "手眼标定";
  stage.order_index = order_index;
  stage.executor_endpoint_name = "/sensor_calibration/execute_task";
  stage.description = "启动独立传感器标定包执行手眼标定任务。stage.metadata 需要提供 sensor.sensor_id、hand_eye.arm_id、hand_eye.required_pose_count；可选 hand_eye.timeout_sec。";
  stage.retry_limit = 0;
  stage.auto_generated = true;
  apply_default_hand_eye_metadata(stage, profile);
  apply_task_config(stage, task_cfg);
  if (task_cfg) {
    stage.stage_id += make_task_suffix(task_cfg->task_code);
    apply_task_identity(stage, *task_cfg);
  }
  return stage;
}

}  // namespace workshop_orchestrator_v2
