#include "workshop_orchestrator_v2/precheck_runner.hpp"

#include "calibration_workshop_orchestration_interfaces/msg/precheck_item.hpp"

namespace workshop_orchestrator_v2
{

bool PrecheckRunner::has_metadata_key(const StagePlan & stage, const std::string & key) const
{
  for (const auto & kv : stage.metadata) {
    if (kv.key == key) {
      return true;
    }
  }
  return false;
}

bool PrecheckRunner::has_metadata_prefix(const StagePlan & stage, const std::string & prefix) const
{
  for (const auto & kv : stage.metadata) {
    if (kv.key.rfind(prefix, 0) == 0) {
      return true;
    }
  }
  return false;
}

WorkshopPrecheckResponse PrecheckRunner::run(const WorkshopSession & session) const
{
  WorkshopPrecheckResponse response;
  response.success = true;
  response.error_code = make_error_code(ErrorCode::OK);
  response.checked_timestamp_us = now_us();

  auto append_item = [&](const std::string & code,
                         const std::string & name,
                         bool passed,
                         const std::string & message,
                         uint8_t stage_type,
                         uint8_t module_type) {
    calibration_workshop_orchestration_interfaces::msg::PrecheckItem item;
    item.item_code = code;
    item.display_name = name;
    item.passed = passed;
    item.blocking = true;
    item.error_code = make_error_code(passed ? ErrorCode::OK : ErrorCode::INVALID_STATE);
    item.message = message;
    item.related_stage_type = make_stage_type(stage_type);
    item.related_module_type = make_module_type(module_type);
    response.items.push_back(item);
    if (!passed) {
      response.blocking_issue_count += 1;
      response.all_passed = false;
    }
  };

  response.all_passed = true;
  response.blocking_issue_count = 0;

  append_item(
    "plan_not_empty",
    "Execution plan exists",
    !session.stage_plan.empty(),
    session.stage_plan.empty() ? "Session has no executable stages." : "Session has executable stages.",
    WorkflowStageType::WORKFLOW_STAGE_UNSPECIFIED,
    CalibrationModuleType::CALIBRATION_MODULE_UNSPECIFIED);

  for (const auto & stage : session.stage_plan) {
    if (stage.module_type.value == CalibrationModuleType::CHASSIS_MODULE) {
      const bool has_primitive_type = has_metadata_key(stage, metadata_keys::CHASSIS_PRIMITIVE_TYPE);
      bool passed = false;
      std::string message = "底盘阶段缺少 primitive_type。";

      if (has_primitive_type) {
        std::string primitive_type;
        for (const auto & kv : stage.metadata) {
          if (kv.key == metadata_keys::CHASSIS_PRIMITIVE_TYPE) {
            primitive_type = kv.value;
            break;
          }
        }

        if (primitive_type == "straight_line") {
          passed =
            has_metadata_key(stage, metadata_keys::CHASSIS_STRAIGHT_LINE_DISTANCE_M) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_STRAIGHT_LINE_SPEED_MS);
          message = passed ? "底盘直行动作输入完整。" : "straight_line 缺少距离或速度参数。";
        } else if (primitive_type == "arc") {
          passed =
            has_metadata_key(stage, metadata_keys::CHASSIS_ARC_SPEED_MS) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_ARC_RADIUS_M) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_ARC_SWEEP_ANGLE_DEG);
          message = passed ? "底盘圆弧动作输入完整。" : "arc 缺少速度、半径或扫角参数。";
        } else if (primitive_type == "in_place_rotation") {
          passed =
            has_metadata_key(stage, metadata_keys::CHASSIS_ROTATION_TARGET_YAW_DEG) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_ROTATION_TARGET_ANGULAR_VEL_DEG_S);
          message = passed ? "原地旋转动作输入完整。" : "in_place_rotation 缺少角度或角速度参数。";
        } else if (primitive_type == "steering_sweep") {
          passed =
            has_metadata_key(stage, metadata_keys::CHASSIS_STEERING_SWEEP_TARGET_ANGLE_DEG) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_STEERING_SWEEP_AMPLITUDE_DEG) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_STEERING_SWEEP_FREQUENCY_HZ) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_STEERING_SWEEP_DURATION_SEC);
          message = passed ? "舵角扫动动作输入完整。" : "steering_sweep 缺少目标角、幅值、频率或持续时间。";
        } else if (primitive_type == "lateral_translation") {
          passed =
            has_metadata_key(stage, metadata_keys::CHASSIS_LATERAL_SPEED_MS) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_LATERAL_DISTANCE_M);
          message = passed ? "横移动作输入完整。" : "lateral_translation 缺少速度或距离参数。";
        } else if (primitive_type == "diagonal_motion") {
          passed =
            has_metadata_key(stage, metadata_keys::CHASSIS_DIAGONAL_SPEED_MS) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_DIAGONAL_DISTANCE_M) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_DIAGONAL_HEADING_DEG);
          message = passed ? "斜移动作输入完整。" : "diagonal_motion 缺少速度、距离或方向角参数。";
        } else if (primitive_type == "module_alignment") {
          passed =
            has_metadata_key(stage, metadata_keys::CHASSIS_MODULE_ALIGNMENT_IDS) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_MODULE_ALIGNMENT_ZERO_DEG) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_MODULE_ALIGNMENT_TOLERANCE_DEG);
          message = passed ? "模块零位检查输入完整。" : "module_alignment 缺少模块列表、零位或容差参数。";
        } else if (primitive_type == "coordinated_steering") {
          passed =
            has_metadata_key(stage, metadata_keys::CHASSIS_COORDINATED_STEERING_IDS) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_COORDINATED_STEERING_TARGET_ANGLE_DEG) &&
            has_metadata_key(stage, metadata_keys::CHASSIS_COORDINATED_STEERING_HOLD_TIME_SEC);
          message = passed ? "协同转向动作输入完整。" : "coordinated_steering 缺少模块列表、目标角或保持时间参数。";
        } else {
          message = "底盘阶段 primitive_type 不受支持。";
        }
      }

      append_item(
        stage.stage_id + ".metadata",
        stage.display_name + " metadata",
        passed,
        message,
        stage.stage_type.value,
        stage.module_type.value);
    } else if (stage.module_type.value == CalibrationModuleType::CONTROL_MODULE) {
      bool has_task_type = has_metadata_key(stage, metadata_keys::CONTROL_TASK_TYPE);
      bool passed = false;
      std::string message = "运控阶段缺少 control.task_type。";
      std::string task_type;
      if (has_task_type) {
        for (const auto & kv : stage.metadata) {
          if (kv.key == metadata_keys::CONTROL_TASK_TYPE) {
            task_type = kv.value;
            break;
          }
        }

        if (task_type == "trajectory_tracking") {
          passed = has_metadata_prefix(stage, metadata_keys::CONTROL_TRAJECTORY_PREFIX);
          message = passed ? "轨迹跟踪任务输入完整。" : "trajectory_tracking 缺少轨迹点输入。";
        } else if (task_type == "velocity_step") {
          passed =
            has_metadata_key(stage, metadata_keys::CONTROL_VELOCITY_STEP_TARGET_VELOCITY_MS) &&
            has_metadata_key(stage, metadata_keys::CONTROL_VELOCITY_STEP_HOLD_TIME_SEC) &&
            has_metadata_key(stage, metadata_keys::CONTROL_VELOCITY_STEP_SETTLE_BEFORE_STEP_SEC);
          message = passed ? "速度阶跃任务输入完整。" : "velocity_step 缺少目标速度、保持时间或阶跃前静稳时间。";
        } else if (task_type == "accel_decel") {
          passed =
            has_metadata_key(stage, metadata_keys::CONTROL_ACCEL_DECEL_START_VELOCITY_MS) &&
            has_metadata_key(stage, metadata_keys::CONTROL_ACCEL_DECEL_TARGET_VELOCITY_MS) &&
            has_metadata_key(stage, metadata_keys::CONTROL_ACCEL_DECEL_TARGET_ACCEL_MS2) &&
            has_metadata_key(stage, metadata_keys::CONTROL_ACCEL_DECEL_HOLD_TIME_SEC);
          message = passed ? "加减速任务输入完整。" : "accel_decel 缺少起始速度、目标速度、目标加速度或保持时间。";
        } else if (task_type == "stop_accuracy") {
          passed =
            has_metadata_key(stage, metadata_keys::CONTROL_STOP_ACCURACY_X_M) &&
            has_metadata_key(stage, metadata_keys::CONTROL_STOP_ACCURACY_Y_M) &&
            has_metadata_key(stage, metadata_keys::CONTROL_STOP_ACCURACY_YAW_RAD) &&
            has_metadata_key(stage, metadata_keys::CONTROL_TIMEOUT_SEC);
          message = passed ? "停车精度任务输入完整。" : "stop_accuracy 缺少目标停车位姿或 control.timeout_sec。";
        } else {
          message = "运控阶段 control.task_type 不受支持。";
        }
      }

      append_item(
        stage.stage_id + ".metadata",
        stage.display_name + " metadata",
        passed,
        message,
        stage.stage_type.value,
        stage.module_type.value);
    } else if (
      stage.module_type.value == CalibrationModuleType::SENSOR_INTRINSIC_MODULE ||
      stage.module_type.value == CalibrationModuleType::SENSOR_EXTRINSIC_MODULE ||
      stage.module_type.value == CalibrationModuleType::HAND_EYE_MODULE) {
      bool passed =
        has_metadata_key(stage, metadata_keys::SENSOR_ID) &&
        has_metadata_key(stage, metadata_keys::SENSOR_TASK_SUBTYPE);
      std::string message = passed ? "传感器阶段基础输入完整。" : "传感器阶段缺少 sensor.sensor_id 或 sensor.task_subtype。";

      if (passed && stage.module_type.value == CalibrationModuleType::SENSOR_INTRINSIC_MODULE) {
        std::string subtype;
        for (const auto & kv : stage.metadata) {
          if (kv.key == metadata_keys::SENSOR_TASK_SUBTYPE) {
            subtype = kv.value;
            break;
          }
        }

        if (subtype == "imu_intrinsic") {
          passed =
            has_metadata_key(stage, metadata_keys::IMU_INTRINSIC_REQUIRED_STATIC_SEGMENT_COUNT) &&
            has_metadata_key(stage, metadata_keys::IMU_INTRINSIC_REQUIRED_MOTION_SEGMENT_COUNT);
          message = passed ? "IMU 内参阶段输入完整。" : "IMU 内参阶段缺少静止段数或运动段数。";
        } else {
          passed = has_metadata_key(stage, metadata_keys::CAMERA_INTRINSIC_REQUIRED_IMAGE_COUNT);
          message = passed ? "相机内参阶段输入完整。" : "相机内参阶段缺少 required_image_count。";
        }
      }

      if (passed && stage.module_type.value == CalibrationModuleType::SENSOR_EXTRINSIC_MODULE) {
        passed =
          has_metadata_key(stage, metadata_keys::SENSOR_EXTRINSIC_BASE_FRAME_ID) &&
          has_metadata_key(stage, metadata_keys::SENSOR_EXTRINSIC_REQUIRED_SAMPLE_COUNT);
        message = passed ? "传感器外参阶段输入完整。" : "传感器外参阶段缺少 base_frame_id 或 required_sample_count。";
      }

      if (passed && stage.module_type.value == CalibrationModuleType::HAND_EYE_MODULE) {
        passed =
          has_metadata_key(stage, metadata_keys::HAND_EYE_ARM_ID) &&
          has_metadata_key(stage, metadata_keys::HAND_EYE_REQUIRED_POSE_COUNT);
        message = passed ? "手眼阶段输入完整。" : "手眼阶段缺少 arm_id 或 required_pose_count。";
      }

      append_item(
        stage.stage_id + ".metadata",
        stage.display_name + " metadata",
        passed,
        message,
        stage.stage_type.value,
        stage.module_type.value);
    } else if (stage.module_type.value == CalibrationModuleType::EXTERNAL_LOCALIZATION_MODULE) {
      const bool passed =
        has_metadata_key(stage, metadata_keys::EXTERNAL_STATIC_SAMPLE_COUNT) &&
        has_metadata_key(stage, metadata_keys::EXTERNAL_DYNAMIC_SAMPLE_COUNT) &&
        has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_POSITION_STDDEV_M) &&
        has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_YAW_STDDEV_RAD) &&
        has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_TRACKING_LOSS_RATIO) &&
        has_metadata_key(stage, metadata_keys::EXTERNAL_MAX_TIME_SYNC_OFFSET_MS) &&
        has_metadata_key(stage, metadata_keys::EXTERNAL_TIMEOUT_SEC);
      append_item(
        stage.stage_id + ".metadata",
        stage.display_name + " metadata",
        passed,
        passed ? "external 阶段输入完整。" : "external 阶段缺少真值校核阈值配置。",
        stage.stage_type.value,
        stage.module_type.value);
    }
  }

  response.ready_for_start = response.all_passed;
  response.message = response.ready_for_start ? "Precheck passed." : "Precheck failed.";
  return response;
}

}  // namespace workshop_orchestrator_v2
