behaviortree-core 0.1.0

// Copyright © 2026 Stephan Kunz
//! [`behaviortree`](crate) groot2 connector module.

#[cfg(not(feature = "std"))]
pub mod connector_no_std;
#[cfg(feature = "std")]
pub mod connector_std;
#[cfg(not(feature = "std"))]
pub mod socket_no_std;
#[cfg(feature = "std")]
pub mod socket_std;

#[cfg(not(feature = "std"))]
pub mod error_no_std;

pub mod connector_data;
pub mod dispatch;
pub mod message;
pub mod protocol;

#[cfg(not(feature = "std"))]
use embassy_time::Instant;

use crate::{
	Arc, ConstString, Mutex,
	behavior_data::BehaviorData,
	behavior_state::BehaviorState,
	tree::{
		observer::groot2::{connector_data::Groot2ConnectorData, protocol::Groot2TransitionInfo},
		tree::BehaviorTree,
	},
};

const FLAG_MORE: u8 = 0x01;
const FLAG_LONG: u8 = 0x02;
const FLAG_COMMAND: u8 = 0x04;

const GREETING_SIZE: usize = 64;

/// READY command body for a REP socket (25 bytes).
/// Layout: [name_len(1)]["READY"(5)][key_len(1)]["Socket-Type"(11)][val_len BE(4)]["REP"(3)]
const READY_BODY: &[u8] = &[
	0x05, b'R', b'E', b'A', b'D', b'Y', 0x0b, b'S', b'o', b'c', b'k', b'e', b't', b'-', b'T', b'y', b'p', b'e', 0x00, 0x00,
	0x00, 0x03, b'R', b'E', b'P',
];

/// Predefined size of the behavior state transition buffer.
/// This amount will be buffered between sends.
/// If there are more state transitions happening, the eldest will be dropped.
const TRANSITION_SIZE: u32 = 100;

/// constants
pub const GROOT_STATE: &str = "groot_state";

/// get the microseconds since 1970-01-01 (std) or since the embassy time driver epoch (no_std).
fn timestamp() -> u64 {
	#[cfg(not(feature = "std"))]
	{
		Instant::now().as_micros()
	}
	#[cfg(feature = "std")]
	#[allow(clippy::cast_possible_truncation)]
	#[allow(clippy::expect_used)]
	{
		std::time::SystemTime::now()
			.duration_since(std::time::UNIX_EPOCH)
			.expect("Time went backwards")
			.as_micros() as u64
	}
}

/// Build the 64-byte ZMTP 3.1 greeting (NULL mechanism).
pub fn build_greeting(is_server: bool) -> [u8; GREETING_SIZE] {
	let mut g = [0u8; GREETING_SIZE];
	// Signature
	g[0] = 0xff;
	g[1..9].copy_from_slice(&[0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01]);
	g[9] = 0x7f;
	// ZMTP 3.1
	g[10] = 0x03;
	g[11] = 0x01;
	// Mechanism: "NULL" padded to 20 bytes (bytes 12-31)
	g[12..16].copy_from_slice(b"NULL");
	// As-server flag; bytes 33-63 are filler, already zero
	g[32] = u8::from(is_server);
	g
}

/// Attach the Groot2 communication callbacks to a [`BehaviorTree`].
/// # Panics
/// - if an unknown message from Groot2 arrives
pub fn attach_groot_callback(tree: &mut BehaviorTree, shared: Arc<Mutex<Groot2ConnectorData>>) {
	let id: ConstString = GROOT_STATE.into();
	// add a callback for each tree element
	let shared = shared;
	for element in tree.iter_mut() {
		let shared_clone = shared.clone();
		// the callback
		let callback = move |behavior: &BehaviorData, new_state: &mut BehaviorState| {
			if behavior.state() != *new_state {
				// Groot does not need a state for root
				if behavior.uid() != 0 {
					let state = if *new_state == BehaviorState::Idle {
						behavior.state() as u8 + 10
					} else {
						*new_state as u8
					};
					let mut shared_guard = shared_clone.lock();
					let uid = behavior.uid().to_le_bytes();
					let index = 3 * ((behavior.uid() - 1) as usize);
					shared_guard.state_buffer[index] = uid[0];
					shared_guard.state_buffer[index + 1] = uid[1];
					shared_guard.state_buffer[index + 2] = state;

					if shared_guard.recording {
						let info = Groot2TransitionInfo::new(timestamp(), behavior.uid(), *new_state);
						if shared_guard.transitions_buffer.is_empty() {
							shared_guard.transitions = 0;
						} else if shared_guard.transitions >= TRANSITION_SIZE {
							shared_guard.transitions_buffer.pop_front();
						} else {
							shared_guard.transitions += 1;
						}
						shared_guard.transitions_buffer.push_back(info);
					}
					drop(shared_guard);
				}
			}
		};
		element.add_pre_state_change_callback(id.clone(), callback);
	}
}