#include "chassis_calibration_service/chassis_calibration_service_node.hpp"

#include <chrono>
#include <future>
#include <thread>

#include "rclcpp_components/register_node_macro.hpp"

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

namespace chassis_calibration_service
{

namespace
{
// 统一使用微秒时间戳，方便和仓库里其他 msg 的 *_timestamp_us 字段对齐。
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;

ChassisCalibrationServiceNode::ChassisCalibrationServiceNode(const rclcpp::NodeOptions & options)
: Node("chassis_calibration_service", options)
{
  data_input_config_ = load_data_input_config();

  // readiness：供 orchestrator 在开跑前检查底盘是否具备执行条件。
  readiness_service_ = create_service<ReadinessSrv>(
    "/chassis/get_readiness",
    std::bind(&ChassisCalibrationServiceNode::handle_readiness, this, std::placeholders::_1, std::placeholders::_2));

  // 车辆画像客户端：底盘类型、车辆静态配置等都从这里查询。
  get_vehicle_profile_client_ = create_client<GetVehicleProfileSrv>("/vehicle_profile_manager/get_vehicle_profile");

  // action：底盘动作原语执行入口，后续真实算法也从这里接入。
  execute_task_action_server_ = rclcpp_action::create_server<ExecuteTask>(
    this,
    "/chassis/execute_motion_primitive",
    std::bind(&ChassisCalibrationServiceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
    std::bind(&ChassisCalibrationServiceNode::handle_cancel, this, std::placeholders::_1),
    std::bind(&ChassisCalibrationServiceNode::handle_accepted, this, std::placeholders::_1));
}

ChassisCalibrationServiceNode::DataInputConfig ChassisCalibrationServiceNode::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.chassis_motion_data_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.chassis_motion_data_files", std::vector<std::string>{}),
    "chassis motion data");
  config.actuator_command_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.actuator_command_files", std::vector<std::string>{}),
    "actuator command and feedback data");
  config.truth_trajectory_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.truth_trajectory_files", std::vector<std::string>{}),
    "external truth trajectory data");
  return config;
}

void ChassisCalibrationServiceNode::handle_readiness(
  const std::shared_ptr<ReadinessSrv::Request> request,
  std::shared_ptr<ReadinessSrv::Response> response)
{
  (void)request;

  // 当前先返回“全部就绪”的最小闭环状态，等真实底盘驱动/安全链路接入后再替换成真实检查。
  response->response.success = true;
  response->response.error_code.code = ErrorCode::OK;
  response->response.message = "chassis_calibration_service is ready.";
  response->response.agent_ready = true;
  response->response.chassis_driver_online = true;
  response->response.motion_control_ready = true;
  response->response.estop_released = true;
  response->response.vehicle_safe_to_move = true;
  response->response.telemetry_available = true;
  response->response.checked_timestamp_us = now_us();
}

rclcpp_action::GoalResponse ChassisCalibrationServiceNode::handle_goal(
  const rclcpp_action::GoalUUID & /*uuid*/,
  std::shared_ptr<const ExecuteTask::Goal> goal)
{
  // 最小合法性检查：只有 request_id 为空时拒绝。
  // 后续接真实算法时，可在这里继续加更严格的参数校验。
  if (goal->goal.header.request_id.empty()) {
    return rclcpp_action::GoalResponse::REJECT;
  }
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

rclcpp_action::CancelResponse ChassisCalibrationServiceNode::handle_cancel(
  const std::shared_ptr<GoalHandleExecuteTask> /*goal_handle*/)
{
  // 当前最小版本接受取消请求，但不在这里实现复杂中止逻辑。
  return rclcpp_action::CancelResponse::ACCEPT;
}

void ChassisCalibrationServiceNode::handle_accepted(
  const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
{
  // 长任务放后台线程，避免阻塞 ROS action server 的处理线程。
  std::thread(std::bind(&ChassisCalibrationServiceNode::execute_goal, this, goal_handle)).detach();
}

bool ChassisCalibrationServiceNode::fetch_chassis_type(
  const std::string & vehicle_id,
  ChassisCalibrationAlgorithmTemplate::ChassisType & chassis_type,
  std::string & failure_reason)
{
  if (vehicle_id.empty()) {
    failure_reason = "vehicle_id 不能为空，无法查询车辆画像。";
    return false;
  }

  if (!get_vehicle_profile_client_->wait_for_service(std::chrono::seconds(2))) {
    failure_reason = "车辆画像服务暂不可用。";
    return false;
  }

  auto request = std::make_shared<GetVehicleProfileSrv::Request>();
  request->request.vehicle_id = vehicle_id;
  auto future = get_vehicle_profile_client_->async_send_request(request);

  // 当前节点已经由 main/launch 的 executor spin，不能在执行线程里再次 spin 同一个节点。
  if (future.wait_for(std::chrono::seconds(2)) != std::future_status::ready) {
    failure_reason = "查询车辆画像超时。";
    return false;
  }

  const auto response = future.get();
  if (!response->response.success) {
    failure_reason = response->response.message.empty() ? "查询车辆画像失败。" : response->response.message;
    return false;
  }

  chassis_type = response->response.profile.chassis_type;
  if (chassis_type.value == ChassisCalibrationAlgorithmTemplate::ChassisType::CHASSIS_TYPE_UNSPECIFIED) {
    failure_reason = "车辆画像里没有配置底盘类型。";
    return false;
  }

  return true;
}

void ChassisCalibrationServiceNode::execute_goal(
  const std::shared_ptr<GoalHandleExecuteTask> goal_handle)
{
  // 先发一个进度反馈，让前端和 orchestrator 知道底盘任务已经开始运行。
  auto feedback = std::make_shared<ExecuteTask::Feedback>();
  feedback->feedback.state.state = JobState::RUNNING;
  goal_handle->publish_feedback(feedback);

  // 底盘算法本体通过模板类执行。
  // 这里先把 ROS goal 转成模板输入，再让模板内部按 chassis_type 派发到不同底盘分支。
  // chassis_type 不再靠 request_id 猜，而是从车辆画像服务中按 vehicle_id 查询。
  ChassisCalibrationAlgorithmTemplate::Input input;
  input.request = *goal_handle->get_goal();
  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.chassis_motion_data_files = data_input_config_.chassis_motion_data_files;
  input.actuator_command_files = data_input_config_.actuator_command_files;
  input.truth_trajectory_files = data_input_config_.truth_trajectory_files;
  std::string failure_reason;
  if (!fetch_chassis_type(
        input.request.goal.header.vehicle_id,
        input.chassis_type,
        failure_reason)) {
    auto result = std::make_shared<ExecuteTask::Result>();
    result->result.success = false;
    result->result.error_code.code = ErrorCode::INVALID_ARGUMENT;
    result->result.message = failure_reason;
    result->result.job_id = input.request.goal.header.request_id;
    result->result.data_quality_passed = false;
    result->result.suitable_for_commit = false;
    result->result.recommended_parameter_version.clear();
    goal_handle->abort(result);
    return;
  }

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

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

}  // namespace chassis_calibration_service

RCLCPP_COMPONENTS_REGISTER_NODE(chassis_calibration_service::ChassisCalibrationServiceNode)
