#include "control_calibration_service/control_calibration_service_node.hpp"

#include <chrono>
#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_control_interfaces/msg/control_job_result.hpp"
#include "calibration_control_interfaces/msg/control_readiness_response.hpp"
#include "calibration_control_interfaces/msg/controller_evaluation_task_type.hpp"

namespace control_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;
using calibration_control_interfaces::msg::ControllerEvaluationTaskType;

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

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

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

ControlCalibrationServiceNode::DataInputConfig ControlCalibrationServiceNode::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.control_evaluation_data_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.control_evaluation_data_files", std::vector<std::string>{}),
    "control evaluation data");
  config.reference_signal_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.reference_signal_files", std::vector<std::string>{}),
    "control reference signal data");
  config.chassis_response_files = make_file_references(
    declare_parameter<std::vector<std::string>>(
      "data_input.chassis_response_files", std::vector<std::string>{}),
    "chassis response data for control evaluation");
  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 ControlCalibrationServiceNode::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 = "control_calibration_service is ready.";
  response->response.agent_ready = true;
  response->response.trajectory_executor_ready = true;
  response->response.vehicle_feedback_ready = true;
  response->response.control_output_ready = true;
  response->response.estop_released = true;
  response->response.vehicle_safe_to_move = true;
  response->response.checked_timestamp_us = now_us();
}

rclcpp_action::GoalResponse ControlCalibrationServiceNode::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 ControlCalibrationServiceNode::handle_cancel(
  const std::shared_ptr<GoalHandleExecuteTask> /*goal_handle*/)
{
  return rclcpp_action::CancelResponse::ACCEPT;
}

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

void ControlCalibrationServiceNode::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);

  // ===== 组装算法输入上下文 =====
  // 当前模板阶段先把“算法最常用的任务侧输入”显式展开。
  // 后续如果要接真实车辆画像、控制参数查询、历史遥测缓存、真值源缓存，
  // 也应继续在这里补齐并写入 ControlCalibrationInput。
  ControlCalibrationAlgorithmTemplate::Input input;
  input.request = *goal_handle->get_goal();
  input.controller_algorithm = resolve_controller_algorithm(*goal_handle->get_goal());
  input.task_type = goal_handle->get_goal()->goal.selected_task;
  input.trajectory_tracking_task = goal_handle->get_goal()->goal.trajectory_tracking;
  input.velocity_step_task = goal_handle->get_goal()->goal.velocity_step;
  input.acceleration_deceleration_task = goal_handle->get_goal()->goal.accel_decel;
  input.stop_accuracy_task = goal_handle->get_goal()->goal.stop_accuracy;
  input.reference_trajectory = goal_handle->get_goal()->goal.trajectory_tracking.path;
  input.reference_target_velocity_ms = goal_handle->get_goal()->goal.velocity_step.target_velocity_ms;
  input.reference_start_velocity_ms = goal_handle->get_goal()->goal.accel_decel.start_velocity_ms;
  input.reference_target_accel_ms2 = goal_handle->get_goal()->goal.accel_decel.target_accel_ms2;
  input.reference_hold_time_sec = goal_handle->get_goal()->goal.velocity_step.hold_time_sec;
  input.reference_timeout_sec = goal_handle->get_goal()->goal.trajectory_tracking.timeout_sec;
  input.reference_stop_x_m = goal_handle->get_goal()->goal.stop_accuracy.target_stop_x_m;
  input.reference_stop_y_m = goal_handle->get_goal()->goal.stop_accuracy.target_stop_y_m;
  input.reference_stop_yaw_rad = goal_handle->get_goal()->goal.stop_accuracy.target_stop_yaw_rad;
  input.reference_stop_at_end = goal_handle->get_goal()->goal.trajectory_tracking.stop_at_end;
  input.control_history_available = false;
  input.chassis_history_available = false;
  input.sensor_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.control_evaluation_data_files = data_input_config_.control_evaluation_data_files;
  input.reference_signal_files = data_input_config_.reference_signal_files;
  input.chassis_response_files = data_input_config_.chassis_response_files;
  input.truth_trajectory_files = data_input_config_.truth_trajectory_files;

  ControlCalibrationAlgorithmTemplate::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);
}

ControlCalibrationServiceNode::ControllerAlgorithmType
ControlCalibrationServiceNode::resolve_controller_algorithm(const ExecuteTask::Goal & goal) const
{
  // 这里给出一版“任务类型 -> 默认控制算法类型”的模板映射。
  // 目的不是固定真实系统必须这么做，而是先告诉算法工程师：
  // node 层会在进入算法前给出一个默认算法分支，后续如果项目里已有更精确的控制器选择策略，
  // 可以把这里替换成：
  // 1. 来自 vehicle_profile.controller_selections 的静态配置；
  // 2. 来自 session 的阶段策略；
  // 3. 来自上层显式指定的本轮控制器算法。
  ControllerAlgorithmType algorithm;

  switch (goal.goal.selected_task.value) {
    case ControllerEvaluationTaskType::TRAJECTORY_TRACKING:
      algorithm.value = ControllerAlgorithmType::MPC;
      break;
    case ControllerEvaluationTaskType::VELOCITY_STEP:
      algorithm.value = ControllerAlgorithmType::PID;
      break;
    case ControllerEvaluationTaskType::ACCELERATION_DECELERATION:
      algorithm.value = ControllerAlgorithmType::LQR;
      break;
    case ControllerEvaluationTaskType::STOP_ACCURACY:
      algorithm.value = ControllerAlgorithmType::PURE_PURSUIT;
      break;
    default:
      algorithm.value = ControllerAlgorithmType::CONTROLLER_ALGORITHM_UNSPECIFIED;
      break;
  }

  return algorithm;
}

}  // namespace control_calibration_service

RCLCPP_COMPONENTS_REGISTER_NODE(control_calibration_service::ControlCalibrationServiceNode)
