#pragma once

#include <cstdint>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>

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

#include "sensor_calibration_service/sensor_calibration_algorithm_template.hpp"

#include "calibration_common_interfaces/msg/file_reference.hpp"
#include "calibration_sensor_interfaces/action/execute_sensor_calibration_task.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_task_type.hpp"
#include "calibration_sensor_interfaces/srv/get_sensor_readiness.hpp"

namespace sensor_calibration_service
{

// 传感器标定服务节点：
// 1. 负责暴露 readiness service 和 execute action，作为传感器专项对外的 ROS 接口入口。
// 2. 负责把 ExecuteSensorCalibrationTask::Goal 组装成 SensorCalibrationInput。
// 3. 负责在进入算法前，先做最小任务合法性检查，并拆解常用任务字段。
// 4. 负责调用传感器算法门面，再把输出统一回填到 Action Result。
// 5. 这里不直接写具体标定算法，避免算法工程师在修改相机/IMU/外参/手眼逻辑时碰 ROS 通信代码。
class SensorCalibrationServiceNode : public rclcpp::Node
{
public:
  explicit SensorCalibrationServiceNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

private:
  using ReadinessSrv = calibration_sensor_interfaces::srv::GetSensorReadiness;
  using ExecuteTask = calibration_sensor_interfaces::action::ExecuteSensorCalibrationTask;
  using GoalHandleExecuteTask = rclcpp_action::ServerGoalHandle<ExecuteTask>;
  using TaskType = calibration_sensor_interfaces::msg::SensorCalibrationTaskType;

  struct ReadinessConfig
  {
    std::string storage_root;
    std::string capture_pipeline_name;
    std::string sensor_registry_raw;
    std::string telemetry_topics_raw;
    std::string arm_id;
    bool vehicle_safe_to_move{true};
    bool arm_ready{true};
    std::vector<std::string> ready_sensor_ids;
  };

  struct DataInputConfig
  {
    int64_t data_window_start_timestamp_us{0};
    int64_t data_window_end_timestamp_us{0};
    std::vector<calibration_common_interfaces::msg::FileReference> image_sample_files;
    std::vector<calibration_common_interfaces::msg::FileReference> imu_sample_files;
    std::vector<calibration_common_interfaces::msg::FileReference> pointcloud_sample_files;
    std::vector<calibration_common_interfaces::msg::FileReference> laser_scan_sample_files;
    std::vector<calibration_common_interfaces::msg::FileReference> target_detection_files;
    std::vector<calibration_common_interfaces::msg::FileReference> robot_pose_sample_files;
    std::vector<calibration_common_interfaces::msg::FileReference> synchronized_dataset_files;
  };

  rclcpp::Service<ReadinessSrv>::SharedPtr readiness_service_;
  rclcpp_action::Server<ExecuteTask>::SharedPtr execute_task_action_server_;
  SensorCalibrationAlgorithmTemplate algorithm_;
  ReadinessConfig readiness_config_;
  DataInputConfig data_input_config_;

  ReadinessConfig load_readiness_config();
  DataInputConfig load_data_input_config();
  std::vector<std::string> parse_csv_list(const std::string & raw) const;
  void append_issue(
    calibration_sensor_interfaces::msg::SensorReadinessResponse & response,
    const std::string & issue_code,
    const std::string & message,
    uint16_t error_code,
    bool blocking,
    const std::string & related_resource_id = "") const;

  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);

  std::string resolve_target_sensor_id(const ExecuteTask::Goal & goal) const;
};

}  // namespace sensor_calibration_service
