// Copyright 2025
// CerebrumNode orchestrates behavior tree construction and ticking,
// mediating between high-level skill selection (possibly via a VLM model)
// and the unified ExecuteBtAction channel handled by the Cerebellum.

// Own header first.
#include "brain/cerebrum_node.hpp"

// C++ standard library
#include <algorithm>
#include <chrono>
#include <fstream>
#include <iostream>
#include <random>
#include <sstream>
#include <string>
#include <thread>
#include <utility>

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <behaviortree_cpp/action_node.h>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>

// Project headers
#include "brain/constants.hpp"
#include "brain_interfaces/action/execute_bt_action.hpp"

using namespace std::chrono_literals; // NOLINT

namespace brain
{

namespace
{

/**
 * @brief Determine whether a skill exposes at least one action-capable interface.
 *
 * Scans the skill's declared interfaces and treats any entry whose final
 * component (suffix after the last '.') case-insensitively matches "action" or
 * "service" as action-capable. Services are included because they can be
 * wrapped as action-like BT leaves in this framework.
 *
 * @param spec SkillSpec to inspect.
 * @return true if an interface ending with .action or .service is found; false otherwise.
 * @thread_safety Read-only; safe for concurrent calls.
 */
bool HasActionInterface(const SkillSpec & spec)
{
  for (const auto & iface : spec.interfaces) {
    auto pos = iface.rfind('.');
    if (pos != std::string::npos) {
      std::string kind = iface.substr(pos + 1);
      for (auto & c : kind) {
        c = static_cast<char>(::tolower(static_cast<unsigned char>(c)));
      }
      if (kind == "action" || kind == "service") {
        return true;
      }
    }
  }
  return false;
}

struct UpdatingFlagGuard
{
  explicit UpdatingFlagGuard(std::atomic<bool> & f)
  : flag(f) {}
  std::atomic<bool> & flag;
  ~UpdatingFlagGuard() {flag = false;}
};

}  // namespace

/**
 * @brief Construct the CerebrumNode.
 *
 * Initializes registries (action clients, BT registry, skill manager), declares and loads
 * parameters, loads skill definitions, registers BT actions, sets up timers for periodic
 * task dispatch (sequence selection) and BT ticking, and creates a rebuild service.
 * @thread_safety Construction is single-threaded; no concurrent access occurs.
 */
CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
: rclcpp::Node("cerebrum_node", options),
  share_directory_(ament_index_cpp::get_package_share_directory("brain")),
  robot_skill_file_path_(share_directory_ + std::string("/config/robot_skills.yaml")),
  bt_config_file_path_(share_directory_ + std::string("/config/bt_carry_boxes.xml")),
  bt_params_file_path_(share_directory_ + std::string("/config/bt_carry_boxes.params.yaml"))
{
  // Initialize
  action_registry_ = std::make_unique<ActionClientRegistry>(this);
  bt_registry_ = std::make_unique<BTRegistry>(factory_, this);
  skill_manager_ = std::make_unique<SkillManager>(this, action_registry_.get(), bt_registry_.get());
  if (!skill_manager_->load_from_file(robot_skill_file_path_)) {
    RCLCPP_WARN(this->get_logger(), "Failed to load skills from %s", robot_skill_file_path_.c_str());
  }

  DeclareParameters();
  LoadParameters();
  SetupDynamicParameterHandling();

  RegisterSkillBtActions();
  RegisterActionClient();

  bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();});
  task_timer_ = this->create_wall_timer(5000ms, [this]() {CerebrumTask();});

  CreateServices();

  RCLCPP_INFO(this->get_logger(), "CerebrumNode initialized (timeout=%.2f random_pick=%zu)", 
    node_timeout_sec_, random_skill_pick_count_);
}

/**
 * @brief Register a BT leaf action with lifecycle handlers.
 * @param type_name BT node type (<SkillName>_H).
 * @param handlers Callback bundle (start/running/halted).
 * @throws May propagate BT::BehaviorTreeException on duplicate registration.
 * @thread_safety Call during initialization prior to concurrent ticking.
 */
void CerebrumNode::RegisterBtAction(
  const std::string & type_name,
  const BTActionHandlers & handlers)
{
  bt_registry_->register_bt_action(type_name, handlers);
}

/**
 * @brief Explicitly send the unified ExecuteBtAction goal (manual/test trigger).
 * @param bt_action_name Typically brain::kExecuteBtActionName.
 * @thread_safety Relies on ActionClientRegistry thread-safety for send.
 */
void CerebrumNode::SendExecuteBt(const std::string & bt_action_name)
{
  if (!action_registry_) {
    RCLCPP_WARN(this->get_logger(), "Action registry not initialized; cannot send %s", bt_action_name.c_str());
    return;
  }
  action_registry_->send(bt_action_name, this->get_logger());
}

/**
 * @brief Ensure ExecuteBtAction client is registered (idempotent) and attach callbacks.
 *
 * Goal builder serializes current sequence into comma list; feedback handler updates
 * per-node execution state with progress and results; result handler finalizes outstanding
 * nodes. Epoch filtering prevents stale feedback application.
 * @thread_safety Called during initialization; callbacks internally use mutex/atomics.
 */
void CerebrumNode::RegisterActionClient()
{
  using EBA = brain_interfaces::action::ExecuteBtAction;
  if (registered_actions_.count(brain::kExecuteBtActionName)) {
    return;
  }
  registered_actions_.insert(brain::kExecuteBtActionName);
  action_registry_->register_client<EBA>(
    brain::kExecuteBtActionName,
    // Goal builder
    [this]() {
      EBA::Goal g;
      g.epoch = static_cast<uint32_t>(epoch_filter_.epoch());
      std::lock_guard<std::mutex> lk(exec_mutex_);
      std::ostringstream oss;
      if (single_skill_goal_override_) {
        oss << *single_skill_goal_override_;
      } else {
        for (std::size_t i = 0; i < current_sequence_skills_.size(); ++i) {
          if (i) {oss << ',';}
          oss << current_sequence_skills_[i];
        }
      }
      g.action_name = oss.str();
      // Populate per-skill calls[] using dynamic parameters
      g.calls.clear();
      if (single_skill_goal_override_) {
        g.calls.push_back(BuildSkillCallForSkill(*single_skill_goal_override_, single_skill_instance_override_));
      } else {
        for (const auto & s : current_sequence_skills_) {
          g.calls.push_back(BuildSkillCallForSkill(s, std::nullopt));
        }
      }
      // Debug: log outgoing calls being sent to Cerebellum
      RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Sending goal with calls_count=%zu", g.calls.size());
      for (size_t i = 0; i < g.calls.size(); ++i) {
        const auto & c = g.calls[i];
        RCLCPP_INFO(this->get_logger(), "  out_call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu",
          i, c.name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.payload_yaml.size());
      }
      RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction]Sending goal epoch=%u skills=%s", g.epoch, g.action_name.c_str());
      return g;
    },
    // Feedback
    [this](
      typename ActionClientRegistry::GoalHandle<EBA>::SharedPtr,
      const std::shared_ptr<const EBA::Feedback> feedback) {
      if (!feedback) {
        return;
      }
      if (!epoch_filter_.accept(
        feedback->epoch, feedback->current_skill, bt_pending_run_))
      {
        return;
      }
      {
        std::lock_guard<std::mutex> lk(exec_mutex_);
        // Match per-skill BT node by skill_name.
        const std::string bt_type = feedback->current_skill + std::string("_H");
        auto it = per_node_exec_.find(bt_type);
        if (it != per_node_exec_.end()) {
          auto & info = it->second;
          if (!info.in_flight && !info.result) {
            // Late feedback for an already closed node; ignore.
          } else {
            if (feedback->stage == "RUN" && feedback->progress + 1e-4 < info.last_progress) {
              return; // non-monotonic
            }
            if (feedback->stage == "RUN") {
              info.last_progress = feedback->progress;
            }
            if (feedback->stage == "END") {
              info.result_code = ParseResultDetail(feedback->detail);
              info.result = (info.result_code == ExecResultCode::SUCCESS);
              if (info.result_code == ExecResultCode::SUCCESS) {
                info.awaiting_result = true;
                info.end_feedback_time = this->now();
              } else {
                info.in_flight = false;
                info.awaiting_result = false;
              }
            }
          }
        }
      }
      RCLCPP_INFO(
        this->get_logger(),
        "[ExecuteBtAction][fb][epoch=%u] stage=%s skill=%s progress=%.2f detail=%s",
        feedback->epoch,
        feedback->stage.c_str(),
        feedback->current_skill.c_str(),
        feedback->progress,
        feedback->detail.c_str());
    },
    // Result
    [this](const ActionClientRegistry::GoalHandle<EBA>::WrappedResult & res) {
      bool ok = (
        res.code == rclcpp_action::ResultCode::SUCCEEDED &&
        res.result &&
        res.result->success);
      if (ok) {
        RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction][result] OK: %s",
          res.result ? res.result->message.c_str() : "<null>");
      } else {
        RCLCPP_ERROR(this->get_logger(), "[ExecuteBtAction][result] FAIL code=%d msg=%s",
          static_cast<int>(res.code), res.result ? res.result->message.c_str() : "<null>");
      }
      for (auto & kv : per_node_exec_) {
        auto & info = kv.second;
        if (info.in_flight || info.awaiting_result) {
          info.in_flight = false;
          info.awaiting_result = false;
          info.result = ok;               // authoritative final
          info.result_code = ok ? ExecResultCode::SUCCESS : ExecResultCode::FAILURE;
          RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction][result] Finalizing node %s result=%s",
            kv.first.c_str(), info.result ? "SUCCESS" : "FAILURE");
        }
      }
    });
}

/**
 * @brief Send a previously registered standalone action (not the unified BT action).
 * @param name Action client key.
 * @thread_safety Depends on underlying registry implementation; assumed safe for concurrent sends.
 */
void CerebrumNode::SendAction(const std::string & name)
{
  if (!action_registry_) {
    return;
  }
  action_registry_->send(name, this->get_logger());
}

/**
 * @brief Periodic high-level task (timer callback) selecting and applying a new sequence.
 *
 * Steps:
 *  1. Skip if BT run is active and rebuilds are disallowed.
 *  2. Run model hook to choose sequence.
 *  3. Cancel active ExecuteBtAction goal (if any) to avoid stale execution.
 *  4. Rebuild behavior tree (increments epoch, resets tracking) and schedule run.
 * @thread_safety Timer callback; coordinates with mutex and atomic flags.
 */
void CerebrumNode::CerebrumTask()
{
  if (bt_pending_run_ && !allow_bt_rebuild_during_execution_) {
    return;
  }

  if (!bt_config_file_path_.empty()) {
    BuildBehaviorTreeFromFile(bt_config_file_path_);
  } else {
    // Use correctly cased API methods
    RunVlmModel();
    CancelActiveExecuteBtGoal();
    UpdateBehaviorTree();
  }
}

/**
 * @brief Tick the behavior tree root if a run is pending.
 *
 * Clears bt_pending_run_ when root terminates (SUCCESS/FAILURE). Skips ticking while a
 * rebuild is in progress or the tree root is null.
 * @thread_safety Timer callback; guarded by atomics; tree_ access not concurrently mutated except during rebuild.
 */
void CerebrumNode::ExecuteBehaviorTree()
{
  if (bt_updating_ || !bt_pending_run_) {
    return;
  }
  if (!tree_.rootNode()) {
    bt_pending_run_ = false;
    return;
  }
  auto status = tree_.rootNode()->executeTick();
  if (status != BT::NodeStatus::RUNNING) {
    bt_pending_run_ = false;
    RCLCPP_INFO(this->get_logger(), "BT finished status=%s", BT::toStr(status, true).c_str());
  }
}

/**
 * @brief Build a behavior tree from an XML file, advancing the epoch.
 *
 * Resets per-node execution tracking and marks bt_pending_run_ to start ticking. Uses an
 * atomic flag and RAII guard to prevent re-entrancy.
 * @param xml_file Path to XML file.
 * @thread_safety Not re-entrant (atomic gate); safe against concurrent timer callbacks.
 */
void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
{
  BT::Tree new_tree;
  if (!bt_registry_->build_tree_from_xml_file(xml_file, new_tree)) {
    RCLCPP_ERROR(this->get_logger(), "Failed building BT from file %s", xml_file.c_str());
    return;
  }
  try {
    tree_.haltTree();
  } catch (...) {
    RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT");
  }
  tree_ = std::move(new_tree);
  uint64_t new_epoch = epoch_filter_.epoch() + 1;
  std::vector<std::string> seq_skills;
  {
    std::lock_guard<std::mutex> lk(exec_mutex_);
    seq_skills = current_sequence_skills_;
  }
  epoch_filter_.reset_epoch(new_epoch, seq_skills);
  per_node_exec_.clear();
  bt_pending_run_ = true;
  RCLCPP_INFO(this->get_logger(), "BT built from file epoch=%llu", static_cast<unsigned long long>(new_epoch));
}

/**
 * @brief Rebuild the behavior tree for the active sequence, advancing the epoch.
 *
 * Resets per-node execution tracking and marks bt_pending_run_ to start ticking. Uses an
 * atomic flag and RAII guard to prevent re-entrancy.
 * @thread_safety Not re-entrant (atomic gate); safe against concurrent timer callbacks.
 */
void CerebrumNode::UpdateBehaviorTree()
{
  if (bt_updating_) {
    return;
  }
  bt_updating_ = true;
  UpdatingFlagGuard guard(bt_updating_);
  BT::Tree new_tree;
  if (!bt_registry_->build_tree_from_sequence(active_sequence_, new_tree)) {
    RCLCPP_ERROR(this->get_logger(), "Failed building BT for sequence %s", active_sequence_.c_str());
    return;
  }
  try {
    tree_.haltTree();
  } catch (...) {
    // Swallow halt exceptions.
    RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT");
  }
  tree_ = std::move(new_tree);
  uint64_t new_epoch = epoch_filter_.epoch() + 1;
  std::vector<std::string> seq_skills;
  {
    std::lock_guard<std::mutex> lk(exec_mutex_);
    seq_skills = current_sequence_skills_;
  }
  epoch_filter_.reset_epoch(new_epoch, seq_skills);
  per_node_exec_.clear();
  bt_pending_run_ = true;
  RCLCPP_INFO(this->get_logger(), "BT updated epoch=%llu sequence=%s",
    static_cast<unsigned long long>(new_epoch), active_sequence_.c_str());
}

/**
 * @brief Register per-skill BehaviorTree action handlers (leaf nodes).
 *
 * For each loaded skill a corresponding BT leaf type <SkillName>_H is
 * registered (idempotent: duplicate registrations are caught and logged at
 * debug level). The start handler sends a single-skill ExecuteBtAction goal by
 * temporarily setting a goal override so that the unified action infrastructure
 * stays consistent while still yielding per-node granularity. Running and halt
 * handlers manage timeout enforcement, progress monitoring, and logging.
 *
 * Preconditions: skill_manager_ initialized and loaded. Safe to call once
 * during construction. Subsequent calls will just skip already registered
 * types.
 */
void CerebrumNode::RegisterSkillBtActions()
{
  if (!skill_manager_) {return;}
  for (const auto & kv : skill_manager_->skills()) {
    const auto & skill = kv.second;
    const std::string bt_type = skill.name + std::string("_H");
    BTActionHandlers handlers;
  handlers.on_start = [this, bt_type, skill](rclcpp::Node *, BT::TreeNode & node) {
        auto & info = per_node_exec_[bt_type];
        if (info.in_flight || info.result) {
          return info.result ? BT::NodeStatus::SUCCESS : BT::NodeStatus::RUNNING;
        }
        info.in_flight = true;
        info.result = false;
        info.result_code = ExecResultCode::UNKNOWN;
        info.start_time = this->now();
        info.skill_name = skill.name;
        info.last_progress = 0.0;
        info.cancel_state = PerNodeExecInfo::CancelState::NONE;

        const std::string instance_name = node.name();
        DeclareBtActionParamsForSkillInstance(skill.name, instance_name);
        // Override goal builder to send only this skill.
        single_skill_goal_override_ = skill.name;
        single_skill_instance_override_ = instance_name;
        if (!action_registry_->wait_for_server(brain::kExecuteBtActionName, 1500ms)) {
          RCLCPP_ERROR(this->get_logger(), "ExecuteBtAction server unavailable");
          info.in_flight = false;
          info.result = false;
          info.result_code = ExecResultCode::FAILURE;
          single_skill_goal_override_.reset();
          return BT::NodeStatus::FAILURE;
        }
        action_registry_->send(brain::kExecuteBtActionName, this->get_logger());
        RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_start", bt_type.c_str());
        // Clear override after send to avoid affecting other triggers.
        single_skill_goal_override_.reset();
        single_skill_instance_override_.reset();
        return BT::NodeStatus::RUNNING;
      };
    handlers.on_running = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
        auto it = per_node_exec_.find(bt_type);
        if (it == per_node_exec_.end()) {return BT::NodeStatus::FAILURE;}
        auto & info = it->second;
        if (info.in_flight) {
          double elapsed = (this->now() - info.start_time).seconds();
          if (elapsed > node_timeout_sec_) {
            info.in_flight = false;
            info.result = false;
            info.result_code = ExecResultCode::FAILURE;
            RCLCPP_WARN(this->get_logger(), "[%s] BTAction on_running timeout after %.2f sec", bt_type.c_str(), elapsed);
            action_registry_->cancel(brain::kExecuteBtActionName, this->get_logger());
            // return BT::NodeStatus::FAILURE;
          }
          return BT::NodeStatus::RUNNING;
        }
        if (info.awaiting_result) {
          double wait_elapsed = (this->now() - info.end_feedback_time).seconds();
          if (wait_elapsed > result_wait_warn_sec_) {
            RCLCPP_WARN_THROTTLE(
              this->get_logger(), *this->get_clock(), 2000,
              "Skill %s awaiting final result %.2fs after END feedback",
              bt_type.c_str(), wait_elapsed);
          }
          return BT::NodeStatus::RUNNING;
        }
        return info.result ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
      };
    handlers.on_halted = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
        RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_halted", bt_type.c_str());
      };
    try {RegisterBtAction(bt_type, handlers);} catch (const BT::BehaviorTreeException & e) {
      RCLCPP_WARN(this->get_logger(), "BT type exists %s: %s", bt_type.c_str(), e.what());
    }
  }
}

/**
 * @brief Update BT action parameters for a skill instance
 * 
 * @param skill_name
 * @param instance_name 
 * @param instance_params 
 */
void CerebrumNode::UpdateBtActionParamsForSkillInstance(
    const std::string & skill_name, 
    const std::string & instance_name, 
    const std::string & instance_params)
{
  if (skill_name.empty() || instance_name.empty() || instance_params.empty()) {
    RCLCPP_WARN(this->get_logger(), "Update BtAction Params ForSkillInstance called with empty skill_name, instance_name, or instance_params");
    return;
  }
    // Per-instance namespace: cerebrum.bt.<Skill>.<Instance>.*
  const std::string ns = std::string("cerebrum.bt.") + skill_name + "." + instance_name + ".";
  const std::string p_topic = ns + "topic";
  const std::string p_payload = ns + "payload_yaml";
  if (!this->has_parameter(p_topic)) {
    this->declare_parameter<std::string>(p_topic, std::string(""));
  }
  if (!this->has_parameter(p_payload)) {
    this->declare_parameter<std::string>(p_payload, std::string(""));
  }
  RCLCPP_INFO(this->get_logger(), "Declaring BT action parameters for skill instance %s", instance_name.c_str());
  try {
    this->set_parameter(rclcpp::Parameter(p_payload, instance_params));
    RCLCPP_INFO(this->get_logger(), "Seeded sample payload for %s instance %s", skill_name.c_str(), instance_name.c_str());
  } catch (const std::exception & e) {
    RCLCPP_ERROR(this->get_logger(), "[%s] seeding sample payload failed: %s", skill_name.c_str(), e.what());
  }
}

/**
 * @brief Declare BT action parameters for a skill instance
 * 
 * @param skill_name 
 * @param instance_name 
 */
void CerebrumNode::DeclareBtActionParamsForSkillInstance(
    const std::string & skill_name, 
    const std::string & instance_name)
{
  if (skill_name.empty() || instance_name.empty()) {
    RCLCPP_WARN(this->get_logger(), "Declare BtAction Params ForSkillInstance called with empty skill_name or instance_name");
    return;
  }

  std::string instance_params;
  if (skill_name == "MoveWaist" && instance_name == "s1_waist_bend_down") {
    instance_params = "move_pitch_degree: 10.0\nmove_yaw_degree: -15.0\n";
  } else if (skill_name == "MoveLeg" && instance_name == "s7_leg_move_back") {
    instance_params = "move_up_distance: 0.08\n";
  } else if (skill_name == "MoveWheel" && instance_name == "s8_wheel_move_back") {
    instance_params = "move_distance: 0.60\nmove_angle: 20.0\n";
  } else if (skill_name == "MoveHome" && instance_name == "s0_move_home") {
    // MoveHome goal is empty; seed explicit empty map for clarity
    instance_params = "{}\n";
  } else if (skill_name == "Arm" && instance_name == "s2_arm_stretch_out") {
    instance_params = "body_id: 1\ndata_type: 3\ndata_length: 7\ncommand_id: 1\nframe_time_stamp: 123456789\ndata_array: [0.3, 0.4, 0.5, -0.6, 0.07, -0.18, 0.72]\n";
  }

  UpdateBtActionParamsForSkillInstance(skill_name, instance_name, instance_params);
}

/**
 * @brief Install on_set_parameters callback to accept dynamic per-skill updates.
 *
 * Only parameters within the prefix "cerebrum.bt." are intercepted. Minimal validation:
 *  - topic: any string accepted
 *  - payload_yaml: any string accepted (executor will validate downstream)
      
      # Set MoveWheel topic
      ros2 param set /cerebrum_node cerebrum.bt.MoveWheel.topic /robot/move_wheel

      # Set Arm payload
      ros2 param set /cerebrum_node cerebrum.bt.Arm.payload_yaml "body_id: 1
      data_type: 3
      data_length: 7
      command_id: 1
      frame_time_stamp: 123456789
      data_array: [0.1, 0.2, 0.3, -0.4, 0.05, -0.19, 0.73]"
 */
void CerebrumNode::SetupDynamicParameterHandling()
{
  using rclcpp::Parameter;
  using rcl_interfaces::msg::SetParametersResult;
  auto cb = [this](const std::vector<Parameter> & params) -> SetParametersResult {
    SetParametersResult result;
    result.successful = true;
    result.reason = "ok";
    const std::string prefix = "cerebrum.bt.";
    for (const auto & p : params) {
      const std::string & name = p.get_name();
      if (name.rfind(prefix, 0) != 0) {
        continue; // other params handled by default mechanisms
      }
      // Expected pattern: cerebrum.bt.<Skill>.topic | payload_yaml
      // We perform a light parse to ensure a field segment exists
      auto last_dot = name.rfind('.');
      if (last_dot == std::string::npos || last_dot + 1 >= name.size()) {
        result.successful = false;
        result.reason = "invalid per-skill param name";
        break;
      }
      const std::string field = name.substr(last_dot + 1);
      if (field != "topic" && field != "payload_yaml") {
        // Reject unknown fields under this namespace to avoid typos
        result.successful = false;
        result.reason = "unsupported field for cerebrum.bt.*";
        break;
      }
      // Accept string-only values
      if (p.get_type() != rclcpp::ParameterType::PARAMETER_STRING) {
        result.successful = false;
        result.reason = "per-skill params must be strings";
        break;
      }
      // No further validation here; downstream will interpret.
      RCLCPP_INFO(this->get_logger(), "[params] updated %s", name.c_str());
    }
    return result;
  };
  param_cb_handle_ = this->add_on_set_parameters_callback(cb);
}

/**
 * @brief Build a SkillCall message for a specific skill.
 *
 * @param skill_name The name of the skill.
 * @return brain_interfaces::msg::SkillCall The constructed SkillCall message.
 */
brain_interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std::string & skill_name) const
{
  brain_interfaces::msg::SkillCall call;
  call.name = skill_name;
  // Interface type/kind inference from SkillSpec
  if (!skill_manager_) {return call;}
  auto it = skill_manager_->skills().find(skill_name);
  if (it != skill_manager_->skills().end() && !it->second.interfaces.empty()) {
    // Use first interface as primary
    const std::string & iface = it->second.interfaces.front();
    call.interface_type = iface;
    auto pos = iface.rfind('.');
    std::string kind = (pos == std::string::npos) ? std::string{} : iface.substr(pos + 1);
    for (auto & c : kind) c = static_cast<char>(::tolower(static_cast<unsigned char>(c)));
    if (kind == "srv") kind = "service";
    call.call_kind = kind;
  }
  // Read dynamic params
  const std::string ns = std::string("cerebrum.bt.") + skill_name + ".";
  const std::string p_topic = ns + "topic";
  const std::string p_payload = ns + "payload_yaml";
  try {
    if (this->has_parameter(p_topic)) {
      call.topic = this->get_parameter(p_topic).as_string();
    }
    if (this->has_parameter(p_payload)) {
      call.payload_yaml = this->get_parameter(p_payload).as_string();
    }
  } catch (const std::exception & e) {
    RCLCPP_WARN(this->get_logger(), "[%s] read params failed: %s", skill_name.c_str(), e.what());
  }
  return call;
}

/**
 * @brief Declare BT action parameters for a skill instance
 * 
 * @param skill_name 
 * @param instance_name 
 * @return brain_interfaces::msg::SkillCall 
 */
brain_interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(
  const std::string & skill_name,
  const std::optional<std::string> & instance_name) const
{
  auto base = BuildSkillCallForSkill(skill_name);
  // If instance_name provided, try per-instance override first, falling back to per-skill
  if (!instance_name) {
    return base;
  }
  const std::string ns = std::string("cerebrum.bt.") + skill_name + "." + *instance_name + ".";
  const std::string p_topic = ns + "topic";
  const std::string p_payload = ns + "payload_yaml";
  try {
    // topic override
    if (this->has_parameter(p_topic)) {
      auto v = this->get_parameter(p_topic).as_string();
      if (!v.empty()) base.topic = v;
    }
    // payload override
    if (this->has_parameter(p_payload)) {
      auto v = this->get_parameter(p_payload).as_string();
      if (!v.empty()) base.payload_yaml = v;
    }
  } catch (const std::exception & e) {
    RCLCPP_WARN(this->get_logger(), "[%s/%s] read instance params failed: %s", skill_name.c_str(), instance_name->c_str(), e.what());
  }
  return base;
}

/**
 * @brief Randomly select up to count distinct action-capable skills (respecting whitelist).
 * @param count Maximum number of skills.
 * @return Vector of selected skill names (size <= count).
 * @thread_safety Read-only access to skill_manager_ (assumed immutable post-init).
 */
std::vector<std::string> CerebrumNode::PickRandomActionSkills(std::size_t count)
{
  if (!skill_manager_) {
    return {};
  }
  std::vector<std::string> pool;
  pool.reserve(skill_manager_->skills().size());
  const bool filter = !allowed_action_skills_.empty();
  for (const auto & kv : skill_manager_->skills()) {
    const auto & spec = kv.second;
    if (!HasActionInterface(spec)) {
      continue;
    }
    if (filter &&
      std::find(
        allowed_action_skills_.begin(),
        allowed_action_skills_.end(),
        spec.name) == allowed_action_skills_.end())
    {
      continue;
    }
    pool.push_back(spec.name);
  }
  if (pool.empty()) {
    return {};
  }
  std::shuffle(
    pool.begin(),
    pool.end(),
    std::mt19937{std::random_device{}()});
  if (pool.size() > count) {
    pool.resize(count);
  }
  return pool;
}

/**
 * @brief Convert ordered skill names into a BehaviorTree sequence specification.
 *
 * Each skill name is transformed into a BTActionSpec with type and name set to
 * <Skill>_H (handler leaf). No additional configuration is attached at this
 * stage; customization would occur earlier at registration time. The function
 * preserves ordering and reserves storage for efficiency.
 *
 * @param names Ordered list of skill identifiers.
 * @return BTSequenceSpec ready for registration/build.
 */
BTSequenceSpec CerebrumNode::MakeSequenceFromSkills(const std::vector<std::string> & names)
{
  BTSequenceSpec seq; seq.reserve(names.size());
  for (const auto & n : names) {
    seq.emplace_back(BTActionSpec{n + std::string("_H"), n + std::string("_H"), {}});
  }
  return seq;
}

/**
 * @brief Build minimal XML representation of a sequence for diagnostics.
 * @param seq Sequence specification.
 * @return XML string containing a single root <Sequence> element.
 * @thread_safety Pure function.
 */
std::string CerebrumNode::BuildXmlFromSequence(const BTSequenceSpec & seq) const
{
  std::ostringstream oss;
  oss << "<root BTCPP_format=\"4\"><BehaviorTree ID=\"MainTree\"><Sequence name=\"root_sequence\">";
  for (const auto & act : seq) {
    oss << "<" << act.type << " name=\"" << act.name << "\"/>";
  }
  oss << "</Sequence></BehaviorTree></root>";
  return oss.str();
}

/**
 * @brief Choose and activate a sequence (fixed preferred over random selection pool).
 *
 * Updates current_sequence_skills_ under mutex, registers sequence, and sets it active.
 * Does not rebuild immediately; the caller triggers UpdateBehaviorTree().
 * @thread_safety Uses mutex for sequence list assignment.
 */
void CerebrumNode::RunVlmModel()
{
  auto register_and_activate = [this](
    const std::string & name,
    const std::vector<std::string> & skills) {
      if (skills.empty()) {
        return false;
      }
      {
        std::lock_guard<std::mutex> lk(exec_mutex_);
        current_sequence_skills_ = skills;
      }
      auto seq = MakeSequenceFromSkills(skills);
      RegisterBtSequence(name, seq);
      SetActiveSequence(name);
      return true;
    };
  if (fixed_skill_sequence_) {
    if (!register_and_activate("FixedSequence", *fixed_skill_sequence_)) {
      return;
    }
  } else {
    auto picked = PickRandomActionSkills(random_skill_pick_count_);
    if (!register_and_activate("VLMSequence", picked)) {
      RCLCPP_WARN(
        this->get_logger(),
        "No action-capable skills available");
    }
  }
}

/**
 * @brief Parse a delimited string into unique tokens (first occurrence preserved).
 * @param raw Input delimited string.
 * @return Vector of unique tokens.
 * @thread_safety Pure static utility.
 */
std::vector<std::string> CerebrumNode::ParseListString(const std::string & raw)
{
  std::vector<std::string> result;
  result.reserve(8);
  std::unordered_set<std::string> seen;
  std::string token;
  token.reserve(raw.size());
  auto flush = [&]() {
      if (!token.empty()) {
        if (seen.insert(token).second) {
          result.push_back(token);
        }
        token.clear();
      }
    };
  for (char c : raw) {
    if (c == ',' || c == ';' || c == ' ' || c == '\t' || c == '\n') {
      flush();
    } else {
      token.push_back(c);
    }
  }
  flush();
  return result;
}

/**
 * @brief Cancel the active unified ExecuteBtAction goal (if any).
 * @thread_safety Pass-through to registry cancellation (assumed thread-safe).
 */
void CerebrumNode::CancelActiveExecuteBtGoal()
{
  if (action_registry_) {
    action_registry_->cancel(
      brain::kExecuteBtActionName,
      this->get_logger());
  }
}

/**
 * @brief Legacy epoch skill setter (now a no-op; filtering handled elsewhere).
 * @param skills Ignored.
 * @thread_safety Pure no-op.
 */
void CerebrumNode::SetEpochSkills(const std::vector<std::string> & skills)
{
  (void)skills;  // Now handled by epoch_filter_
}

/**
 * @brief Heuristically classify textual detail into an ExecResultCode.
 * @param detail Input detail string (prefix or code= pattern interpreted).
 * @return ExecResultCode classification (UNKNOWN if unrecognized).
 * @thread_safety Pure function.
 */
CerebrumNode::ExecResultCode CerebrumNode::ParseResultDetail(const std::string & detail)
{
  if (detail.rfind("OK", 0) == 0 || detail.rfind("SUCCESS", 0) == 0) {
    return ExecResultCode::SUCCESS;
  }
  if (detail.rfind("FAIL", 0) == 0 || detail.rfind("ERROR", 0) == 0) {
    return ExecResultCode::FAILURE;
  }
  if (detail.rfind("CANCEL", 0) == 0 || detail.rfind("CANCELLED", 0) == 0) {
    return ExecResultCode::CANCELLED;
  }
  auto pos = detail.find("code=");
  if (pos != std::string::npos) {
    auto code = detail.substr(pos + 5, 16);
    if (code.rfind("0", 0) == 0) {
      return ExecResultCode::SUCCESS;
    }
    if (code.rfind("cancel", 0) == 0) {
      return ExecResultCode::CANCELLED;
    }
    return ExecResultCode::FAILURE;
  }
  return ExecResultCode::UNKNOWN;
}

/**
 * @brief Declare all ROS 2 parameters consumed by this node.
 *
 * Separation of declaration and retrieval enables later logic (like default
 * skill file resolution) to consult already-declared parameters without
 * triggering implicit declarations. Not intended for repeated runtime calls.
 */
void CerebrumNode::DeclareParameters()
{
  this->declare_parameter<double>("node_timeout_sec", node_timeout_sec_);
  this->declare_parameter<bool>("allow_bt_rebuild_during_execution", allow_bt_rebuild_during_execution_);
  this->declare_parameter<std::string>("allowed_action_skills", "");
  this->declare_parameter<std::string>("fixed_skill_sequence", "");
  this->declare_parameter<int>("random_skill_pick_count", static_cast<int>(random_skill_pick_count_));
}

/**
 * @brief Retrieve parameter values and apply normalization / parsing.
 *
 * Tasks performed:
 *  - Clamp random_skill_pick_count_ to at least 1.
 *  - Parse allowed_action_skills_ delimited list into unique tokens.
 *  - Parse fixed_skill_sequence_ if provided.
 *  - Log summary for transparency.
 * Safe to call only after DeclareParameters().
 */
void CerebrumNode::LoadParameters()
{
  this->get_parameter("node_timeout_sec", node_timeout_sec_);
  this->get_parameter("allow_bt_rebuild_during_execution", allow_bt_rebuild_during_execution_);
  int random_pick_param = static_cast<int>(random_skill_pick_count_);
  this->get_parameter("random_skill_pick_count", random_pick_param);
  if (random_pick_param < 1) {random_pick_param = 1;}
  random_skill_pick_count_ = static_cast<std::size_t>(random_pick_param);

  std::string allowed_raw;
  this->get_parameter("allowed_action_skills", allowed_raw);
  if (!allowed_raw.empty()) {
    allowed_action_skills_ = ParseListString(allowed_raw);
    RCLCPP_INFO(this->get_logger(), "Allowed action skills count=%zu", allowed_action_skills_.size());
  }

  std::string fixed_seq_raw;
  this->get_parameter("fixed_skill_sequence", fixed_seq_raw);
  if (!fixed_seq_raw.empty()) {
    auto tmp = ParseListString(fixed_seq_raw);
    if (!tmp.empty()) {fixed_skill_sequence_ = tmp;}
  }
  if (fixed_skill_sequence_) {
    RCLCPP_INFO(this->get_logger(), "Using fixed skill sequence size=%zu", fixed_skill_sequence_->size());
  }
}

/**
 * @brief Create externally callable services.
 *
 * Currently exposes a Trigger service to force an immediate sequence rebuild.
 * The callback enforces rebuild policy (respecting allow_bt_rebuild_during_execution_)
 * and follows the same steps as the periodic task: model run -> cancel goal -> rebuild.
 */
void CerebrumNode::CreateServices()
{
  rebuild_service_ = this->create_service<std_srvs::srv::Trigger>(
    "cerebrum/rebuild_now",
    [this](
      const std::shared_ptr<std_srvs::srv::Trigger::Request>,
      std::shared_ptr<std_srvs::srv::Trigger::Response> resp) {
      if (bt_pending_run_ && !allow_bt_rebuild_during_execution_) {
        resp->success = false;
        resp->message = "BT running and rebuild not allowed";
        return;
      }

      if (!bt_config_file_path_.empty()) {
        BuildBehaviorTreeFromFile(bt_config_file_path_);
      } else {
        RunVlmModel();
        CancelActiveExecuteBtGoal();
        UpdateBehaviorTree();
      }

      resp->success = true;
      resp->message = "Rebuild triggered";
    });
}

} // namespace brain
