#include "sensor_calibration_service/sensor_calibration_service_node.hpp"

#include <chrono>
#include <filesystem>
#include <sstream>
#include <thread>

#include "calibration_common_interfaces/msg/error_code.hpp"
#include "calibration_common_interfaces/msg/job_state.hpp"
#include "calibration_common_interfaces/msg/readiness_issue.hpp"
#include "calibration_sensor_interfaces/msg/capture_intent_type.hpp"
#include "calibration_sensor_interfaces/msg/sensor_calibration_job_result.hpp"
#include "calibration_sensor_interfaces/msg/sensor_readiness_response.hpp"
#include "calibration_vehicle_profile_interfaces/msg/sensor_type.hpp"

namespace sensor_calibration_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;

SensorCalibrationServiceNode::SensorCalibrationServiceNode(const rclcpp::NodeOptions & options)
: Node("sensor_calibration_service", options)
{
  readiness_config_ = load_readiness_config();
  data_input_config_ = load_data_input_config();

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

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

void SensorCalibrationServiceNode::handle_readiness(
  const std::shared_ptr<ReadinessSrv::Request> request,
  std::shared_ptr<ReadinessSrv::Response> response)
{
  (void)request;
  auto & rsp = response->response;
  rsp.success = true;
  rsp.error_code.code = ErrorCode::OK;
  rsp.message = "sensor_calibration_service is ready.";
  rsp.agent_ready = true;
  rsp.capture_pipeline_ready = !readiness_config_.capture_pipeline_name.empty();
  rsp.storage_ready = !readiness_config_.storage_root.empty() &&
    std::filesystem::exists(readiness_config_.storage_root) &&
    std::filesystem::is_directory(readiness_config_.storage_root);
  rsp.telemetry_ready = !readiness_config_.telemetry_topics_raw.empty();
  rsp.vehicle_safe_to_move = readiness_config_.vehicle_safe_to_move;
  rsp.arm_ready = readiness_config_.arm_ready;
  rsp.ready_sensor_ids = readiness_config_.ready_sensor_ids;
  rsp.checked_timestamp_us = now_us();

  if (!rsp.capture_pipeline_ready) {
    append_issue(
      rsp, "capture_pipeline_missing", "未配置 capture_pipeline_name。",
      ErrorCode::NOT_READY, true);
  }
  if (!rsp.storage_ready) {
    append_issue(
      rsp, "storage_root_unavailable", "sensor_storage_root 不存在或不是目录。",
      ErrorCode::FILE_NOT_FOUND, true, readiness_config_.storage_root);
  }
  if (!rsp.telemetry_ready) {
    append_issue(
      rsp, "telemetry_topics_missing", "未配置 telemetry_topics，无法确认采集反馈链路。",
      ErrorCode::NOT_READY, true);
  }
  if (rsp.ready_sensor_ids.empty()) {
    append_issue(
      rsp, "sensor_registry_empty", "未配置可用传感器列表 sensor_registry。",
      ErrorCode::INVALID_ARGUMENT, true);
  }
  if (!rsp.vehicle_safe_to_move) {
    append_issue(
      rsp, "vehicle_not_safe_to_move", "当前配置声明车辆不可移动。",
      ErrorCode::SAFETY_TRIGGERED, true);
  }
  if (!rsp.arm_ready && !readiness_config_.arm_id.empty()) {
    append_issue(
      rsp, "arm_not_ready", "当前配置声明机械臂未就绪。",
      ErrorCode::NOT_READY, true, readiness_config_.arm_id);
  }

  if (!rsp.issues.empty()) {
    rsp.success = false;
    rsp.agent_ready = false;
    rsp.message = "sensor_calibration_service readiness 检查未通过。";
    rsp.error_code.code = rsp.storage_ready ? ErrorCode::NOT_READY : ErrorCode::FILE_NOT_FOUND;
  }
}

rclcpp_action::GoalResponse SensorCalibrationServiceNode::handle_goal(
  const rclcpp_action::GoalUUID & /*uuid*/,
  std::shared_ptr<const ExecuteTask::Goal> goal)
{
  if (goal->goal.header.request_id.empty()) {
    return rclcpp_action::GoalResponse::REJECT;
  }
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

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

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

SensorCalibrationServiceNode::ReadinessConfig SensorCalibrationServiceNode::load_readiness_config()
{
  ReadinessConfig config;
  config.storage_root = declare_parameter<std::string>("sensor_storage_root", "/tmp/agv_sensor_calibration");
  config.capture_pipeline_name = declare_parameter<std::string>("capture_pipeline_name", "sensor_capture_pipeline");
  config.sensor_registry_raw = declare_parameter<std::string>("sensor_registry", "demo_sensor_001");
  config.telemetry_topics_raw = declare_parameter<std::string>("telemetry_topics", "/sensor_calibration/telemetry");
  config.arm_id = declare_parameter<std::string>("arm_id", "");
  config.vehicle_safe_to_move = declare_parameter<bool>("vehicle_safe_to_move", true);
  config.arm_ready = declare_parameter<bool>("arm_ready", true);
  config.ready_sensor_ids = parse_csv_list(config.sensor_registry_raw);
  return config;
}

SensorCalibrationServiceNode::DataInputConfig SensorCalibrationServiceNode::load_data_input_config()
{
  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.image_sample_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.image_sample_files", std::vector<std::string>{}),
    "sensor image samples");
  config.imu_sample_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.imu_sample_files", std::vector<std::string>{}),
    "sensor imu samples");
  config.pointcloud_sample_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.pointcloud_sample_files", std::vector<std::string>{}),
    "sensor pointcloud samples");
  config.laser_scan_sample_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.laser_scan_sample_files", std::vector<std::string>{}),
    "sensor laser scan samples");
  config.target_detection_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.target_detection_files", std::vector<std::string>{}),
    "sensor calibration target detection results");
  config.robot_pose_sample_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.robot_pose_sample_files", std::vector<std::string>{}),
    "robot or platform pose samples");
  config.synchronized_dataset_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.synchronized_dataset_files", std::vector<std::string>{}),
    "synchronized sensor calibration dataset index");

  return config;
}

std::vector<std::string> SensorCalibrationServiceNode::parse_csv_list(const std::string & raw) const
{
  std::vector<std::string> items;
  std::stringstream ss(raw);
  std::string item;
  while (std::getline(ss, item, ',')) {
    const auto begin = item.find_first_not_of(" \t\n\r");
    if (begin == std::string::npos) {
      continue;
    }
    const auto end = item.find_last_not_of(" \t\n\r");
    items.push_back(item.substr(begin, end - begin + 1));
  }
  return items;
}

void SensorCalibrationServiceNode::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
{
  calibration_common_interfaces::msg::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;
  response.issues.push_back(issue);
}

void SensorCalibrationServiceNode::execute_goal(
  const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
{
  // 先回一帧 RUNNING feedback，告诉 orchestrator 当前任务已经进入执行阶段。
  auto feedback = std::make_shared<ExecuteTask::Feedback>();
  feedback->feedback.state.state = JobState::RUNNING;
  goal_handle->publish_feedback(feedback);

  // ===== 组装算法输入上下文 =====
  // 当前模板阶段先把“算法最常用的任务侧输入”显式展开。
  // 后续如果要接真实车辆画像、已生效参数查询、历史遥测缓存、外部定位缓存，
  // 也应继续在这里补齐并写入 SensorCalibrationInput。
  SensorCalibrationAlgorithmTemplate::Input input;
  input.request = *goal_handle->get_goal();
  input.task_type = goal_handle->get_goal()->goal.selected_task;
  input.task_subtype = goal_handle->get_goal()->goal.task_subtype;
  input.target_sensor_id = resolve_target_sensor_id(*goal_handle->get_goal());
  input.camera_intrinsic_task = goal_handle->get_goal()->goal.camera_intrinsic;
  input.imu_intrinsic_task = goal_handle->get_goal()->goal.imu_intrinsic;
  input.sensor_to_base_extrinsic_task = goal_handle->get_goal()->goal.sensor_to_base_extrinsic;
  input.hand_eye_task = goal_handle->get_goal()->goal.hand_eye;
  input.sensor_readiness.success = true;
  input.sensor_readiness.error_code.code = ErrorCode::OK;
  input.sensor_readiness.agent_ready = true;
  input.sensor_readiness.capture_pipeline_ready = !readiness_config_.capture_pipeline_name.empty();
  input.sensor_readiness.storage_ready = !readiness_config_.storage_root.empty() &&
    std::filesystem::exists(readiness_config_.storage_root) &&
    std::filesystem::is_directory(readiness_config_.storage_root);
  input.sensor_readiness.telemetry_ready = !readiness_config_.telemetry_topics_raw.empty();
  input.sensor_readiness.vehicle_safe_to_move = readiness_config_.vehicle_safe_to_move;
  input.sensor_readiness.arm_ready = readiness_config_.arm_ready;
  input.sensor_readiness.ready_sensor_ids = readiness_config_.ready_sensor_ids;
  input.required_image_count = goal_handle->get_goal()->goal.camera_intrinsic.required_image_count;
  input.required_static_segment_count = goal_handle->get_goal()->goal.imu_intrinsic.required_static_segment_count;
  input.required_motion_segment_count = goal_handle->get_goal()->goal.imu_intrinsic.required_motion_segment_count;
  input.required_sample_count = goal_handle->get_goal()->goal.sensor_to_base_extrinsic.required_sample_count;
  input.required_pose_count = goal_handle->get_goal()->goal.hand_eye.required_pose_count;
  input.capture_quality_diagnostics.push_back("capture_pipeline=" + readiness_config_.capture_pipeline_name);
  input.storage_diagnostics.push_back("storage_root=" + readiness_config_.storage_root);
  input.truth_source_diagnostics.push_back("telemetry_topics=" + readiness_config_.telemetry_topics_raw);
  switch (goal_handle->get_goal()->goal.selected_task.value) {
    case TaskType::CAMERA_INTRINSIC:
      input.reference_timeout_sec = goal_handle->get_goal()->goal.camera_intrinsic.timeout_sec;
      break;
    case TaskType::IMU_INTRINSIC:
      input.reference_timeout_sec = goal_handle->get_goal()->goal.imu_intrinsic.timeout_sec;
      break;
    case TaskType::SENSOR_TO_BASE_EXTRINSIC:
      input.reference_timeout_sec = goal_handle->get_goal()->goal.sensor_to_base_extrinsic.timeout_sec;
      break;
    case TaskType::HAND_EYE:
      input.reference_timeout_sec = goal_handle->get_goal()->goal.hand_eye.timeout_sec;
      break;
    default:
      input.reference_timeout_sec = 0.0;
      break;
  }
  input.reference_board_id = goal_handle->get_goal()->goal.camera_intrinsic.target_board_id;
  input.reference_base_frame_id = goal_handle->get_goal()->goal.sensor_to_base_extrinsic.base_frame_id;
  input.reference_arm_id = goal_handle->get_goal()->goal.hand_eye.arm_id;
  if (goal_handle->get_goal()->goal.task_subtype.value ==
    calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype::FRONT_CAMERA_INTRINSIC ||
    goal_handle->get_goal()->goal.task_subtype.value ==
    calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype::FRONT_CAMERA_EXTRINSIC)
  {
    input.target_sensor_type.value = calibration_vehicle_profile_interfaces::msg::SensorType::FRONT_CAMERA;
  } else if (goal_handle->get_goal()->goal.task_subtype.value ==
    calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_INTRINSIC ||
    goal_handle->get_goal()->goal.task_subtype.value ==
    calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype::DOWNWARD_CAMERA_EXTRINSIC)
  {
    input.target_sensor_type.value = calibration_vehicle_profile_interfaces::msg::SensorType::DOWNWARD_CAMERA;
  } else if (goal_handle->get_goal()->goal.task_subtype.value ==
    calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype::IMU_INTRINSIC ||
    goal_handle->get_goal()->goal.task_subtype.value ==
    calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype::IMU_EXTRINSIC)
  {
    input.target_sensor_type.value = calibration_vehicle_profile_interfaces::msg::SensorType::IMU;
  } else if (goal_handle->get_goal()->goal.task_subtype.value ==
    calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype::LIDAR_2D_EXTRINSIC)
  {
    input.target_sensor_type.value = calibration_vehicle_profile_interfaces::msg::SensorType::LIDAR_2D;
  } else if (goal_handle->get_goal()->goal.task_subtype.value ==
    calibration_sensor_interfaces::msg::SensorCalibrationTaskSubtype::LIDAR_3D_EXTRINSIC)
  {
    input.target_sensor_type.value = calibration_vehicle_profile_interfaces::msg::SensorType::LIDAR_3D;
  }
  input.sensor_history_available = false;
  input.chassis_history_available = false;
  input.control_history_available = false;
  input.truth_history_available = false;
  input.data_window_start_timestamp_us = data_input_config_.data_window_start_timestamp_us;
  input.data_window_end_timestamp_us = data_input_config_.data_window_end_timestamp_us;
  input.image_sample_files = data_input_config_.image_sample_files;
  input.imu_sample_files = data_input_config_.imu_sample_files;
  input.pointcloud_sample_files = data_input_config_.pointcloud_sample_files;
  input.laser_scan_sample_files = data_input_config_.laser_scan_sample_files;
  input.target_detection_files = data_input_config_.target_detection_files;
  input.robot_pose_sample_files = data_input_config_.robot_pose_sample_files;
  input.synchronized_dataset_files = data_input_config_.synchronized_dataset_files;
  input.storage_diagnostics.push_back(
    "synchronized_dataset_files=" + std::to_string(input.synchronized_dataset_files.size()));

  SensorCalibrationAlgorithmTemplate::Output output;
  std::string failure_reason;
  if (!algorithm_.run(input, output, failure_reason)) {
    auto result = std::make_shared<ExecuteTask::Result>();
    result->result.success = false;
    result->result.error_code.code = ErrorCode::INVALID_STATE;
    result->result.message = failure_reason;
    result->result.job_id = goal_handle->get_goal()->goal.header.request_id;
    result->result.data_quality_passed = false;
    result->result.suitable_for_commit = false;
    goal_handle->abort(result);
    return;
  }

  auto result = std::make_shared<ExecuteTask::Result>(output.response);
  goal_handle->succeed(result);
}

std::string SensorCalibrationServiceNode::resolve_target_sensor_id(const ExecuteTask::Goal & goal) const
{
  switch (goal.goal.selected_task.value) {
    case TaskType::CAMERA_INTRINSIC:
      return goal.goal.camera_intrinsic.sensor_id;
    case TaskType::IMU_INTRINSIC:
      return goal.goal.imu_intrinsic.sensor_id;
    case TaskType::SENSOR_TO_BASE_EXTRINSIC:
      return goal.goal.sensor_to_base_extrinsic.sensor_id;
    case TaskType::HAND_EYE:
      return goal.goal.hand_eye.sensor_id;
    default:
      return "";
  }
}

}  // namespace sensor_calibration_service
