behaviortree-core 0.1.0

// Copyright © 2025 Stephan Kunz
//! [`BehaviorTree`] implementation.

#[cfg(feature = "groot")]
use alloc::boxed::Box;
use alloc::string::String;
#[cfg(feature = "std")]
use alloc::vec::Vec;

use databoard::Databoard;
#[cfg(feature = "groot")]
use embassy_sync::{
	blocking_mutex::raw::CriticalSectionRawMutex,
	channel::{Channel, Sender},
};
#[cfg(feature = "std")]
use libloading::Library;
use tinyscript::SharedRuntime;
#[cfg(feature = "std")]
use uuid::Uuid;

#[cfg(feature = "groot")]
use crate::tree::observer::groot2::{GROOT_STATE, attach_groot_callback, connector_data::Groot2ConnectorData};
use crate::{
	Arc, BehaviorResult, Mutex,
	behavior_state::BehaviorState,
	behavior_traits::BehaviorRegistry,
	tree::{
		error::Error,
		tree_element::{BehaviorTreeElement, TreeElementKind},
		tree_iter::{TreeIter, TreeIterMut},
	},
};

#[cfg(feature = "groot")]
use super::CHANNEL_SIZE;

/// Tree identifier - 16 raw bytes (UUID-wire-compatible).
pub type TreeId = [u8; 16];

/// Recursion function to print a (sub)tree recursively, limit is a tree-depth of 127
/// # Errors
/// - [`Error::RecursionLimit`] if limit of 127 for tree depth is exceeded
fn print_recursively(level: i8, behavior: &BehaviorTreeElement) -> Result<(), Error> {
	if level == i8::MAX {
		return Err(Error::RecursionLimit {
			behavior: behavior.name().clone(),
		});
	}

	let next_level = level + 1;
	let mut indentation = String::new();
	for _ in 0..level {
		indentation.push_str("  ");
	}

	#[cfg(feature = "std")]
	std::println!("{indentation}{}", behavior.name());
	for child in &**behavior.children() {
		print_recursively(next_level, child)?;
	}
	Ok(())
}

#[cfg(feature = "groot")]
#[derive(Clone, Default)]
pub enum BehaviorTreeMessage {
	#[default]
	NothingToDo,
	AddGrootCallback(Arc<Mutex<Groot2ConnectorData>>),
	RemoveAllGrootHooks,
}

/// A Tree of [`BehaviorTreeElement`]s.
/// A certain [`BehaviorTree`] can contain up to 65536 [`BehaviorTreeElement`]s.
pub struct BehaviorTree {
	/// The trees unique id
	uuid: TreeId,
	/// Root element of tree.
	root: BehaviorTreeElement,
	/// `runtime` is shared between elements.
	runtime: SharedRuntime,
	#[cfg(feature = "groot")]
	channel: &'static Channel<CriticalSectionRawMutex, BehaviorTreeMessage, CHANNEL_SIZE>,
	/// `libraries` stores a reference to the used shared libraries aka plugins.
	/// This is necessary to avoid memory deallocation of libs while tree is in use.
	#[cfg(feature = "std")]
	_libraries: Vec<Arc<Library>>,
}

impl BehaviorTree {
	/// create a Tree with reference to its libraries
	#[must_use]
	pub fn new(root: BehaviorTreeElement, registry: &impl BehaviorRegistry) -> Self {
		// create a [`SharedRuntime`](https://docs.rs/tinyscript/latest/tinyscript/runtime/type.SharedRuntime.html)
		// based on the current state of registriesscripting runtime
		let runtime = Arc::new(Mutex::new(registry.runtime().clone()));
		// clone the current state of registered libraries so that they are not deallocated while tree is running
		#[cfg(feature = "std")]
		let mut libraries = Vec::with_capacity(registry.libraries().capacity() + 1);
		#[cfg(feature = "std")]
		for lib in registry.libraries() {
			libraries.push(lib.clone());
		}

		#[cfg(feature = "groot")]
		let channel: &'static Channel<CriticalSectionRawMutex, BehaviorTreeMessage, CHANNEL_SIZE> =
			Box::leak(Box::new(Channel::new()));
		Self {
			#[cfg(feature = "std")]
			uuid: Uuid::new_v4().into_bytes(),
			#[cfg(not(feature = "std"))]
			uuid: [0u8; 16], // @TODO: replace with unique value
			root,
			runtime,
			#[cfg(feature = "groot")]
			channel,
			#[cfg(feature = "std")]
			_libraries: libraries,
		}
	}

	/// Access the root blackboard of the tree.
	#[must_use]
	pub const fn blackboard(&self) -> &Databoard {
		self.root.data().blackboard()
	}

	/// Access the root blackboard of the tree.
	#[must_use]
	pub const fn blackboard_mut(&mut self) -> &mut Databoard {
		self.root.data_mut().blackboard_mut()
	}

	/// Pretty print the tree.
	/// # Errors
	/// - if tree depth exceeds 127 (sub)tree levels.
	pub fn print(&self) -> Result<(), Error> {
		print_recursively(0, &self.root)
	}

	/// Get a (sub)tree where index 0 is root tree.
	/// # Errors
	/// - if subtree is not found.
	pub fn subtree(&self, index: usize) -> Result<&BehaviorTreeElement, Error> {
		let mut i = 0_usize;
		for element in self.iter() {
			if matches!(element.kind(), TreeElementKind::SubTree) {
				if i == index {
					return Ok(element);
				}
				i += 1;
			}
		}
		Err(Error::SubtreeNotFound { index })
	}

	/// Get the trees uuid.
	#[must_use]
	pub const fn uuid(&self) -> &TreeId {
		&self.uuid
	}

	// /// Set the trees uuid.
	// pub fn set_uuid(&mut self, uuid: TreeId) {
	// 	self.uuid = uuid;
	// }

	/// Get a message sender.
	/// This sender can be used to modify the tree while running.
	#[cfg(feature = "groot")]
	#[must_use]
	pub fn sender(&self) -> Sender<'static, CriticalSectionRawMutex, BehaviorTreeMessage, CHANNEL_SIZE> {
		self.channel.sender()
	}

	/// Get the trees total number of children.
	#[must_use]
	pub fn size(&self) -> u16 {
		let mut count = 0;
		let iter = self.iter();
		for _ in iter {
			count += 1;
		}
		count
	}

	/// Handle incoming message    
	#[cfg(feature = "groot")]
	fn handle_message(&mut self, message: BehaviorTreeMessage) {
		match message {
			BehaviorTreeMessage::RemoveAllGrootHooks => {
				for element in self.iter_mut() {
					element.remove_pre_state_change_callback(GROOT_STATE);
				}
			}
			BehaviorTreeMessage::AddGrootCallback(data) => {
				attach_groot_callback(self, data);
			}
			BehaviorTreeMessage::NothingToDo => {}
		}
	}

	/// Ticks the tree exactly once.
	/// # Errors
	pub async fn tick_exactly_once(&mut self) -> BehaviorResult {
		#[cfg(feature = "groot")]
		while let Ok(message) = self.channel.try_receive() {
			self.handle_message(message);
		}
		self.root.tick(&self.runtime).await
	}

	/// Ticks the tree once.
	/// @TODO: The wakeup mechanism is not yet implemented
	/// # Errors
	pub async fn tick_once(&mut self) -> BehaviorResult {
		#[cfg(feature = "groot")]
		while let Ok(message) = self.channel.try_receive() {
			self.handle_message(message);
		}
		self.root.tick(&self.runtime).await
	}

	/// Ticks the tree until it finishes either with [`BehaviorState::Success`] or [`BehaviorState::Failure`].
	/// # Errors
	pub async fn tick_while_running(&mut self) -> BehaviorResult {
		let mut state = BehaviorState::Running;
		while state == BehaviorState::Running || state == BehaviorState::Idle {
			#[cfg(feature = "groot")]
			while let Ok(message) = self.channel.try_receive() {
				self.handle_message(message);
			}
			state = self.root.tick(&self.runtime).await?;

			// Not implemented: Check for wake-up conditions and tick again if so
			// Not sure if this is still necessary with real async
			// @TODO!

			// yield after every tick so other futures/tasks get scheduled
			// (needed when behaviors complete synchronously without suspending)
			#[cfg(feature = "std")]
			tokio::task::yield_now().await;
			#[cfg(not(feature = "std"))]
			embassy_futures::yield_now().await;
		}

		// handle eventually pending messages
		#[cfg(feature = "groot")]
		while let Ok(message) = self.channel.try_receive() {
			self.handle_message(message);
		}
		Ok(state)
	}

	/// Get an iterator over the tree.
	pub fn iter(&self) -> impl Iterator<Item = &BehaviorTreeElement> {
		TreeIter::new(&self.root)
	}

	/// Get a mutable iterator over the tree.
	pub fn iter_mut(&mut self) -> impl Iterator<Item = &mut BehaviorTreeElement> {
		TreeIterMut::new(&mut self.root)
	}

	/// Reset tree to initial state.
	/// # Errors
	/// - if reset of children failed
	pub fn reset(&mut self) -> Result<(), crate::error::Error> {
		self.root.halt(&self.runtime)?;
		self.runtime.lock().clear();
		Ok(())
	}
}

#[cfg(all(test, feature = "std"))]
mod tests {
	use super::*;

	use core::ops::Range;

	use databoard::Databoard;

	use crate::{
		BehaviorDescription,
		behavior_data::BehaviorData,
		behavior_state::BehaviorState,
		behaviors::mock_behavior::{MockBehavior, MockBehaviorConfig},
		tree::tree_element::{BehaviorTreeElement, TreeElementKind},
	};

	struct TestRegistry {
		runtime: tinyscript::Runtime,
		libraries: Vec<Arc<Library>>,
	}

	impl crate::behavior_traits::BehaviorRegistry for TestRegistry {
		fn add_behavior(
			&mut self,
			_: BehaviorDescription,
			_: alloc::boxed::Box<crate::BehaviorCreationFn>,
		) -> Result<(), crate::error::Error> {
			Ok(())
		}
		fn add_tree_defintion(
			&mut self,
			_: &str,
			_: crate::ConstString,
			_: Range<usize>,
		) -> Result<(), crate::error::Error> {
			Ok(())
		}
		fn libraries(&self) -> &Vec<Arc<Library>> {
			&self.libraries
		}
		fn runtime(&self) -> &tinyscript::Runtime {
			&self.runtime
		}
		fn register_enum_tuple(&mut self, _: &str, _: i8) -> Result<(), crate::error::Error> {
			Ok(())
		}
	}

	fn make_success_leaf() -> BehaviorTreeElement {
		let config = MockBehaviorConfig {
			return_state: BehaviorState::Success,
			..Default::default()
		};
		let behavior: crate::BehaviorPtr = alloc::boxed::Box::new(MockBehavior::new(config));
		let data = BehaviorData::create(0, Databoard::default(), BehaviorDescription::new("leaf", "leaf", false));
		BehaviorTreeElement::create(TreeElementKind::Leaf, behavior, data)
	}

	/// Covers `BehaviorTree::new()` with a non-empty library list (lines 102-104).
	/// Loads an actual shared library so the `for lib in registry.libraries()` body executes.
	#[test]
	fn new_with_library_clones_libraries() {
		// Load a real shared library. SAFETY: Loading libc is safe in tests.
		let lib = unsafe {
			Library::new("/usr/lib/x86_64-linux-gnu/libc.so.6")
				.or_else(|_| Library::new("libc.so.6"))
				.or_else(|_| Library::new("libc.so"))
		};
		if let Ok(loaded_lib) = lib {
			let registry = TestRegistry {
				runtime: tinyscript::Runtime::default(),
				libraries: alloc::vec![Arc::new(loaded_lib)],
			};
			// BehaviorTree::new clones the library into _libraries (covers lines 102-104).
			let tree = BehaviorTree::new(make_success_leaf(), &registry);
			assert_eq!(tree.size(), 1);
		}
		// If no shared library found, the test is skipped gracefully.
	}

	/// Covers the unused-but-required `BehaviorRegistry` trait methods on `TestRegistry`
	/// (lines 290, 291, 294): `add_behavior`, `add_tree_defintion`, `register_enum_tuple`.
	#[test]
	fn test_registry_unused_trait_methods() {
		let mut registry = TestRegistry {
			runtime: tinyscript::Runtime::default(),
			libraries: Vec::new(),
		};
		let desc = BehaviorDescription::new("x", "x", false);
		let creation_fn: alloc::boxed::Box<crate::BehaviorCreationFn> =
			alloc::boxed::Box::new(|_| alloc::boxed::Box::new(MockBehavior::new(MockBehaviorConfig::default())));
		let _ = crate::behavior_traits::BehaviorRegistry::add_behavior(&mut registry, desc, creation_fn);
		let _ = crate::behavior_traits::BehaviorRegistry::add_tree_defintion(&mut registry, "id", "xml".into(), 0..1);
		let _ = crate::behavior_traits::BehaviorRegistry::register_enum_tuple(&mut registry, "E", 1);
	}

	/// Covers tick_while_running post-loop message drain (lines 246-247).
	/// The spawned task enqueues a message; it runs during yield_now() (after the tick loop
	/// exits but before the post-loop drain), so the post-loop drain picks it up.
	#[tokio::test]
	async fn tick_while_running_post_loop_drain() {
		let registry = TestRegistry {
			runtime: tinyscript::Runtime::default(),
			libraries: Vec::new(),
		};
		let mut tree = BehaviorTree::new(make_success_leaf(), &registry);
		let sender = tree.sender();

		// Spawn: enqueues a message immediately, runs during yield_now() in tick_while_running.
		tokio::spawn(async move {
			let _ = sender.try_send(BehaviorTreeMessage::NothingToDo);
		});

		let state = tree.tick_while_running().await.unwrap();
		assert_eq!(state, BehaviorState::Success);
	}
}