#include "workshop_orchestrator_v2/external_localization_client.hpp"

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

#include "workshop_orchestrator_v2/types.hpp"

namespace workshop_orchestrator_v2
{

using namespace std::chrono_literals;
using calibration_workshop_orchestration_interfaces::msg::StagePlan;
using calibration_workshop_orchestration_interfaces::msg::StageResultSummary;
using calibration_workshop_orchestration_interfaces::msg::WorkshopSession;

ExternalLocalizationClient::ExternalLocalizationClient(rclcpp::Node * node)
: node_(node)
{
  // orchestrator 这里只保留对 external service 的调用入口。
  readiness_client_ = node_->create_client<ReadinessSrv>("/external_localization/get_readiness");
  execute_task_client_ = rclcpp_action::create_client<ExecuteTask>(
    node_, "/external_localization/execute_task");
}

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

bool ExternalLocalizationClient::check_ready(std::string & failure_reason)
{
  if (!readiness_client_->wait_for_service(2s)) {
    failure_reason = "external_localization readiness 服务不可用。";
    return false;
  }

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

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

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

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

  if (!response->response.ready_for_reference_validation) {
    failure_reason = response->response.message.empty() ?
      "external_localization 当前不允许做参考源校核。" : response->response.message;
    return false;
  }

  return true;
}

bool ExternalLocalizationClient::build_goal(
  const WorkshopSession & session,
  const StagePlan & stage,
  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.task_purpose.value =
    calibration_external_localization_interfaces::msg::ExternalLocalizationTaskPurpose::REFERENCE_READY_CHECK;
  goal.goal.selected_task.value =
    calibration_external_localization_interfaces::msg::ExternalLocalizationTaskPayloadType::TRUTH_SOURCE_VALIDATION;

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

  if (session.config.localization_source_id.empty()) {
    failure_reason = "external 阶段缺少 localization_source_id。";
    return false;
  }
  if (session.config.workcell_zone_id.empty()) {
    failure_reason = "external 阶段缺少 workcell_zone_id。";
    return false;
  }

  goal.goal.localization_source_id = session.config.localization_source_id;
  goal.goal.workcell_zone_id = session.config.workcell_zone_id;

  const auto static_count_it = metadata_map.find(metadata_keys::EXTERNAL_STATIC_SAMPLE_COUNT);
  const auto dynamic_count_it = metadata_map.find(metadata_keys::EXTERNAL_DYNAMIC_SAMPLE_COUNT);
  const auto pos_stddev_it = metadata_map.find(metadata_keys::EXTERNAL_MAX_POSITION_STDDEV_M);
  const auto yaw_stddev_it = metadata_map.find(metadata_keys::EXTERNAL_MAX_YAW_STDDEV_RAD);
  const auto loss_ratio_it = metadata_map.find(metadata_keys::EXTERNAL_MAX_TRACKING_LOSS_RATIO);
  const auto sync_offset_it = metadata_map.find(metadata_keys::EXTERNAL_MAX_TIME_SYNC_OFFSET_MS);
  const auto timeout_it = metadata_map.find(metadata_keys::EXTERNAL_TIMEOUT_SEC);
  if (static_count_it == metadata_map.end() || dynamic_count_it == metadata_map.end() ||
      pos_stddev_it == metadata_map.end() || yaw_stddev_it == metadata_map.end() ||
      loss_ratio_it == metadata_map.end() || sync_offset_it == metadata_map.end() ||
      timeout_it == metadata_map.end()) {
    failure_reason = "external 阶段缺少真值校核阈值配置。";
    return false;
  }

  try {
    goal.goal.truth_source_validation.static_sample_count = static_cast<uint32_t>(std::stoul(static_count_it->second));
    goal.goal.truth_source_validation.dynamic_sample_count = static_cast<uint32_t>(std::stoul(dynamic_count_it->second));
    goal.goal.truth_source_validation.max_position_stddev_m = std::stod(pos_stddev_it->second);
    goal.goal.truth_source_validation.max_yaw_stddev_rad = std::stod(yaw_stddev_it->second);
    goal.goal.truth_source_validation.max_tracking_loss_ratio = std::stod(loss_ratio_it->second);
    goal.goal.truth_source_validation.max_time_sync_offset_ms = std::stod(sync_offset_it->second);
    goal.goal.truth_source_validation.timeout_sec = std::stod(timeout_it->second);
  } catch (...) {
    failure_reason = "external 阶段阈值配置格式错误。";
    return false;
  }

  const auto short_motion_it = metadata_map.find(metadata_keys::EXTERNAL_REQUIRE_SHORT_MOTION_SEGMENT);
  goal.goal.truth_source_validation.require_short_motion_segment =
    short_motion_it == metadata_map.end() || short_motion_it->second == "true" || short_motion_it->second == "1";

  goal.goal.test_case_id = session.config.reference_target_id.empty() ? stage.stage_id : session.config.reference_target_id;
  goal.goal.source_iteration_id = session.session_id;

  return true;
}

bool ExternalLocalizationClient::execute_reference_check(
  const WorkshopSession & session,
  const StagePlan & stage,
  StageResultSummary & result,
  std::string & failure_reason)
{
  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;
  }

  ExecuteTask::Goal goal;
  if (!build_goal(session, stage, 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;
  }

  if (!execute_task_client_->wait_for_action_server(2s)) {
    failure_reason = "external_localization 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;
  }

  auto goal_handle_future = execute_task_client_->async_send_goal(goal);
  if (goal_handle_future.wait_for(5s) != std::future_status::ready) {
    failure_reason = "发送 external_localization 任务请求超时。";
    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 = "external_localization 拒绝了这次外部真值校核请求。";
    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 result_future = execute_task_client_->async_get_result(goal_handle);
  if (result_future.wait_for(35s) != std::future_status::ready) {
    failure_reason = "等待 external_localization 结果超时。";
    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 = "external_localization 返回了空结果。";
    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;
  }

  const auto & job_result = wrapped_result.result->result;
  result.success = job_result.success;
  result.auto_acceptance_passed =
    job_result.data_quality_passed && job_result.validation_summary.recommended_as_truth_source;
  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;
  result.artifacts = job_result.artifacts;
  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
