#include "vehicle_profile_manager/vehicle_profile_manager_node.hpp"

#include <chrono>
#include <filesystem>
#include <fstream>
#include <sstream>

#include "calibration_common_interfaces/msg/error_code.hpp"
#include "calibration_vehicle_profile_interfaces/msg/calibration_ability_type.hpp"
#include "calibration_vehicle_profile_interfaces/msg/camera_mount_type.hpp"
#include "calibration_vehicle_profile_interfaces/msg/control_axis_type.hpp"
#include "calibration_vehicle_profile_interfaces/msg/controller_algorithm_type.hpp"
#include "calibration_vehicle_profile_interfaces/msg/sensor_type.hpp"
#include "calibration_vehicle_profile_interfaces/msg/chassis_type.hpp"
#include "calibration_vehicle_profile_interfaces/msg/workflow_stage_type.hpp"
#include "rclcpp_components/register_node_macro.hpp"

namespace vehicle_profile_manager
{

using calibration_common_interfaces::msg::ErrorCode;
using calibration_vehicle_profile_interfaces::msg::CalibrationAbilityType;
using calibration_vehicle_profile_interfaces::msg::CameraMountType;
using calibration_vehicle_profile_interfaces::msg::ChassisType;
using calibration_vehicle_profile_interfaces::msg::ControlAxisType;
using calibration_vehicle_profile_interfaces::msg::ControllerAlgorithmType;
using calibration_vehicle_profile_interfaces::msg::SensorType;
using calibration_vehicle_profile_interfaces::msg::WorkflowStageType;

namespace
{

std::string trim_copy(const std::string & value)
{
  const auto begin = value.find_first_not_of(" \t\n\r");
  if (begin == std::string::npos) {
    return std::string();
  }
  const auto end = value.find_last_not_of(" \t\n\r");
  return value.substr(begin, end - begin + 1);
}

bool parse_bool_text(const std::string & value)
{
  return value == "1" || value == "true" || value == "TRUE" || value == "True";
}

std::vector<std::string> split_csv(const std::string & value)
{
  std::vector<std::string> items;
  std::stringstream ss(value);
  std::string item;
  while (std::getline(ss, item, ',')) {
    item = trim_copy(item);
    if (!item.empty()) {
      items.push_back(item);
    }
  }
  return items;
}

std::vector<std::string> split_pipe_rows(const std::string & value)
{
  std::vector<std::string> items;
  std::stringstream ss(value);
  std::string item;
  while (std::getline(ss, item, '|')) {
    item = trim_copy(item);
    if (!item.empty()) {
      items.push_back(item);
    }
  }
  return items;
}

}  // namespace

VehicleProfileManagerNode::VehicleProfileManagerNode(const rclcpp::NodeOptions & options)
: Node("vehicle_profile_manager", options)
{
  profile_storage_path_ = declare_parameter<std::string>(
    "profile_storage_path", "/tmp/agv_calib_vehicle_profiles.db");
  get_profile_service_ = create_service<GetProfileSrv>(
    "/vehicle_profile_manager/get_vehicle_profile",
    std::bind(
      &VehicleProfileManagerNode::handle_get_profile, this,
      std::placeholders::_1, std::placeholders::_2));

  register_service_ = create_service<RegisterSrv>(
    "/vehicle_profile_manager/register_or_update_vehicle_profile",
    std::bind(
      &VehicleProfileManagerNode::handle_register, this,
      std::placeholders::_1, std::placeholders::_2));

  applicability_service_ = create_service<ApplicabilitySrv>(
    "/vehicle_profile_manager/evaluate_vehicle_calibration_applicability",
    std::bind(
      &VehicleProfileManagerNode::handle_applicability, this,
      std::placeholders::_1, std::placeholders::_2));

  heartbeat_service_ = create_service<HeartbeatSrv>(
    "/vehicle_profile_manager/heartbeat",
    std::bind(
      &VehicleProfileManagerNode::handle_heartbeat, this,
      std::placeholders::_1, std::placeholders::_2));

  load_profiles_from_file();
  load_demo_profile_if_empty();

  RCLCPP_INFO(
    get_logger(), "VehicleProfileManagerNode 启动，当前画像数量=%zu，存储文件=%s",
    profiles_.size(), profile_storage_path_.c_str());
}

void VehicleProfileManagerNode::handle_get_profile(
  const std::shared_ptr<GetProfileSrv::Request> request,
  std::shared_ptr<GetProfileSrv::Response> response)
{
  const auto & vehicle_id = request->request.vehicle_id;
  auto it = profiles_.find(vehicle_id);
  if (it == profiles_.end()) {
    response->response.success = false;
    response->response.error_code.code = ErrorCode::INVALID_ARGUMENT;
    response->response.message = "找不到 vehicle_id=[" + vehicle_id + "] 的车辆画像。";
    return;
  }
  response->response.success = true;
  response->response.error_code.code = ErrorCode::OK;
  response->response.message = "查询成功。";
  response->response.profile = it->second;
}

void VehicleProfileManagerNode::handle_register(
  const std::shared_ptr<RegisterSrv::Request> request,
  std::shared_ptr<RegisterSrv::Response> response)
{
  const auto & vehicle_id = request->request.profile.base_info.vehicle_id;
  if (vehicle_id.empty()) {
    response->response.success = false;
    response->response.error_code.code = ErrorCode::INVALID_ARGUMENT;
    response->response.message = "vehicle_id 不能为空。";
    return;
  }
  auto profile = request->request.profile;
  profile.enabled_workflow_stages = evaluate_supported_stages(profile);
  profiles_[vehicle_id] = profile;

  std::string save_error;
  if (!save_profiles_to_file(save_error)) {
    response->response.success = false;
    response->response.error_code.code = ErrorCode::INTERNAL_ERROR;
    response->response.message = save_error;
    return;
  }

  RCLCPP_INFO(get_logger(), "已注册/更新车辆画像 vehicle_id=[%s]", vehicle_id.c_str());
  response->response.success = true;
  response->response.error_code.code = ErrorCode::OK;
  response->response.message = "注册/更新成功。";
}

void VehicleProfileManagerNode::handle_applicability(
  const std::shared_ptr<ApplicabilitySrv::Request> request,
  std::shared_ptr<ApplicabilitySrv::Response> response)
{
  const auto & profile = request->request.profile_snapshot;
  auto stages = evaluate_supported_stages(profile);

  response->response.success = true;
  response->response.error_code.code = ErrorCode::OK;
  response->response.message = "适用性评估完成。";
  response->response.overall_supported = !stages.empty();
  response->response.recommended_workflow_stages = stages;
}

void VehicleProfileManagerNode::handle_heartbeat(
  const std::shared_ptr<HeartbeatSrv::Request> /*request*/,
  std::shared_ptr<HeartbeatSrv::Response> response)
{
  const auto now_us = std::chrono::duration_cast<std::chrono::microseconds>(
    std::chrono::system_clock::now().time_since_epoch()).count();
  response->response.success = true;
  response->response.error_code.code = ErrorCode::OK;
  response->response.message = "vehicle_profile_manager 在线。";
  response->response.server_timestamp_us = now_us;
  response->response.vehicle_ready = true;
}

std::vector<WorkflowStageType>
VehicleProfileManagerNode::evaluate_supported_stages(const VehicleProfile & profile) const
{
  std::vector<WorkflowStageType> stages;

  auto make_stage = [](uint8_t v) {
    WorkflowStageType s;
    s.value = v;
    return s;
  };

  // 预检和画像校验始终支持。
  stages.push_back(make_stage(WorkflowStageType::PROFILE_VALIDATION_STAGE));
  stages.push_back(make_stage(WorkflowStageType::WORKSHOP_PRECHECK_STAGE));

  // 底盘标定：底盘类型已指定时支持。
  if (profile.chassis_type.value != ChassisType::CHASSIS_TYPE_UNSPECIFIED) {
    stages.push_back(make_stage(WorkflowStageType::CHASSIS_CALIBRATION_STAGE));
    stages.push_back(make_stage(WorkflowStageType::CONTROL_CALIBRATION_STAGE));
  }

  // 传感器标定：有传感器配置时支持。
  if (!profile.sensors.empty()) {
    stages.push_back(make_stage(WorkflowStageType::SENSOR_INTRINSIC_CALIBRATION_STAGE));
    stages.push_back(make_stage(WorkflowStageType::SENSOR_EXTRINSIC_CALIBRATION_STAGE));
  }

  // 手眼标定：有机械臂且有传感器时支持。
  if (profile.arm_profile.has_mechanical_arm && !profile.sensors.empty()) {
    stages.push_back(make_stage(WorkflowStageType::HAND_EYE_CALIBRATION_STAGE));
  }

  // 最终阶段始终加入。
  stages.push_back(make_stage(WorkflowStageType::PARAMETER_COMMIT_STAGE));
  stages.push_back(make_stage(WorkflowStageType::REPORT_ARCHIVE_STAGE));

  return stages;
}

void VehicleProfileManagerNode::load_profiles_from_file()
{
  profiles_.clear();
  if (profile_storage_path_.empty()) {
    RCLCPP_WARN(get_logger(), "profile_storage_path 为空，跳过画像文件加载。");
    return;
  }
  if (!std::filesystem::exists(profile_storage_path_)) {
    RCLCPP_WARN(get_logger(), "画像存储文件不存在：%s，后续将回退到 demo 画像。", profile_storage_path_.c_str());
    return;
  }

  std::ifstream ifs(profile_storage_path_);
  if (!ifs.is_open()) {
    RCLCPP_WARN(get_logger(), "无法打开画像存储文件：%s", profile_storage_path_.c_str());
    return;
  }

  std::stringstream buffer;
  buffer << ifs.rdbuf();
  for (const auto & block : parse_profile_blocks(buffer.str())) {
    auto profile = make_profile_from_kv_map(block);
    if (!profile.base_info.vehicle_id.empty()) {
      profile.enabled_workflow_stages = evaluate_supported_stages(profile);
      profiles_[profile.base_info.vehicle_id] = profile;
    }
  }
}

bool VehicleProfileManagerNode::save_profiles_to_file(std::string & error_message) const
{
  if (profile_storage_path_.empty()) {
    error_message = "profile_storage_path 为空，无法保存车辆画像。";
    return false;
  }

  const auto parent = std::filesystem::path(profile_storage_path_).parent_path();
  if (!parent.empty()) {
    std::error_code ec;
    std::filesystem::create_directories(parent, ec);
    if (ec) {
      error_message = "创建画像存储目录失败：" + ec.message();
      return false;
    }
  }

  std::ofstream ofs(profile_storage_path_, std::ios::trunc);
  if (!ofs.is_open()) {
    error_message = "无法打开画像存储文件进行写入：" + profile_storage_path_;
    return false;
  }

  for (const auto & [vehicle_id, profile] : profiles_) {
    ofs << "[profile]\n";
    ofs << "vehicle_id=" << vehicle_id << "\n";
    ofs << "vehicle_name=" << profile.base_info.vehicle_name << "\n";
    ofs << "model_name=" << profile.base_info.model_name << "\n";
    ofs << "serial_number=" << profile.base_info.serial_number << "\n";
    ofs << "manufacturer=" << profile.base_info.manufacturer << "\n";
    ofs << "description=" << profile.base_info.description << "\n";
    ofs << "profile_version=" << profile.profile_version << "\n";
    ofs << "base_link_frame=" << profile.base_link_frame << "\n";
    ofs << "chassis_type=" << static_cast<int>(profile.chassis_type.value) << "\n";
    ofs << "has_mechanical_arm=" << (profile.arm_profile.has_mechanical_arm ? "true" : "false") << "\n";
    ofs << "arm_id=" << profile.arm_profile.arm_id << "\n";
    ofs << "arm_model=" << profile.arm_profile.arm_model << "\n";
    ofs << "arm_dof=" << profile.arm_profile.dof << "\n";
    ofs << "arm_base_frame=" << profile.arm_profile.arm_base_frame << "\n";
    ofs << "tool_frame=" << profile.arm_profile.tool_frame << "\n";

    for (const auto & sensor : profile.sensors) {
      ofs << "sensor="
          << sensor.sensor_id << ","
          << static_cast<int>(sensor.sensor_type.value) << ","
          << sensor.sensor_name << ","
          << sensor.frame_id << ","
          << static_cast<int>(sensor.camera_mount_type.value) << ","
          << (sensor.enabled ? "true" : "false") << ","
          << (sensor.needs_intrinsic_calibration ? "true" : "false") << ","
          << (sensor.needs_extrinsic_calibration ? "true" : "false") << ","
          << sensor.device_hint << ","
          << (sensor.selected_for_this_session ? "true" : "false") << ","
          << (sensor.supports_intrinsic_calibration ? "true" : "false") << ","
          << (sensor.supports_extrinsic_calibration ? "true" : "false") << "\n";
    }

    for (const auto & cap : profile.capabilities) {
      ofs << "capability="
          << static_cast<int>(cap.ability_type.value) << ","
          << (cap.supported ? "true" : "false") << ","
          << cap.message << "\n";
    }

    for (const auto & controller : profile.controllers) {
      ofs << "controller="
          << static_cast<int>(controller.control_axis.value) << ","
          << static_cast<int>(controller.default_algorithm.value) << "\n";
    }

    ofs << "\n";
  }

  return true;
}

void VehicleProfileManagerNode::load_demo_profile_if_empty()
{
  if (!profiles_.empty()) {
    return;
  }
  load_demo_profile();
  std::string save_error;
  if (!save_profiles_to_file(save_error)) {
    RCLCPP_WARN(get_logger(), "写入 demo 画像到存储文件失败：%s", save_error.c_str());
  }
}

VehicleProfileManagerNode::VehicleProfile VehicleProfileManagerNode::make_profile_from_kv_map(
  const std::unordered_map<std::string, std::string> & values) const
{
  VehicleProfile profile;
  auto get = [&](const std::string & key) -> std::string {
    const auto it = values.find(key);
    return it == values.end() ? std::string() : it->second;
  };

  profile.base_info.vehicle_id = get("vehicle_id");
  profile.base_info.vehicle_name = get("vehicle_name");
  profile.base_info.model_name = get("model_name");
  profile.base_info.serial_number = get("serial_number");
  profile.base_info.manufacturer = get("manufacturer");
  profile.base_info.description = get("description");
  profile.profile_version = get("profile_version");
  profile.base_link_frame = get("base_link_frame");

  try {
    if (!get("chassis_type").empty()) {
      profile.chassis_type.value = static_cast<uint8_t>(std::stoul(get("chassis_type")));
    }
    if (!get("arm_dof").empty()) {
      profile.arm_profile.dof = static_cast<uint32_t>(std::stoul(get("arm_dof")));
    }
  } catch (...) {
  }

  profile.arm_profile.has_mechanical_arm = parse_bool_text(get("has_mechanical_arm"));
  profile.arm_profile.arm_id = get("arm_id");
  profile.arm_profile.arm_model = get("arm_model");
  profile.arm_profile.arm_base_frame = get("arm_base_frame");
  profile.arm_profile.tool_frame = get("tool_frame");

  const auto sensor_rows = split_pipe_rows(get("sensor_rows"));
  for (const auto & row : sensor_rows) {
    auto parts = split_csv(row);
    if (parts.size() < 12) {
      continue;
    }
    calibration_vehicle_profile_interfaces::msg::SensorProfile sensor;
    sensor.sensor_id = parts[0];
    sensor.sensor_type.value = static_cast<uint8_t>(std::stoul(parts[1]));
    sensor.sensor_name = parts[2];
    sensor.frame_id = parts[3];
    sensor.camera_mount_type.value = static_cast<uint8_t>(std::stoul(parts[4]));
    sensor.enabled = parse_bool_text(parts[5]);
    sensor.needs_intrinsic_calibration = parse_bool_text(parts[6]);
    sensor.needs_extrinsic_calibration = parse_bool_text(parts[7]);
    sensor.device_hint = parts[8];
    sensor.selected_for_this_session = parse_bool_text(parts[9]);
    sensor.supports_intrinsic_calibration = parse_bool_text(parts[10]);
    sensor.supports_extrinsic_calibration = parse_bool_text(parts[11]);
    profile.sensors.push_back(sensor);
  }

  const auto capability_rows = split_pipe_rows(get("capability_rows"));
  for (const auto & row : capability_rows) {
    auto parts = split_csv(row);
    if (parts.size() < 3) {
      continue;
    }
    calibration_vehicle_profile_interfaces::msg::CalibrationCapability capability;
    capability.ability_type.value = static_cast<uint8_t>(std::stoul(parts[0]));
    capability.supported = parse_bool_text(parts[1]);
    capability.message = parts[2];
    profile.capabilities.push_back(capability);
  }

  const auto controller_rows = split_pipe_rows(get("controller_rows"));
  for (const auto & row : controller_rows) {
    auto parts = split_csv(row);
    if (parts.size() < 2) {
      continue;
    }
    calibration_vehicle_profile_interfaces::msg::ControllerProfile controller;
    controller.control_axis.value = static_cast<uint8_t>(std::stoul(parts[0]));
    controller.default_algorithm.value = static_cast<uint8_t>(std::stoul(parts[1]));
    profile.controllers.push_back(controller);
  }

  return profile;
}

std::vector<std::unordered_map<std::string, std::string>> VehicleProfileManagerNode::parse_profile_blocks(
  const std::string & text) const
{
  std::vector<std::unordered_map<std::string, std::string>> blocks;
  std::unordered_map<std::string, std::string> current;
  std::stringstream ss(text);
  std::string line;
  std::vector<std::string> sensor_rows;
  std::vector<std::string> capability_rows;
  std::vector<std::string> controller_rows;

  auto flush_current = [&]() {
    if (!current.empty()) {
      std::string sensor_rows_joined;
      for (size_t i = 0; i < sensor_rows.size(); ++i) {
        if (i != 0) {
          sensor_rows_joined += "|";
        }
        sensor_rows_joined += sensor_rows[i];
      }
      std::string capability_rows_joined;
      for (size_t i = 0; i < capability_rows.size(); ++i) {
        if (i != 0) {
          capability_rows_joined += "|";
        }
        capability_rows_joined += capability_rows[i];
      }
      std::string controller_rows_joined;
      for (size_t i = 0; i < controller_rows.size(); ++i) {
        if (i != 0) {
          controller_rows_joined += "|";
        }
        controller_rows_joined += controller_rows[i];
      }
      current["sensor_rows"] = sensor_rows_joined;
      current["capability_rows"] = capability_rows_joined;
      current["controller_rows"] = controller_rows_joined;
      blocks.push_back(current);
    }
    current.clear();
    sensor_rows.clear();
    capability_rows.clear();
    controller_rows.clear();
  };

  while (std::getline(ss, line)) {
    line = trim_copy(line);
    if (line.empty()) {
      flush_current();
      continue;
    }
    if (line == "[profile]") {
      flush_current();
      continue;
    }
    const auto sep = line.find('=');
    if (sep == std::string::npos) {
      continue;
    }
    const auto key = trim_copy(line.substr(0, sep));
    const auto value = trim_copy(line.substr(sep + 1));
    if (key == "sensor") {
      sensor_rows.push_back(value);
    } else if (key == "capability") {
      capability_rows.push_back(value);
    } else if (key == "controller") {
      controller_rows.push_back(value);
    } else {
      current[key] = value;
    }
  }
  flush_current();
  return blocks;
}

void VehicleProfileManagerNode::load_demo_profile()
{
  VehicleProfile demo;

  // 基础信息
  demo.base_info.vehicle_id   = "demo_agv_001";
  demo.base_info.vehicle_name = "Demo AGV";
  demo.base_info.model_name   = "DemoModel-X1";
  demo.base_info.manufacturer = "Demo Manufacturer";

  // 底盘类型：差速
  demo.chassis_type.value = ChassisType::DIFFERENTIAL;

  // base_link
  demo.base_link_frame = "base_link";

  // 画像版本
  demo.profile_version = "demo_v1";

  calibration_vehicle_profile_interfaces::msg::SensorProfile front_camera;
  front_camera.sensor_id = "demo_front_camera";
  front_camera.sensor_type.value = SensorType::FRONT_CAMERA;
  front_camera.sensor_name = "Demo Front Camera";
  front_camera.frame_id = "front_camera_link";
  front_camera.camera_mount_type.value = CameraMountType::FRONT_MOUNTED;
  front_camera.enabled = true;
  front_camera.needs_intrinsic_calibration = true;
  front_camera.needs_extrinsic_calibration = true;
  front_camera.device_hint = "/dev/video0";
  front_camera.selected_for_this_session = true;
  front_camera.supports_intrinsic_calibration = true;
  front_camera.supports_extrinsic_calibration = true;
  demo.sensors.push_back(front_camera);

  calibration_vehicle_profile_interfaces::msg::CalibrationCapability external_cap;
  external_cap.ability_type.value = CalibrationAbilityType::EXTERNAL_LOCALIZATION_CALIBRATION;
  external_cap.supported = true;
  external_cap.message = "demo external localization supported";
  demo.capabilities.push_back(external_cap);

  calibration_vehicle_profile_interfaces::msg::CalibrationCapability chassis_cap;
  chassis_cap.ability_type.value = CalibrationAbilityType::CHASSIS_CALIBRATION;
  chassis_cap.supported = true;
  chassis_cap.message = "demo chassis calibration supported";
  demo.capabilities.push_back(chassis_cap);

  calibration_vehicle_profile_interfaces::msg::CalibrationCapability control_cap;
  control_cap.ability_type.value = CalibrationAbilityType::CONTROL_CALIBRATION;
  control_cap.supported = true;
  control_cap.message = "demo control calibration supported";
  demo.capabilities.push_back(control_cap);

  calibration_vehicle_profile_interfaces::msg::CalibrationCapability sensor_cap;
  sensor_cap.ability_type.value = CalibrationAbilityType::SENSOR_CALIBRATION;
  sensor_cap.supported = true;
  sensor_cap.message = "demo sensor calibration supported";
  demo.capabilities.push_back(sensor_cap);

  calibration_vehicle_profile_interfaces::msg::ControllerProfile controller;
  controller.control_axis.value = ControlAxisType::LATERAL_CONTROL;
  controller.default_algorithm.value = ControllerAlgorithmType::PURE_PURSUIT;
  demo.controllers.push_back(controller);

  // 启用的工作流阶段
  demo.enabled_workflow_stages = evaluate_supported_stages(demo);

  profiles_[demo.base_info.vehicle_id] = demo;
  RCLCPP_INFO(get_logger(), "已预载 demo 车辆画像 vehicle_id=[%s]",
    demo.base_info.vehicle_id.c_str());
}

}  // namespace vehicle_profile_manager

RCLCPP_COMPONENTS_REGISTER_NODE(vehicle_profile_manager::VehicleProfileManagerNode)
