#include "omni_node/omni_node.hpp"

#include <iostream>

namespace omni_node {

OmniNode::OmniNode(rclcpp::NodeOptions options)
    : Node("omni_node", options), engine_initialized_(false) {
  // 步骤1：读取参数
  this->declare_parameter<std::string>("config_path", "config/omni_config.json");
  this->declare_parameter<bool>("enable_legacy_asr_mode", false);
  this->declare_parameter<bool>("enable_planner_mode", true);

  std::string config_path = this->get_parameter("config_path").as_string();
  enable_legacy_asr_mode_ = this->get_parameter("enable_legacy_asr_mode").as_bool();
  enable_planner_mode_ = this->get_parameter("enable_planner_mode").as_bool();

  // 步骤2：创建 publisher（早于 engine 创建，避免 callback 时 publisher 未就绪）
  pub_response_ = this->create_publisher<std_msgs::msg::String>("llm_response", 10);
  if (enable_planner_mode_) {
    pub_planner_response_ = this->create_publisher<std_msgs::msg::String>("planner/llm_response", 10);
  }

  RCLCPP_INFO(this->get_logger(), "Initializing OmniEngine with config: %s", config_path.c_str());

  // 步骤3：创建 engine
  engine_ = std::make_unique<OmniEngine>(config_path);

  // 步骤4：注册 callback（此时 publisher 已就绪）
  engine_->SetStreamCallback(
      [this](const std::string& text, bool is_final) {
        std_msgs::msg::String msg;
        msg.data = text;
        RCLCPP_INFO(this->get_logger(), "STREAM_CALLBACK: publishing %zu bytes, is_final=%d",
                    text.length(), is_final);
        this->pub_response_->publish(msg);
        if (is_final) {
          RCLCPP_INFO(this->get_logger(), "LLM stream completed");
        }
      });

  engine_->SetPlannerResponseCallback(
      [this](const std::string& json_response, bool is_final) {
        if (is_final) {
          std_msgs::msg::String msg;
          msg.data = json_response;
          this->pub_planner_response_->publish(msg);
          RCLCPP_INFO(this->get_logger(), "Planner response published to /planner/llm_response");
        }
      });

  // 步骤5：初始化 engine
  bool init_ok = engine_->Init();
  if (!init_ok) {
    RCLCPP_ERROR(this->get_logger(), "OmniEngine init failed");
    return;
  }
  engine_initialized_ = true;

  // 同步 Node 层和 Engine 层的配置
  engine_->SetEnableMode(enable_legacy_asr_mode_, enable_planner_mode_);

  RCLCPP_INFO(this->get_logger(), "OmniNode mode configuration:");
  RCLCPP_INFO(this->get_logger(), "  enable_legacy_asr_mode: %s", enable_legacy_asr_mode_ ? "true" : "false");
  RCLCPP_INFO(this->get_logger(), "  enable_planner_mode: %s", enable_planner_mode_ ? "true" : "false");

  if (!enable_legacy_asr_mode_ && !enable_planner_mode_) {
    RCLCPP_ERROR(this->get_logger(), "FATAL: Both modes are disabled! Please enable at least one mode.");
    RCLCPP_ERROR(this->get_logger(), "  Set enable_legacy_asr_mode:=true and/or enable_planner_mode:=true");
    engine_initialized_ = false;
    return;
  }

  // 步骤6：创建 subscriber（在 engine 初始化后）
  if (enable_legacy_asr_mode_) {
    sub_asr_ = this->create_subscription<std_msgs::msg::String>(
        "asr", 10,
        [this](const std_msgs::msg::String::SharedPtr msg) {
          this->OnAsrReceived(msg);
        });
    RCLCPP_INFO(this->get_logger(), "Legacy ASR mode enabled - subscribed to /asr");
  } else {
    RCLCPP_INFO(this->get_logger(), "Legacy ASR mode disabled - /asr not subscribed");
  }

  if (enable_planner_mode_) {
    sub_planner_request_ = this->create_subscription<std_msgs::msg::String>(
        "planner/llm_request", 10,
        [this](const std_msgs::msg::String::SharedPtr msg) {
          this->OnPlannerLlmRequestReceived(msg);
        });
    RCLCPP_INFO(this->get_logger(), "Planner mode enabled - subscribed to /planner/llm_request");
  } else {
    RCLCPP_INFO(this->get_logger(), "Planner mode disabled - /planner/llm_request not subscribed");
  }

  RCLCPP_INFO(this->get_logger(), "omni_node started successfully.");
  RCLCPP_INFO(this->get_logger(), "Topics:");
  RCLCPP_INFO(this->get_logger(), "  Publishing: /llm_response");
  if (enable_planner_mode_) {
    RCLCPP_INFO(this->get_logger(), "  Publishing: /planner/llm_response");
  }
}

OmniNode::~OmniNode() = default;

void OmniNode::OnAsrReceived(const std_msgs::msg::String::SharedPtr msg) {
  if (!engine_initialized_) {
    RCLCPP_WARN(this->get_logger(), "Engine not initialized, ignoring message");
    return;
  }

  if (!enable_legacy_asr_mode_) {
    RCLCPP_WARN(this->get_logger(), "Legacy ASR mode is disabled, ignoring message");
    return;
  }

  RCLCPP_INFO(this->get_logger(), "Received ASR text: %s", msg->data.c_str());

  engine_->FeedText(msg->data);
}

void OmniNode::OnPlannerLlmRequestReceived(const std_msgs::msg::String::SharedPtr msg) {
  if (!engine_initialized_) {
    RCLCPP_WARN(this->get_logger(), "Engine not initialized, ignoring message");
    return;
  }

  if (!enable_planner_mode_) {
    RCLCPP_WARN(this->get_logger(), "Planner mode is disabled, ignoring message");
    return;
  }

  // 检查 route_hint，如果是 "cloud" 则忽略（由 CloudLLMNode 处理）
  std::string data = msg->data;
  size_t route_hint_pos = data.find("\"route_hint\"");
  if (route_hint_pos != std::string::npos) {
    size_t colon_pos = data.find(":", route_hint_pos);
    if (colon_pos != std::string::npos) {
      size_t value_start = data.find("\"", colon_pos + 1);
      size_t value_end = data.find("\"", value_start + 1);
      if (value_start != std::string::npos && value_end != std::string::npos) {
        std::string route_value = data.substr(value_start + 1, value_end - value_start - 1);
        if (route_value == "cloud") {
          RCLCPP_INFO(this->get_logger(), "Ignored cloud route_hint request (handled by CloudLLMNode)");
          return;
        }
      }
    }
  }

  RCLCPP_INFO(this->get_logger(), "Received planner LLM request, length: %zu bytes", msg->data.length());

  engine_->FeedPlannerRequest(msg->data);
}

}  // namespace omni_node

