#pragma once

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

#include "rclcpp/rclcpp.hpp"

#include "calibration_vehicle_profile_interfaces/msg/vehicle_profile.hpp"
#include "calibration_vehicle_profile_interfaces/srv/evaluate_vehicle_calibration_applicability.hpp"
#include "calibration_vehicle_profile_interfaces/srv/get_vehicle_profile.hpp"
#include "calibration_vehicle_profile_interfaces/srv/heartbeat.hpp"
#include "calibration_vehicle_profile_interfaces/srv/register_or_update_vehicle_profile.hpp"

namespace vehicle_profile_manager
{

// 车辆画像管理节点：
// 1. 内存存储 vehicle_id → VehicleProfile 映射。
// 2. 提供 get_vehicle_profile / register_or_update_vehicle_profile / evaluate_applicability / heartbeat 四个服务。
// 3. 启动时预载一份 demo 车辆画像，供本地联调使用。
class VehicleProfileManagerNode : public rclcpp::Node
{
public:
  explicit VehicleProfileManagerNode(
    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

private:
  using GetProfileSrv      = calibration_vehicle_profile_interfaces::srv::GetVehicleProfile;
  using RegisterSrv        = calibration_vehicle_profile_interfaces::srv::RegisterOrUpdateVehicleProfile;
  using ApplicabilitySrv   = calibration_vehicle_profile_interfaces::srv::EvaluateVehicleCalibrationApplicability;
  using HeartbeatSrv       = calibration_vehicle_profile_interfaces::srv::Heartbeat;
  using VehicleProfile     = calibration_vehicle_profile_interfaces::msg::VehicleProfile;
  using WorkflowStageType  = calibration_vehicle_profile_interfaces::msg::WorkflowStageType;

  rclcpp::Service<GetProfileSrv>::SharedPtr    get_profile_service_;
  rclcpp::Service<RegisterSrv>::SharedPtr      register_service_;
  rclcpp::Service<ApplicabilitySrv>::SharedPtr applicability_service_;
  rclcpp::Service<HeartbeatSrv>::SharedPtr     heartbeat_service_;

  // 内存存储：vehicle_id → 完整车辆画像
  std::unordered_map<std::string, VehicleProfile> profiles_;
  std::string profile_storage_path_;

  void load_profiles_from_file();
  bool save_profiles_to_file(std::string & error_message) const;
  void load_demo_profile_if_empty();
  VehicleProfile make_profile_from_kv_map(
    const std::unordered_map<std::string, std::string> & values) const;
  std::vector<std::unordered_map<std::string, std::string>> parse_profile_blocks(
    const std::string & text) const;

  void handle_get_profile(
    const std::shared_ptr<GetProfileSrv::Request> request,
    std::shared_ptr<GetProfileSrv::Response> response);

  void handle_register(
    const std::shared_ptr<RegisterSrv::Request> request,
    std::shared_ptr<RegisterSrv::Response> response);

  void handle_applicability(
    const std::shared_ptr<ApplicabilitySrv::Request> request,
    std::shared_ptr<ApplicabilitySrv::Response> response);

  void handle_heartbeat(
    const std::shared_ptr<HeartbeatSrv::Request> request,
    std::shared_ptr<HeartbeatSrv::Response> response);

  // 根据车辆画像内容推断支持哪些工作流阶段。
  std::vector<WorkflowStageType> evaluate_supported_stages(
    const VehicleProfile & profile) const;

  // 预载 demo 车辆画像，vehicle_id = "demo_agv_001"。
  void load_demo_profile();
};

}  // namespace vehicle_profile_manager
