#include "external_localization_service/external_localization_service_node.hpp"

#include <chrono>
#include <cmath>
#include <cstddef>
#include <thread>

#include "calibration_common_interfaces/msg/error_code.hpp"
#include "calibration_common_interfaces/msg/job_state.hpp"

namespace external_localization_service
{

namespace
{
int64_t now_us()
{
  return std::chrono::duration_cast<std::chrono::microseconds>(
           std::chrono::system_clock::now().time_since_epoch())
    .count();
}

calibration_common_interfaces::msg::FileReference make_file_reference(
  const std::string & file_uri,
  const std::string & description)
{
  calibration_common_interfaces::msg::FileReference ref;
  ref.file_uri = file_uri;
  const auto pos = file_uri.find_last_of("/\\");
  ref.file_name = pos == std::string::npos ? file_uri : file_uri.substr(pos + 1);
  ref.size_bytes = 0;
  ref.description = description;
  return ref;
}

std::vector<calibration_common_interfaces::msg::FileReference> make_file_references(
  const std::vector<std::string> & file_uris,
  const std::string & description)
{
  std::vector<calibration_common_interfaces::msg::FileReference> refs;
  refs.reserve(file_uris.size());
  for (const auto & file_uri : file_uris) {
    if (!file_uri.empty()) {
      refs.push_back(make_file_reference(file_uri, description));
    }
  }
  return refs;
}
}  // namespace

using calibration_common_interfaces::msg::ErrorCode;
using calibration_common_interfaces::msg::JobState;

ExternalLocalizationServiceNode::ExternalLocalizationServiceNode(const rclcpp::NodeOptions & options)
: Node("external_localization_service", options),
  readiness_config_(load_readiness_config()),
  data_input_config_(load_data_input_config())
{
  telemetry_subscription_ = create_subscription<ExternalTelemetry>(
    readiness_config_.telemetry_topic,
    rclcpp::QoS(10),
    std::bind(&ExternalLocalizationServiceNode::handle_external_telemetry, this, std::placeholders::_1));

  readiness_service_ = create_service<ReadinessSrv>(
    "/external_localization/get_readiness",
    std::bind(&ExternalLocalizationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2));

  execute_task_action_server_ = rclcpp_action::create_server<ExecuteTask>(
    this,
    "/external_localization/execute_task",
    std::bind(&ExternalLocalizationServiceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
    std::bind(&ExternalLocalizationServiceNode::handle_cancel, this, std::placeholders::_1),
    std::bind(&ExternalLocalizationServiceNode::handle_accepted, this, std::placeholders::_1));
}

ExternalLocalizationServiceNode::ReadinessConfig ExternalLocalizationServiceNode::load_readiness_config()
{
  ReadinessConfig config;
  config.telemetry_topic = declare_parameter<std::string>(
    "external_telemetry_topic", "/isaac/external_localization/telemetry");
  config.expected_reference_source_name = declare_parameter<std::string>(
    "expected_reference_source_name", "isaac_sim_truth_source");
  config.expected_workcell_zone_id = declare_parameter<std::string>(
    "expected_workcell_zone_id", "");
  config.telemetry_timeout_sec = declare_parameter<double>("telemetry_timeout_sec", 1.0);
  config.readiness_max_position_stddev_m = declare_parameter<double>(
    "readiness_max_position_stddev_m", 0.05);
  config.readiness_max_yaw_stddev_rad = declare_parameter<double>(
    "readiness_max_yaw_stddev_rad", 0.05);
  config.readiness_max_tracking_loss_ratio = declare_parameter<double>(
    "readiness_max_tracking_loss_ratio", 0.05);
  config.readiness_max_time_sync_offset_ms = declare_parameter<double>(
    "readiness_max_time_sync_offset_ms", 50.0);
  config.require_valid_pose = declare_parameter<bool>("require_valid_pose", true);
  return config;
}

ExternalReferenceExecutor::DataInputConfig ExternalLocalizationServiceNode::load_data_input_config()
{
  ExternalReferenceExecutor::DataInputConfig config;
  config.data_window_start_timestamp_us =
    declare_parameter<int64_t>("data_input.data_window_start_timestamp_us", 0);
  config.data_window_end_timestamp_us =
    declare_parameter<int64_t>("data_input.data_window_end_timestamp_us", 0);
  config.external_observation_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.external_observation_files", std::vector<std::string>{}),
    "external localization observation data");
  config.marker_observation_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.marker_observation_files", std::vector<std::string>{}),
    "external marker observation data");
  config.time_sync_diagnostic_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.time_sync_diagnostic_files", std::vector<std::string>{}),
    "external time synchronization diagnostics");
  return config;
}

void ExternalLocalizationServiceNode::handle_external_telemetry(const ExternalTelemetry::SharedPtr msg)
{
  std::lock_guard<std::mutex> lock(telemetry_mutex_);
  telemetry_snapshot_.latest = *msg;
  telemetry_snapshot_.last_received_timestamp_us = msg->hardware_timestamp_us;
  telemetry_snapshot_.last_receive_wall_time_us = now_us();
  telemetry_snapshot_.has_sample = true;
  telemetry_snapshot_.history.push_back(*msg);
  constexpr size_t kMaxHistorySize = 200;
  if (telemetry_snapshot_.history.size() > kMaxHistorySize) {
    telemetry_snapshot_.history.erase(
      telemetry_snapshot_.history.begin(),
      telemetry_snapshot_.history.begin() +
      static_cast<std::ptrdiff_t>(telemetry_snapshot_.history.size() - kMaxHistorySize));
  }
}

void ExternalLocalizationServiceNode::append_issue(
  calibration_external_localization_interfaces::msg::ExternalLocalizationValidationSummary & summary,
  const std::string & issue_code,
  const std::string & message,
  uint16_t error_code,
  bool blocking,
  const std::string & related_resource_id) const
{
  ReadinessIssue issue;
  issue.issue_code = issue_code;
  issue.message = message;
  issue.mapped_error_code.code = error_code;
  issue.blocking = blocking;
  issue.related_resource_id = related_resource_id;
  summary.issues.push_back(issue);
}

void ExternalLocalizationServiceNode::populate_readiness_from_snapshot(
  calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse & readiness) const
{
  readiness.success = true;
  readiness.error_code.code = ErrorCode::OK;
  readiness.message = "external_localization_service is ready.";
  readiness.agent_ready = true;
  readiness.ready_for_reference_validation = true;
  readiness.checked_timestamp_us = now_us();
  readiness.validation_summary.time_sync_ok = true;
  readiness.validation_summary.coverage_ok = true;
  readiness.validation_summary.quality_ok = true;
  readiness.validation_summary.tracking_stable = true;
  readiness.validation_summary.recommended_as_truth_source = true;
  readiness.validation_summary.position_stddev_m = 0.0;
  readiness.validation_summary.yaw_stddev_rad = 0.0;
  readiness.validation_summary.tracking_loss_ratio = 0.0;
  readiness.validation_summary.time_sync_offset_ms = 0.0;
  readiness.validation_summary.issues.clear();

  TelemetrySnapshot snapshot;
  {
    std::lock_guard<std::mutex> lock(telemetry_mutex_);
    snapshot = telemetry_snapshot_;
  }

  if (!snapshot.has_sample) {
    readiness.success = false;
    readiness.agent_ready = false;
    readiness.ready_for_reference_validation = false;
    readiness.error_code.code = ErrorCode::NOT_READY;
    readiness.message = "尚未收到 external 仿真遥测。";
    append_issue(
      readiness.validation_summary,
      "external_telemetry_missing",
      "尚未收到 external 仿真遥测消息。",
      ErrorCode::NOT_READY,
      true,
      readiness_config_.telemetry_topic);
    return;
  }

  const double telemetry_age_sec =
    static_cast<double>(now_us() - snapshot.last_receive_wall_time_us) / 1e6;
  const auto & latest = snapshot.latest;
  readiness.validation_summary.position_stddev_m = latest.position_stddev_m;
  readiness.validation_summary.yaw_stddev_rad = latest.yaw_stddev_rad;
  readiness.validation_summary.tracking_loss_ratio = latest.tracking_loss_ratio;
  readiness.validation_summary.time_sync_offset_ms = std::abs(latest.time_sync_offset_ms);

  if (telemetry_age_sec > readiness_config_.telemetry_timeout_sec) {
    readiness.ready_for_reference_validation = false;
    readiness.agent_ready = false;
    readiness.success = false;
    readiness.error_code.code = ErrorCode::TIMEOUT;
    readiness.message = "external 仿真遥测超时。";
    append_issue(
      readiness.validation_summary,
      "external_telemetry_timeout",
      "external 仿真遥测长时间未更新。",
      ErrorCode::TIMEOUT,
      true,
      readiness_config_.telemetry_topic);
  }

  if (readiness_config_.require_valid_pose && !latest.pose_valid) {
    readiness.validation_summary.coverage_ok = false;
    readiness.validation_summary.quality_ok = false;
    readiness.validation_summary.tracking_stable = false;
    readiness.ready_for_reference_validation = false;
    readiness.success = false;
    readiness.error_code.code = ErrorCode::DATA_QUALITY_INSUFFICIENT;
    readiness.message = "external 仿真遥测位姿无效。";
    append_issue(
      readiness.validation_summary,
      "external_pose_invalid",
      "最新 external 仿真遥测位姿无效。",
      ErrorCode::DATA_QUALITY_INSUFFICIENT,
      true,
      latest.reference_source_name);
  }

  if (!readiness_config_.expected_reference_source_name.empty() &&
    latest.reference_source_name != readiness_config_.expected_reference_source_name)
  {
    readiness.validation_summary.coverage_ok = false;
    readiness.ready_for_reference_validation = false;
    readiness.success = false;
    readiness.error_code.code = ErrorCode::INVALID_ARGUMENT;
    readiness.message = "external 真值源名称不匹配。";
    append_issue(
      readiness.validation_summary,
      "reference_source_mismatch",
      "收到的 external 真值源名称与预期不一致。",
      ErrorCode::INVALID_ARGUMENT,
      true,
      latest.reference_source_name);
  }

  readiness.validation_summary.time_sync_ok =
    std::abs(latest.time_sync_offset_ms) <= readiness_config_.readiness_max_time_sync_offset_ms;
  if (!readiness.validation_summary.time_sync_ok) {
    readiness.ready_for_reference_validation = false;
    readiness.success = false;
    readiness.error_code.code = ErrorCode::DATA_QUALITY_INSUFFICIENT;
    readiness.message = "external 时间同步偏差超限。";
    append_issue(
      readiness.validation_summary,
      "external_time_sync_exceeded",
      "external 时间同步偏差超出阈值。",
      ErrorCode::DATA_QUALITY_INSUFFICIENT,
      true,
      latest.reference_source_name);
  }

  readiness.validation_summary.quality_ok =
    latest.position_stddev_m <= readiness_config_.readiness_max_position_stddev_m &&
    latest.yaw_stddev_rad <= readiness_config_.readiness_max_yaw_stddev_rad &&
    latest.quality_score > 0.5;
  if (!readiness.validation_summary.quality_ok) {
    readiness.ready_for_reference_validation = false;
    readiness.success = false;
    readiness.error_code.code = ErrorCode::DATA_QUALITY_INSUFFICIENT;
    readiness.message = "external 观测质量不足。";
    append_issue(
      readiness.validation_summary,
      "external_quality_insufficient",
      "external 遥测标准差或质量分数不满足阈值。",
      ErrorCode::DATA_QUALITY_INSUFFICIENT,
      true,
      latest.reference_source_name);
  }

  readiness.validation_summary.tracking_stable =
    latest.tracking_loss_ratio <= readiness_config_.readiness_max_tracking_loss_ratio;
  if (!readiness.validation_summary.tracking_stable) {
    readiness.ready_for_reference_validation = false;
    readiness.success = false;
    readiness.error_code.code = ErrorCode::DATA_QUALITY_INSUFFICIENT;
    readiness.message = "external 跟踪稳定性不足。";
    append_issue(
      readiness.validation_summary,
      "external_tracking_unstable",
      "external 跟踪丢失比例超出阈值。",
      ErrorCode::DATA_QUALITY_INSUFFICIENT,
      true,
      latest.reference_source_name);
  }

  readiness.validation_summary.coverage_ok = latest.observed_target_count > 0;
  if (!readiness.validation_summary.coverage_ok) {
    readiness.ready_for_reference_validation = false;
    readiness.success = false;
    readiness.error_code.code = ErrorCode::DATA_QUALITY_INSUFFICIENT;
    readiness.message = "external 观测覆盖不足。";
    append_issue(
      readiness.validation_summary,
      "external_target_missing",
      "external 遥测没有有效目标观测。",
      ErrorCode::DATA_QUALITY_INSUFFICIENT,
      true,
      latest.reference_source_name);
  }

  readiness.validation_summary.recommended_as_truth_source =
    readiness.validation_summary.time_sync_ok &&
    readiness.validation_summary.coverage_ok &&
    readiness.validation_summary.quality_ok &&
    readiness.validation_summary.tracking_stable;

  if (!readiness.success && readiness.validation_summary.issues.empty()) {
    append_issue(
      readiness.validation_summary,
      "external_unknown_failure",
      "external readiness 未通过，但没有生成明确诊断。",
      ErrorCode::INTERNAL_ERROR,
      true,
      latest.reference_source_name);
  }
}

void ExternalLocalizationServiceNode::handle_readiness(
  const std::shared_ptr<ReadinessSrv::Request> request,
  std::shared_ptr<ReadinessSrv::Response> response)
{
  (void)request;
  populate_readiness_from_snapshot(response->response);
}

rclcpp_action::GoalResponse ExternalLocalizationServiceNode::handle_goal(
  const rclcpp_action::GoalUUID & /*uuid*/,
  std::shared_ptr<const ExecuteTask::Goal> goal)
{
  std::string reject_reason;
  if (!executor_.validate_goal(*goal, reject_reason)) {
    RCLCPP_WARN(get_logger(), "Reject external_localization goal: %s", reject_reason.c_str());
    return rclcpp_action::GoalResponse::REJECT;
  }

  calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse readiness;
  populate_readiness_from_snapshot(readiness);
  if (!readiness.ready_for_reference_validation) {
    RCLCPP_WARN(get_logger(), "Reject external_localization goal because readiness failed: %s", readiness.message.c_str());
    return rclcpp_action::GoalResponse::REJECT;
  }
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

rclcpp_action::CancelResponse ExternalLocalizationServiceNode::handle_cancel(
  const std::shared_ptr<GoalHandleExecuteTask> /*goal_handle*/)
{
  return rclcpp_action::CancelResponse::ACCEPT;
}

void ExternalLocalizationServiceNode::handle_accepted(
  const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
{
  std::thread(std::bind(&ExternalLocalizationServiceNode::execute_goal, this, goal_handle)).detach();
}

void ExternalLocalizationServiceNode::execute_goal(
  const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
{
  auto feedback = std::make_shared<ExecuteTask::Feedback>();
  feedback->feedback.job_id = goal_handle->get_goal()->goal.header.request_id;
  feedback->feedback.state.state = JobState::RUNNING;
  feedback->feedback.progress = 0.5;
  feedback->feedback.error_code.code = ErrorCode::OK;
  feedback->feedback.message = "external localization task is running.";
  feedback->feedback.server_timestamp_us = now_us();
  feedback->feedback.safe_to_retry = false;
  goal_handle->publish_feedback(feedback);

  auto result = std::make_shared<ExecuteTask::Result>();
  std::string failure_reason;
  calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse readiness;
  populate_readiness_from_snapshot(readiness);

  std::vector<ExternalTelemetry> telemetry_history;
  {
    std::lock_guard<std::mutex> lock(telemetry_mutex_);
    telemetry_history = telemetry_snapshot_.history;
  }

  if (!executor_.build_result(
      *goal_handle->get_goal(),
      readiness,
      telemetry_history,
      data_input_config_,
      *result,
      failure_reason))
  {
    result->result.success = false;
    result->result.error_code.code =
      readiness.success ? ErrorCode::INVALID_ARGUMENT : readiness.error_code.code;
    result->result.message = failure_reason.empty() ? readiness.message : failure_reason;
    result->result.validation_summary = readiness.validation_summary;
    goal_handle->abort(result);
    return;
  }

  goal_handle->succeed(result);
}

}  // namespace external_localization_service
