#include "workshop_orchestrator_v2/sensor_gateway_client.hpp"

#include <chrono>
#include <future>
#include <map>
#include <sstream>
#include <unordered_map>

#include "calibration_sensor_interfaces/msg/hand_eye_calibration_mode.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_task_purpose.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_task_subtype.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_task_type.hpp"
#include "workshop_orchestrator_v2/types.hpp"

namespace workshop_orchestrator_v2
{

using namespace std::chrono_literals;
using calibration_sensor_interfaces::msg::HandEyeCalibrationMode;
using calibration_sensor_interfaces::msg::SensorCalibrationTaskPurpose;
using calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype;
using calibration_sensor_interfaces::msg::SensorCalibrationTaskType;
using calibration_vehicle_profile_interfaces::msg::CalibrationModuleType;
using calibration_workshop_orchestration_interfaces::msg::StagePlan;
using calibration_workshop_orchestration_interfaces::msg::StageResultSummary;
using calibration_workshop_orchestration_interfaces::msg::WorkshopSession;

SensorGatewayClient::SensorGatewayClient(rclcpp::Node * node)
: node_(node)
{
  readiness_client_ = node_->create_client<ReadinessSrv>("/sensor_calibration/get_readiness");
  execute_task_client_ = rclcpp_action::create_client<ExecuteTask>(
    node_, "/sensor_calibration/execute_task");
}

std::string SensorGatewayClient::make_request_id() const
{
  std::ostringstream oss;
  oss << "sensor_req_" << now_us();
  return oss.str();
}

bool SensorGatewayClient::check_ready(std::string & failure_reason)
{
  if (!readiness_client_->wait_for_service(2s)) {
    failure_reason = "传感器标定 readiness 服务不可用。";
    return false;
  }

  auto request = std::make_shared<ReadinessSrv::Request>();
  request->request.agent_name = "sensor_calibration";
  request->request.include_details = true;

  auto future = readiness_client_->async_send_request(request);
  if (future.wait_for(3s) != std::future_status::ready) {
    failure_reason = "等待传感器标定 readiness 响应超时。";
    return false;
  }

  const auto response = future.get();
  if (!response->response.success) {
    failure_reason = response->response.message.empty() ?
      "传感器标定 readiness 检查失败。" : response->response.message;
    return false;
  }

  if (!response->response.agent_ready) {
    failure_reason = "传感器标定 agent 当前未就绪。";
    return false;
  }

  if (!response->response.capture_pipeline_ready) {
    failure_reason = "传感器数据采集链路未就绪。";
    return false;
  }

  if (!response->response.storage_ready) {
    failure_reason = "传感器标定存储链路未就绪，无法保存标定产物。";
    return false;
  }

  return true;
}

uint8_t SensorGatewayClient::resolve_task_type(
  const StagePlan & stage,
  std::string & failure_reason) const
{
  if (stage.module_type.value == CalibrationModuleType::SENSOR_INTRINSIC_MODULE) {
    std::unordered_map<std::string, std::string> metadata_map;
    for (const auto & kv : stage.metadata) {
      metadata_map[kv.key] = kv.value;
    }

    const auto subtype_it = metadata_map.find(metadata_keys::SENSOR_TASK_SUBTYPE);
    if (subtype_it == metadata_map.end()) {
      failure_reason = "传感器内参阶段缺少 sensor.task_subtype。";
      return SensorCalibrationTaskType::SENSOR_CALIBRATION_TASK_TYPE_UNSPECIFIED;
    }

    if (subtype_it->second == "imu_intrinsic") {
      return SensorCalibrationTaskType::IMU_INTRINSIC;
    }
    return SensorCalibrationTaskType::CAMERA_INTRINSIC;
  }
  if (stage.module_type.value == CalibrationModuleType::SENSOR_EXTRINSIC_MODULE) {
    return SensorCalibrationTaskType::SENSOR_TO_BASE_EXTRINSIC;
  }
  if (stage.module_type.value == CalibrationModuleType::HAND_EYE_MODULE) {
    return SensorCalibrationTaskType::HAND_EYE;
  }

  failure_reason = "当前传感器阶段未映射到具体标定任务类型。";
  return SensorCalibrationTaskType::SENSOR_CALIBRATION_TASK_TYPE_UNSPECIFIED;
}

uint8_t resolve_task_subtype_from_metadata(
  const std::unordered_map<std::string, std::string> & metadata_map,
  std::string & failure_reason)
{
  const auto subtype_it = metadata_map.find(metadata_keys::SENSOR_TASK_SUBTYPE);
  if (subtype_it == metadata_map.end()) {
    failure_reason = "传感器阶段缺少 sensor.task_subtype。";
    return SensorCalibrationTaskSubtype::SENSOR_CALIBRATION_TASK_SUBTYPE_UNSPECIFIED;
  }

  const auto & value = subtype_it->second;
  if (value == "front_camera_intrinsic") {
    return SensorCalibrationTaskSubtype::FRONT_CAMERA_INTRINSIC;
  }
  if (value == "downward_camera_intrinsic") {
    return SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_INTRINSIC;
  }
  if (value == "imu_intrinsic") {
    return SensorCalibrationTaskSubtype::IMU_INTRINSIC;
  }
  if (value == "front_camera_extrinsic") {
    return SensorCalibrationTaskSubtype::FRONT_CAMERA_EXTRINSIC;
  }
  if (value == "downward_camera_extrinsic") {
    return SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_EXTRINSIC;
  }
  if (value == "imu_extrinsic") {
    return SensorCalibrationTaskSubtype::IMU_EXTRINSIC;
  }
  if (value == "lidar_2d_extrinsic") {
    return SensorCalibrationTaskSubtype::LIDAR_2D_EXTRINSIC;
  }
  if (value == "lidar_3d_extrinsic") {
    return SensorCalibrationTaskSubtype::LIDAR_3D_EXTRINSIC;
  }
  if (value == "eye_in_hand") {
    return SensorCalibrationTaskSubtype::EYE_IN_HAND;
  }
  if (value == "eye_to_hand") {
    return SensorCalibrationTaskSubtype::EYE_TO_HAND;
  }

  failure_reason = "sensor.task_subtype 不支持：" + value;
  return SensorCalibrationTaskSubtype::SENSOR_CALIBRATION_TASK_SUBTYPE_UNSPECIFIED;
}

bool task_and_subtype_match(
  uint8_t task_type,
  uint8_t task_subtype)
{
  if (task_type == SensorCalibrationTaskType::CAMERA_INTRINSIC) {
    return task_subtype == SensorCalibrationTaskSubtype::FRONT_CAMERA_INTRINSIC ||
           task_subtype == SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_INTRINSIC;
  }
  if (task_type == SensorCalibrationTaskType::IMU_INTRINSIC) {
    return task_subtype == SensorCalibrationTaskSubtype::IMU_INTRINSIC;
  }
  if (task_type == SensorCalibrationTaskType::SENSOR_TO_BASE_EXTRINSIC) {
    return task_subtype == SensorCalibrationTaskSubtype::FRONT_CAMERA_EXTRINSIC ||
           task_subtype == SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_EXTRINSIC ||
           task_subtype == SensorCalibrationTaskSubtype::IMU_EXTRINSIC ||
           task_subtype == SensorCalibrationTaskSubtype::LIDAR_2D_EXTRINSIC ||
           task_subtype == SensorCalibrationTaskSubtype::LIDAR_3D_EXTRINSIC;
  }
  if (task_type == SensorCalibrationTaskType::HAND_EYE) {
    return task_subtype == SensorCalibrationTaskSubtype::EYE_IN_HAND ||
           task_subtype == SensorCalibrationTaskSubtype::EYE_TO_HAND;
  }
  return false;
}

bool SensorGatewayClient::build_goal(
  const WorkshopSession & session,
  const StagePlan & stage,
  uint8_t selected_task_type,
  ExecuteTask::Goal & goal,
  std::string & failure_reason) const
{

  // 填充请求头。
  goal.goal.header.session_id = session.session_id;
  goal.goal.header.task_id = stage.stage_id;
  goal.goal.header.vehicle_id = session.vehicle_profile_snapshot.base_info.vehicle_id;
  goal.goal.header.request_id = make_request_id();
  goal.goal.header.client_send_timestamp_us = now_us();
  goal.goal.header.operator_id = session.operator_info.operator_id;
  goal.goal.header.workshop_host = "workshop_orchestrator_v2";

  goal.goal.test_case_id = stage.stage_id;

  // 当前目的：数据采集（具体标定算法和流程由下游传感器标定包负责）。
  goal.goal.task_purpose.value = SensorCalibrationTaskPurpose::DATA_COLLECTION;
  goal.goal.selected_task.value = selected_task_type;

  std::unordered_map<std::string, std::string> metadata_map;
  for (const auto & kv : stage.metadata) {
    metadata_map[kv.key] = kv.value;
  }

  const auto task_subtype = resolve_task_subtype_from_metadata(metadata_map, failure_reason);
  if (task_subtype == SensorCalibrationTaskSubtype::SENSOR_CALIBRATION_TASK_SUBTYPE_UNSPECIFIED) {
    return false;
  }
  if (!task_and_subtype_match(selected_task_type, task_subtype)) {
    failure_reason = "sensor.task_subtype 与当前模块映射出的 task_type 不匹配。";
    return false;
  }
  goal.goal.task_subtype.value = task_subtype;

  const auto sensor_id_it = metadata_map.find(metadata_keys::SENSOR_ID);
  if (sensor_id_it == metadata_map.end()) {
    failure_reason = "传感器阶段缺少 sensor.sensor_id。";
    return false;
  }

  if (selected_task_type == SensorCalibrationTaskType::CAMERA_INTRINSIC) {
    goal.goal.camera_intrinsic.sensor_id = sensor_id_it->second;
    const auto image_count_it = metadata_map.find(metadata_keys::CAMERA_INTRINSIC_REQUIRED_IMAGE_COUNT);
    if (image_count_it == metadata_map.end()) {
      failure_reason = "camera_intrinsic 缺少 required_image_count。";
      return false;
    }
    try {
      goal.goal.camera_intrinsic.required_image_count = static_cast<uint32_t>(std::stoul(image_count_it->second));
    } catch (...) {
      failure_reason = "camera_intrinsic.required_image_count 格式错误。";
      return false;
    }
    const auto board_it = metadata_map.find(metadata_keys::CAMERA_INTRINSIC_TARGET_BOARD_ID);
    if (board_it != metadata_map.end()) {
      goal.goal.camera_intrinsic.target_board_id = board_it->second;
    }
    const auto timeout_it = metadata_map.find(metadata_keys::CAMERA_INTRINSIC_TIMEOUT_SEC);
    if (timeout_it != metadata_map.end()) {
      try {
        goal.goal.camera_intrinsic.timeout_sec = std::stod(timeout_it->second);
      } catch (...) {
        failure_reason = "camera_intrinsic.timeout_sec 格式错误。";
        return false;
      }
    }
    goal.goal.camera_intrinsic.reference_target.reference_target_id = stage.stage_id;
  } else if (selected_task_type == SensorCalibrationTaskType::IMU_INTRINSIC) {
    goal.goal.imu_intrinsic.sensor_id = sensor_id_it->second;
    const auto static_count_it = metadata_map.find(metadata_keys::IMU_INTRINSIC_REQUIRED_STATIC_SEGMENT_COUNT);
    const auto motion_count_it = metadata_map.find(metadata_keys::IMU_INTRINSIC_REQUIRED_MOTION_SEGMENT_COUNT);
    if (static_count_it == metadata_map.end() || motion_count_it == metadata_map.end()) {
      failure_reason = "imu_intrinsic 缺少 required_static_segment_count 或 required_motion_segment_count。";
      return false;
    }
    try {
      goal.goal.imu_intrinsic.required_static_segment_count =
        static_cast<uint32_t>(std::stoul(static_count_it->second));
      goal.goal.imu_intrinsic.required_motion_segment_count =
        static_cast<uint32_t>(std::stoul(motion_count_it->second));
    } catch (...) {
      failure_reason = "imu_intrinsic 参数格式错误。";
      return false;
    }
    const auto timeout_it = metadata_map.find(metadata_keys::IMU_INTRINSIC_TIMEOUT_SEC);
    if (timeout_it != metadata_map.end()) {
      try {
        goal.goal.imu_intrinsic.timeout_sec = std::stod(timeout_it->second);
      } catch (...) {
        failure_reason = "imu_intrinsic.timeout_sec 格式错误。";
        return false;
      }
    }
  } else if (selected_task_type == SensorCalibrationTaskType::SENSOR_TO_BASE_EXTRINSIC) {
    goal.goal.sensor_to_base_extrinsic.sensor_id = sensor_id_it->second;
    const auto base_frame_it = metadata_map.find(metadata_keys::SENSOR_EXTRINSIC_BASE_FRAME_ID);
    if (base_frame_it == metadata_map.end()) {
      failure_reason = "sensor_extrinsic 缺少 base_frame_id。";
      return false;
    }
    goal.goal.sensor_to_base_extrinsic.base_frame_id = base_frame_it->second;
    const auto sample_count_it = metadata_map.find(metadata_keys::SENSOR_EXTRINSIC_REQUIRED_SAMPLE_COUNT);
    if (sample_count_it == metadata_map.end()) {
      failure_reason = "sensor_extrinsic 缺少 required_sample_count。";
      return false;
    }
    try {
      goal.goal.sensor_to_base_extrinsic.required_sample_count = static_cast<uint32_t>(std::stoul(sample_count_it->second));
    } catch (...) {
      failure_reason = "sensor_extrinsic.required_sample_count 格式错误。";
      return false;
    }
    const auto timeout_it = metadata_map.find(metadata_keys::SENSOR_EXTRINSIC_TIMEOUT_SEC);
    if (timeout_it != metadata_map.end()) {
      try {
        goal.goal.sensor_to_base_extrinsic.timeout_sec = std::stod(timeout_it->second);
      } catch (...) {
        failure_reason = "sensor_extrinsic.timeout_sec 格式错误。";
        return false;
      }
    }
    goal.goal.sensor_to_base_extrinsic.reference_target.reference_target_id = stage.stage_id;

    if (task_subtype == SensorCalibrationTaskSubtype::FRONT_CAMERA_EXTRINSIC ||
      task_subtype == SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_EXTRINSIC)
    {
      goal.goal.sensor_to_base_extrinsic.capture_intent.value =
        calibration_sensor_interfaces::msg::CaptureIntentType::CAMERA_EXTRINSIC_CAPTURE;
    } else if (task_subtype == SensorCalibrationTaskSubtype::LIDAR_2D_EXTRINSIC) {
      goal.goal.sensor_to_base_extrinsic.capture_intent.value =
        calibration_sensor_interfaces::msg::CaptureIntentType::LIDAR_2D_EXTRINSIC_CAPTURE;
    } else if (task_subtype == SensorCalibrationTaskSubtype::LIDAR_3D_EXTRINSIC) {
      goal.goal.sensor_to_base_extrinsic.capture_intent.value =
        calibration_sensor_interfaces::msg::CaptureIntentType::LIDAR_3D_EXTRINSIC_CAPTURE;
    } else if (task_subtype == SensorCalibrationTaskSubtype::IMU_EXTRINSIC) {
      goal.goal.sensor_to_base_extrinsic.capture_intent.value =
        calibration_sensor_interfaces::msg::CaptureIntentType::IMU_DYNAMIC_CAPTURE;
    }
  } else if (selected_task_type == SensorCalibrationTaskType::HAND_EYE) {
    goal.goal.hand_eye.sensor_id = sensor_id_it->second;
    const auto arm_id_it = metadata_map.find(metadata_keys::HAND_EYE_ARM_ID);
    if (arm_id_it == metadata_map.end()) {
      failure_reason = "hand_eye 缺少 arm_id。";
      return false;
    }
    goal.goal.hand_eye.arm_id = arm_id_it->second;
    const auto pose_count_it = metadata_map.find(metadata_keys::HAND_EYE_REQUIRED_POSE_COUNT);
    if (pose_count_it == metadata_map.end()) {
      failure_reason = "hand_eye 缺少 required_pose_count。";
      return false;
    }
    try {
      goal.goal.hand_eye.required_pose_count = static_cast<uint32_t>(std::stoul(pose_count_it->second));
    } catch (...) {
      failure_reason = "hand_eye.required_pose_count 格式错误。";
      return false;
    }
    const auto timeout_it = metadata_map.find(metadata_keys::HAND_EYE_TIMEOUT_SEC);
    if (timeout_it != metadata_map.end()) {
      try {
        goal.goal.hand_eye.timeout_sec = std::stod(timeout_it->second);
      } catch (...) {
        failure_reason = "hand_eye.timeout_sec 格式错误。";
        return false;
      }
    }
    goal.goal.hand_eye.reference_target.reference_target_id = stage.stage_id;

    if (task_subtype == SensorCalibrationTaskSubtype::EYE_IN_HAND) {
      goal.goal.hand_eye.mode.value = HandEyeCalibrationMode::EYE_IN_HAND;
    } else if (task_subtype == SensorCalibrationTaskSubtype::EYE_TO_HAND) {
      goal.goal.hand_eye.mode.value = HandEyeCalibrationMode::EYE_TO_HAND;
    }
  }

  goal.goal.source_iteration_id = session.session_id;

  return true;
}

bool SensorGatewayClient::execute_sensor_stage(
  const WorkshopSession & session,
  const StagePlan & stage,
  StageResultSummary & result,
  std::string & failure_reason)
{
  // Step 1：就绪检查。
  if (!check_ready(failure_reason)) {
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  std::string task_type_error;
  const auto selected_task_type = resolve_task_type(stage, task_type_error);
  if (selected_task_type == SensorCalibrationTaskType::SENSOR_CALIBRATION_TASK_TYPE_UNSPECIFIED) {
    failure_reason = task_type_error;
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  ExecuteTask::Goal goal;
  if (!build_goal(session, stage, selected_task_type, goal, failure_reason)) {
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  // Step 2：等待 action server 上线。
  if (!execute_task_client_->wait_for_action_server(2s)) {
    failure_reason = "传感器标定 action server 不可用。";
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  // Step 3：发送 goal。
  auto goal_handle_future = execute_task_client_->async_send_goal(goal);
  if (goal_handle_future.wait_for(5s) != std::future_status::ready) {
    failure_reason = "发送传感器标定任务请求超时。";
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  auto goal_handle = goal_handle_future.get();
  if (!goal_handle) {
    failure_reason = "传感器标定服务拒绝了本次任务请求。";
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  // Step 4：等待结果，超时 180 秒（传感器标定耗时最长）。
  auto result_future = execute_task_client_->async_get_result(goal_handle);
  if (result_future.wait_for(180s) != std::future_status::ready) {
    failure_reason = "等待传感器标定结果超时。";
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  auto wrapped_result = result_future.get();
  if (!wrapped_result.result) {
    failure_reason = "传感器标定服务返回了空结果。";
    result.success = false;
    result.auto_acceptance_passed = false;
    result.final_job_state = make_job_state(calibration_common_interfaces::msg::JobState::FAILED);
    result.summary = failure_reason;
    result.failure_root_cause = failure_reason;
    return false;
  }

  // Step 5：翻译 SensorCalibrationJobResult → StageResultSummary。
  const auto & job_result = wrapped_result.result->result;
  result.success = job_result.success;
  // 自动验收：数据质量达标 且 标定参数适合写入。
  result.auto_acceptance_passed = job_result.data_quality_passed && job_result.suitable_for_commit;
  result.final_job_state = make_job_state(
    job_result.success ? calibration_common_interfaces::msg::JobState::SUCCEEDED
                       : calibration_common_interfaces::msg::JobState::FAILED);
  result.parameter_version = job_result.recommended_parameter_version;

  // 传感器接口中 artifacts 类型为 CalibrationArtifact[]，
  // StageResultSummary.artifacts 为 FileReference[]，需逐条转换。
  result.artifacts.clear();
  for (const auto & artifact : job_result.artifacts) {
    calibration_common_interfaces::msg::FileReference ref;
    ref.file_name = artifact.file.file_name;
    ref.file_uri = artifact.file.file_uri;
    ref.size_bytes = artifact.file.size_bytes;
    ref.description = artifact.file.description;
    ref.digest = artifact.file.digest;
    result.artifacts.push_back(ref);
  }

  result.summary = job_result.message.empty() ? "传感器标定完成。" : job_result.message;
  result.failure_root_cause = job_result.success ? "" : result.summary;

  if (!job_result.success) {
    failure_reason = result.summary;
    return false;
  }
  if (!result.auto_acceptance_passed) {
    failure_reason = "传感器标定未通过自动验收（数据质量或参数适合性不满足）。";
    result.failure_root_cause = failure_reason;
    return false;
  }

  return true;
}

}  // namespace workshop_orchestrator_v2
