#pragma once

#include <memory>
#include <mutex>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp/subscription.hpp"

#include "calibration_common_interfaces/msg/readiness_issue.hpp"
#include "calibration_external_localization_interfaces/action/execute_external_localization_task.hpp"
#include "calibration_external_localization_interfaces/msg/external_localization_telemetry.hpp"
#include "calibration_external_localization_interfaces/srv/get_external_localization_readiness.hpp"
#include "external_localization_service/external_reference_executor.hpp"

namespace external_localization_service
{

// 这个类是 external_localization_service 的 ROS 接口层。
// 它的职责不是实现外部定位算法本体，而是：
// 1. 暴露 readiness service 与 execute action；
// 2. 接收 ROS 请求并做最小入口校验；
// 3. 调用 ExternalReferenceExecutor 组装算法输入并执行；
// 4. 将算法输出转换为 ROS action result / feedback。
class ExternalLocalizationServiceNode : public rclcpp::Node
{
public:
  explicit ExternalLocalizationServiceNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

private:
  using ReadinessSrv = calibration_external_localization_interfaces::srv::GetExternalLocalizationReadiness;
  using ExecuteTask = calibration_external_localization_interfaces::action::ExecuteExternalLocalizationTask;
  using GoalHandleExecuteTask = rclcpp_action::ServerGoalHandle<ExecuteTask>;
  using ExternalTelemetry = calibration_external_localization_interfaces::msg::ExternalLocalizationTelemetry;
  using ReadinessIssue = calibration_common_interfaces::msg::ReadinessIssue;

  struct ReadinessConfig
  {
    std::string telemetry_topic;
    std::string expected_reference_source_name;
    std::string expected_workcell_zone_id;
    double telemetry_timeout_sec{1.0};
    double readiness_max_position_stddev_m{0.05};
    double readiness_max_yaw_stddev_rad{0.05};
    double readiness_max_tracking_loss_ratio{0.05};
    double readiness_max_time_sync_offset_ms{50.0};
    bool require_valid_pose{true};
  };

  struct TelemetrySnapshot
  {
    ExternalTelemetry latest;
    std::vector<ExternalTelemetry> history;
    int64_t last_received_timestamp_us{0};
    int64_t last_receive_wall_time_us{0};
    bool has_sample{false};
  };

  ReadinessConfig load_readiness_config();
  ExternalReferenceExecutor::DataInputConfig load_data_input_config();
  void handle_external_telemetry(const ExternalTelemetry::SharedPtr msg);
  void 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;
  void populate_readiness_from_snapshot(
    calibration_external_localization_interfaces::msg::ExternalLocalizationReadinessResponse & readiness) const;

  ReadinessConfig readiness_config_;
  ExternalReferenceExecutor::DataInputConfig data_input_config_;
  mutable std::mutex telemetry_mutex_;
  TelemetrySnapshot telemetry_snapshot_;
  rclcpp::Subscription<ExternalTelemetry>::SharedPtr telemetry_subscription_;
  rclcpp::Service<ReadinessSrv>::SharedPtr readiness_service_;
  rclcpp_action::Server<ExecuteTask>::SharedPtr execute_task_action_server_;
  ExternalReferenceExecutor executor_;

  void handle_readiness(
    const std::shared_ptr<ReadinessSrv::Request> request,
    std::shared_ptr<ReadinessSrv::Response> response);
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const ExecuteTask::Goal> goal);
  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
  void handle_accepted(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
  void execute_goal(const std::shared_ptr<GoalHandleExecuteTask> goal_handle);
};

}  // namespace external_localization_service
