crazyflie-lib 0.8.1

//! # Supervisor subsystem
//!
//! The supervisor monitors the Crazyflie's system state and exposes arming, crash recovery,
//! and emergency stop controls. It is the primary interface for flight readiness and safety.
//!
//! ## Reading system state
//!
//! Call [`Supervisor::read_bitfield`] to get a snapshot of the current system state.
//! The returned [`SupervisorInfo`] exposes individual boolean flags:
//! ```no_run
//! # async fn read_state(crazyflie: &crazyflie_lib::Crazyflie) -> crazyflie_lib::Result<()> {
//! let info = crazyflie.supervisor.read_bitfield().await?;
//!
//! if info.can_be_armed() {
//!     println!("Ready to arm");
//! }
//! if info.is_flying() {
//!     println!("Currently airborne");
//! }
//! # Ok(())
//! # }
//! ```
//!
//! ## Arming and crash recovery
//!
//! The Crazyflie must be armed before its motors will respond to setpoints.
//! After a crash the firmware may allow recovery without a full reboot:
//! ```no_run
//! # async fn commands(crazyflie: &crazyflie_lib::Crazyflie) -> crazyflie_lib::Result<()> {
//! // Arm the system
//! crazyflie.supervisor.send_arming_request(true).await?;
//!
//! // Disarm
//! crazyflie.supervisor.send_arming_request(false).await?;
//!
//! // Request recovery from a crashed state
//! crazyflie.supervisor.send_crash_recovery_request().await?;
//! # Ok(())
//! # }
//! ```
//!
//! ## Emergency stop
//!
//! The emergency stop functionality allows immediate motor shutdown for safety. Two variants
//! are available. The immediate stop cuts all motors at once and locks the firmware until reboot.
//! The watchdog variant is softer: it arms a timer in the firmware that stops the motors if the
//! message is not refreshed within 1000 ms, allowing controlled failsafe behaviour in a
//! communication-loss scenario:
//! ```no_run
//! # async fn emergency(crazyflie: &crazyflie_lib::Crazyflie) -> crazyflie_lib::Result<()> {
//! // Cut motors immediately
//! crazyflie.supervisor.send_emergency_stop().await?;
//!
//! // Or activate the watchdog — must be repeated every <1000 ms
//! crazyflie.supervisor.send_emergency_stop_watchdog().await?;
//! # Ok(())
//! # }
//! ```

use crate::crtp_utils::crtp_channel_dispatcher;
use crate::{Error, Result};
use crate::crazyflie::SUPERVISOR_PORT;
use crazyflie_link::Packet;
use flume::{Receiver, Sender};
use futures::lock::Mutex;
use std::time::Instant;
use tokio::time::{timeout, Duration};

// Channels
const SUPERVISOR_CH_INFO: u8 = 0;
pub(crate) const SUPERVISOR_CH_COMMAND: u8 = 1;

// Commands
const CMD_GET_STATE_BITFIELD: u8 = 0x0C;
pub(crate) const CMD_ARM_SYSTEM: u8 = 0x01;
pub(crate) const CMD_RECOVER_SYSTEM: u8 = 0x02;
pub(crate) const CMD_EMERGENCY_STOP: u8 = 0x03;
pub(crate) const CMD_EMERGENCY_STOP_WATCHDOG: u8 = 0x04;

/// Reply flag, ORed into the echoed command byte of every supervisor
/// response by the firmware (`CMD_RESPONSE` in crtp_supervisor.h).
const CMD_RESPONSE: u8 = 0x80;

/// How long a fetched state bitfield stays fresh. Reads within this window
/// are served from cache, capping request traffic on the CRTP link at
/// ~10 packets/s no matter how fast the application polls.
const BITFIELD_CACHE_TIMEOUT: Duration = Duration::from_millis(100);

// Bit positions
const BIT_CAN_BE_ARMED: u8 = 0;
const BIT_IS_ARMED: u8 = 1;
const BIT_IS_AUTO_ARMED: u8 = 2;
const BIT_CAN_FLY: u8 = 3;
const BIT_IS_FLYING: u8 = 4;
const BIT_IS_TUMBLED: u8 = 5;
const BIT_IS_LOCKED: u8 = 6;
const BIT_IS_CRASHED: u8 = 7;
const BIT_HL_CONTROL_ACTIVE: u8 = 8;
const BIT_HL_TRAJ_FINISHED: u8 = 9;
const BIT_HL_CONTROL_DISABLED: u8 = 10;

/// Supervisor info bitfield
///
/// Contains the decoded state of the supervisor system. Use the various
/// methods to query specific state flags.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct SupervisorInfo {
    /// Raw bitfield value
    pub raw: u16,
}

impl SupervisorInfo {
    /// Create from raw bitfield value
    pub fn from_bits(bits: u16) -> Self {
        Self { raw: bits }
    }

    /// System can be armed - the system can be armed and will accept an arming command
    pub fn can_be_armed(&self) -> bool {
        (self.raw >> BIT_CAN_BE_ARMED) & 0x01 != 0
    }

    /// System is armed
    pub fn is_armed(&self) -> bool {
        (self.raw >> BIT_IS_ARMED) & 0x01 != 0
    }

    /// System is configured to automatically arm
    pub fn is_auto_armed(&self) -> bool {
        (self.raw >> BIT_IS_AUTO_ARMED) & 0x01 != 0
    }

    /// The Crazyflie is ready to fly
    pub fn can_fly(&self) -> bool {
        (self.raw >> BIT_CAN_FLY) & 0x01 != 0
    }

    /// The Crazyflie is flying
    pub fn is_flying(&self) -> bool {
        (self.raw >> BIT_IS_FLYING) & 0x01 != 0
    }

    /// The Crazyflie is tumbled (upside down)
    pub fn is_tumbled(&self) -> bool {
        (self.raw >> BIT_IS_TUMBLED) & 0x01 != 0
    }

    /// The Crazyflie is in the locked state and must be restarted
    pub fn is_locked(&self) -> bool {
        (self.raw >> BIT_IS_LOCKED) & 0x01 != 0
    }

    /// The Crazyflie has crashed
    pub fn is_crashed(&self) -> bool {
        (self.raw >> BIT_IS_CRASHED) & 0x01 != 0
    }

    /// High level commander is actively flying the drone
    pub fn hl_control_active(&self) -> bool {
        (self.raw >> BIT_HL_CONTROL_ACTIVE) & 0x01 != 0
    }

    /// High level commander trajectory has finished
    pub fn hl_traj_finished(&self) -> bool {
        (self.raw >> BIT_HL_TRAJ_FINISHED) & 0x01 != 0
    }

    /// High level commander is disabled and not producing setpoints
    pub fn hl_control_disabled(&self) -> bool {
        (self.raw >> BIT_HL_CONTROL_DISABLED) & 0x01 != 0
    }

    /// Get list of all active state names
    pub fn active_states(&self) -> Vec<&'static str> {
        let states = [
            ("Can be armed", self.can_be_armed()),
            ("Is armed", self.is_armed()),
            ("Is auto armed", self.is_auto_armed()),
            ("Can fly", self.can_fly()),
            ("Is flying", self.is_flying()),
            ("Is tumbled", self.is_tumbled()),
            ("Is locked", self.is_locked()),
            ("Is crashed", self.is_crashed()),
            ("HL control active", self.hl_control_active()),
            ("HL trajectory finished", self.hl_traj_finished()),
            ("HL control disabled", self.hl_control_disabled()),
        ];

        states
            .iter()
            .filter_map(|(name, active)| if *active { Some(*name) } else { None })
            .collect()
    }
}

/// Access to the supervisor subsystem
///
/// See the [supervisor module documentation](crate::subsystems::supervisor) for more context and information.
pub struct Supervisor {
    uplink: Sender<Packet>,
    info_downlink: Mutex<Receiver<Packet>>,
    cache_timeout: Duration,
    cached_bitfield: std::sync::Mutex<Option<(Instant, u16)>>,
}

impl Supervisor {
    pub(crate) fn new(uplink: Sender<Packet>, downlink: Receiver<Packet>) -> Self {
        let (info_downlink, _cmd_downlink, _misc1, _misc2) = crtp_channel_dispatcher(downlink);
        Self {
            uplink,
            info_downlink: Mutex::new(info_downlink),
            cache_timeout: BITFIELD_CACHE_TIMEOUT,
            cached_bitfield: std::sync::Mutex::new(None),
        }
    }

    /// Read the supervisor bitfield
    ///
    /// Requests the current supervisor bitfield from the Crazyflie and returns it decoded.
    /// Uses time-based caching to avoid sending packets too frequently.
    ///
    /// # Example
    /// ```no_run
    /// # async fn example(crazyflie: &crazyflie_lib::Crazyflie) -> crazyflie_lib::Result<()> {
    /// let info = crazyflie.supervisor.read_bitfield().await?;
    /// println!("Can fly: {}", info.can_fly());
    /// # Ok(())
    /// # }
    /// ```
    pub async fn read_bitfield(&self) -> Result<SupervisorInfo> {
        if let Some(info) = self.cached_info() {
            return Ok(info);
        }

        // Hold the downlink lock across the whole request/reply exchange so
        // concurrent callers cannot interleave requests and replies.
        let downlink = self.info_downlink.lock().await;

        // Re-check the cache: a concurrent caller may have refreshed it while
        // we waited for the lock, in which case no request needs to be sent.
        if let Some(info) = self.cached_info() {
            return Ok(info);
        }

        // Discard stale replies from previously timed-out requests, so they
        // cannot be mistaken for the answer to the request we are about to send.
        while downlink.try_recv().is_ok() {}

        // Send request
        let pk = Packet::new(
            SUPERVISOR_PORT,
            SUPERVISOR_CH_INFO,
            vec![CMD_GET_STATE_BITFIELD],
        );
        self.uplink.send_async(pk).await.map_err(|_| Error::Disconnected)?;

        let bitfield = Self::wait_for_bitfield(&downlink).await?;
        drop(downlink);

        let mut cached = self.cached_bitfield.lock().unwrap();
        *cached = Some((Instant::now(), bitfield));

        Ok(SupervisorInfo::from_bits(bitfield))
    }

    /// Return the cached bitfield if it is still fresh. The MutexGuard never
    /// crosses an .await, keeping `read_bitfield`'s future Send.
    fn cached_info(&self) -> Option<SupervisorInfo> {
        let cached = self.cached_bitfield.lock().unwrap();

        if let Some((fetched_at, bitfield)) = *cached
            && fetched_at.elapsed() < self.cache_timeout
        {
            return Some(SupervisorInfo::from_bits(bitfield));
        }
        None
    }

    async fn wait_for_bitfield(downlink: &Receiver<Packet>) -> Result<u16> {
        loop {
            let packet = timeout(Duration::from_millis(1000), downlink.recv_async())
                .await
                .map_err(|_| Error::Timeout)??;

            let data = packet.get_data();
            if data.len() < 3 {
                continue;
            }

            let cmd = data[0];
            if cmd != CMD_GET_STATE_BITFIELD && cmd != (CMD_GET_STATE_BITFIELD | CMD_RESPONSE) {
                continue;
            }

            let bitfield = u16::from_le_bytes([data[1], data[2]]);
            return Ok(bitfield);
        }
    }

    /// Send system arm/disarm request
    ///
    /// Arms or disarms the Crazyflie's motors. When disarmed, the motors
    /// will not spin even if thrust commands are sent.
    ///
    /// # Arguments
    /// * `do_arm` - true to arm, false to disarm
    ///
    /// # Example
    /// ```no_run
    /// # async fn example(crazyflie: &crazyflie_lib::Crazyflie) -> crazyflie_lib::Result<()> {
    /// // Arm the system
    /// crazyflie.supervisor.send_arming_request(true).await?;
    /// # Ok(())
    /// # }
    /// ```
    pub async fn send_arming_request(&self, do_arm: bool) -> Result<()> {
        let command = if do_arm { 1u8 } else { 0u8 };
        let pk = Packet::new(
            SUPERVISOR_PORT,
            SUPERVISOR_CH_COMMAND,
            vec![CMD_ARM_SYSTEM, command],
        );
        self.uplink.send_async(pk).await.map_err(|_| Error::Disconnected)?;
        Ok(())
    }

    /// Send crash recovery request
    ///
    /// Requests recovery from a crashed state detected by the Crazyflie.
    ///
    /// # Example
    /// ```no_run
    /// # async fn example(crazyflie: &crazyflie_lib::Crazyflie) -> crazyflie_lib::Result<()> {
    /// crazyflie.supervisor.send_crash_recovery_request().await?;
    /// # Ok(())
    /// # }
    /// ```
    pub async fn send_crash_recovery_request(&self) -> Result<()> {
        let pk = Packet::new(
            SUPERVISOR_PORT,
            SUPERVISOR_CH_COMMAND,
            vec![CMD_RECOVER_SYSTEM],
        );
        self.uplink.send_async(pk).await.map_err(|_| Error::Disconnected)?;
        Ok(())
    }

    /// Send emergency stop
    ///
    /// Immediately stops all motors and puts the Crazyflie into a locked state.
    /// The drone will require a reboot before it can fly again.
    pub async fn send_emergency_stop(&self) -> Result<()> {
        let pk = Packet::new(
            SUPERVISOR_PORT,
            SUPERVISOR_CH_COMMAND,
            vec![CMD_EMERGENCY_STOP],
        );
        self.uplink.send_async(pk).await.map_err(|_| Error::Disconnected)?;
        Ok(())
    }

    /// Send emergency stop watchdog
    ///
    /// Activates/resets a watchdog failsafe that will automatically emergency stop
    /// the drone if this message isn't sent every 1000ms. Once activated by the first
    /// call, you must continue sending this periodically forever or the drone will
    /// automatically emergency stop. Use only if you need automatic failsafe behavior.
    pub async fn send_emergency_stop_watchdog(&self) -> Result<()> {
        let pk = Packet::new(
            SUPERVISOR_PORT,
            SUPERVISOR_CH_COMMAND,
            vec![CMD_EMERGENCY_STOP_WATCHDOG],
        );
        self.uplink.send_async(pk).await.map_err(|_| Error::Disconnected)?;
        Ok(())
    }
}