packo 0.3.1

#![no_std]
#![allow(clippy::unnecessary_cast)]
//! Packed datastructure in `no_std` with self referential indexing.
//! See [Packo].

/// Create a packed structure [Packo] (array of `T`s) with self referential
/// indexes.
///
/// `T` will be always be zero initialized in memory without calling any
/// default or constructor. **`T` must be safe to be zero-initializable**.
///
/// The total size of the [Packo] is scaled by `S` + 1 + the used array:
/// S must be greater than 1 as the 0th element is used as an always zeroed
/// sentinel value. And the extra
/// element is used for dirty writes when indexing out of the valid elements.
///
/// Each element will also have a few indexes, at least 4: next, prev, child,
/// parent. In case of `C` > 1 then the child index will repeat `C` times for
/// each category. These indexes will add to the size of each node. Each index
/// is of size of `IDX`.
///
/// When indexing the elements, the first element (`Packo[0]`) will always be
/// a nil value (zeroed `T`). The 0th element is an invalid element and used as
/// sentinel value. Writing to the 0th element will have also no effect.
///
/// If the index is out of the [Packo] bounds the 0th element will be returned.
///
/// Every item in [Packo] can connect as a list and/or a tree depending of the
/// internal indexing capability. Because the internal indexing follows linked
/// list principles, iterating into sub-lists will be linear time, but
/// accessing a single item by index (id) will be constant instead.
///
/// Types:
///
/// - *S*: Size. How many elements are preallocated. One element is
///   reserved for the sentinel value, *S* must be > 1.
/// - *C*: Categories. How many nesting categories are available per node.
///   Each category will have its own separate child+parent indexing
///   allowing for tree data structures based on the category index.
///   Categories is defaulted to 1 and only if used the variant
///   methods with suffix 'c' will select a specific category.
/// - *T*: The type contained per each node.
///
/// After calling [`Packo::insert`] a few times for 1, 2, 3, 4, the collection
/// will contain a number of disconnected root nodes that can be iterated
/// through [`Packo::roots`]:
/// ```
/// use packo::*;
///
/// let mut p = Packo::<20, u8>::default();
/// p.insert(1);
/// p.insert(2);
/// p.insert(3);
/// p.insert(4);
/// ```
/// ```text
/// V IDX  CHILD  PARENT  PREV  NEXT
/// 1   1      0       0     0     0
/// 2   2      0       0     0     0
/// 3   3      0       0     0     0
/// 4   4      0       0     0     0
/// ```
/// ```text
/// |1|  |2|  |3|  |4|
/// ```
///
/// If called differently by appending to each returned ixd, then the elements
/// will be connected together in a linked list that later can be queried with
/// [`Packo::next`] and iterated with [`Packo::siblings`] given the starting
/// point (linked lists are circular).
///
/// ```
/// use packo::*;
///
/// let mut p = Packo::<20, u8>::default();
/// let p1 = p.insert(1);
/// let p2 = p.append(p1,2);
/// let p3 = p.append(p2, 3);
/// p.append(p3, 4);
/// ```
/// ```text
/// V IDX  CHILD  PARENT  PREV  NEXT
/// 1   1      0       0     4     2
/// 2   2      0       0     1     3
/// 3   3      0       0     2     4
/// 4   4      0       0     3     1
/// ```
/// ```text
/// |1| <> |2| <> |3| <> |4|
/// ```
///
/// In this other example 1, 2 are roots and 3, 4 are a sublist of 1.
/// Using [`Packo::nest`] the first element that calls that will spawn under
/// the idx as a tree child.
/// Children can later be checked with [`Packo::child`] to get the first child
/// idx of a parent, or [`Packo::childs`] to iterate on all the childs of that
/// parent.
///
/// ```
/// use packo::*;
///
/// let mut p = Packo::<20, u8>::default();
/// let p1 = p.insert(1);
/// let p2 = p.append(p1, 2);
/// let p3 = p.nest(p1, 3);
/// p.append(p3, 4);
/// ```
/// ```text
/// V IDX  CHILD  PARENT  PREV  NEXT
/// 1   1      3       0     2     2
/// 2   2      0       0     1     1
/// 3   3      0       1     4     4
/// 4   4      0       1     3     3
/// ```
/// ```text
/// |1| <> |2|
///  |
/// |3| <> |4|
/// ```
///
/// When declaring [Packo] with a generic `C` for a number of categories
/// greater than the default (1), then the children can be split into subtree
/// by each category.
/// The method variants ending with 'c' will use the cateogry identifier to
/// select for which category to nest into or check childs for.
///
/// ```
/// use packo::*;
///
/// let mut p = Packo::<20, u8, 2>::default();
/// let p1 = p.insert(1);
/// let p2 = p.append(p1, 2);
/// let p3 = p.nestc(p1, 3, 0);
/// p.append(p3, 4);
/// let p5 = p.nestc(p1, 5, 1);
/// p.append(p5, 6);
/// ```
/// ```text
/// V IDX  CHILD[0] CHILD[1]  PARENT  PREV  NEXT
/// 1   1        3        5        0     2     2
/// 2   2        0        0        0     1     1
/// 3   3        0        0        1     4     4
/// 4   4        0        0        1     3     3
/// 5   5        0        0        1     6     6
/// 6   6        0        0        1     5     5
/// ```
/// ```text
///    |1| <> |2|
///    / \
///   /   \
///  |     | (via C=1)
///  |     |
///  |    |5| <> |6|
///  |
///  | (via C=0)
///  |
/// |3| <> |4|  
/// ```
///
pub struct Packo<const S: usize, T, const C: usize = 1, IDX: IDXTraits = usize> {
    dirty: T,
    items: [Tracko<S, T, C, IDX>; S],
}

pub trait IDXTraits:
    PartialEq + Display + Copy + From<u8> + Into<usize> + TryFrom<usize> + AddAssign
{
}
impl<T> IDXTraits for T where
    T: PartialEq + Display + Copy + From<u8> + Into<usize> + TryFrom<usize> + AddAssign
{
}

use core::{fmt::Display, mem::MaybeUninit, ops::AddAssign};

impl<const S: usize, T, const C: usize, IDX: IDXTraits> Packo<S, T, C, IDX> {
    /// Delete all items, sets all to unused.
    pub fn reset(&mut self) {
        self.items = unsafe { MaybeUninit::zeroed().assume_init() };
    }

    /// Gets the next index, if this item is part of a list.
    /// Failure returns idx 0.
    pub fn next(&self, i: IDX) -> IDX {
        if !self.valid_idx(i) {
            return 0.into();
        }
        self.items[i.into()].next
    }

    /// Gets the previous index, if this item is part of a list.
    /// Failure returns idx 0.
    pub fn prev(&self, i: IDX) -> IDX {
        if !self.valid_idx(i) {
            return 0.into();
        }
        self.items[i.into()].prev
    }

    /// Gets the index of the parent of the given `i`.
    /// Failure returns idx 0.
    pub fn parent(&self, i: IDX) -> IDX {
        if !self.valid_idx(i) {
            return 0.into();
        }
        self.items[i.into()].parent
    }

    /// Gets the index of the first child, or 0 if no childs are present.
    /// Failure returns idx 0.
    pub fn child(&self, i: IDX) -> IDX {
        self.childc(i, 0)
    }

    /// Gets the index of the first child, or 0 if no childs are present.
    /// This retrieves the children for the category `c` if categories C>1,
    /// otherwise use the basic variant [`Packo::child`].
    /// Failure returns idx 0.
    pub fn childc(&self, i: IDX, c: usize) -> IDX {
        if !self.valid_cat(c) {
            return 0.into();
        }
        if !self.valid_idx(i) {
            return 0.into();
        }
        let i = self.items[i.into()].child[c];
        if i == 0.into() {
            return 0.into();
        }
        if !self.items[i.into()].flags.get_used() {
            return 0.into();
        }
        i
    }

    /// Adds the T as a standalone node in the items.
    /// This element will only be found when using the basic iterator.
    /// It has no childs and no parent.
    /// Failure returns idx 0.
    pub fn insert(&mut self, t: T) -> IDX {
        let Some(i): Option<IDX> = self
            .items
            .iter()
            .enumerate()
            .skip(1)
            .find(|(_, item)| !item.flags.get_used())
            .and_then(|(i, _)| i.try_into().ok())
        else {
            return 0.into();
        };
        self.items[i.into()].data = t;
        self.items[i.into()].flags.set_used(true);
        i
    }

    /// Appends this item as a child of the given `i`.
    /// If there were already children, append to the last of them, otherwise
    /// this new element will be the only children.
    /// Failure returns idx 0.
    pub fn nest(&mut self, i: IDX, t: T) -> IDX {
        self.nestc(i, 0, t)
    }

    /// Nests based on category index.
    /// If default C=1 of category sizes, then use [`Packo::nest`].
    /// Failure returns idx 0.
    pub fn nestc(&mut self, i: IDX, c: usize, t: T) -> IDX {
        if !self.valid_cat(c) {
            return 0.into();
        }
        if !self.valid_idx(i) {
            return 0.into();
        }
        let n = self.insert(t);
        if n.into() == 0 {
            return 0.into();
        }
        self.renestc(n, c, i);
        n
    }

    /// Takes an already allocated node and renests it into a different parent.
    /// Yanks it first with [`Packo::yank`].
    pub fn renestc(&mut self, i: IDX, c: usize, tgt: IDX) {
        if !self.valid_cat(c) {
            return;
        }
        if !self.valid_idx(i) {
            return;
        }
        if !self.valid_idx(tgt) {
            return;
        }
        let i = self.yank(i);
        let ch = self.childc(tgt, c);
        if ch.into() == 0 {
            self.items[tgt.into()].child[c] = i;
            self.items[i.into()].parent = tgt;
        } else {
            self.reappend(i, ch);
        }
    }

    /// Appends this element as a sibling after the given `i`.
    /// `i` can be at any hierarchy but must be a valid used node.
    /// Failure returns idx 0.
    pub fn append(&mut self, i: IDX, t: T) -> IDX {
        if !self.valid_idx(i) {
            return 0.into();
        }
        let n = self.insert(t);
        if n.into() == 0 {
            return 0.into();
        }
        self.reappend(n, i);
        n
    }

    /// Reappends a node `i` to `tgt`.
    /// The node being appended must be is already allocated and ids be valid.
    /// Yanks it first with [`Packo::yank`].
    /// If idx are not valid, this is a no-operation.
    pub fn reappend(&mut self, i: IDX, tgt: IDX) {
        if !self.valid_idx(i) {
            return;
        }
        if !self.valid_idx(tgt) {
            return;
        }
        if self.items[tgt.into()].prev == 0.into() {
            self.items[i.into()].next = tgt;
            self.items[tgt.into()].prev = i;
        }
        if self.items[tgt.into()].next == 0.into() {
            self.items[i.into()].prev = tgt;
            self.items[tgt.into()].next = i;
        } else {
            self.items[i.into()].next = self.items[tgt.into()].next;
            self.items[i.into()].prev = tgt;
            self.items[self.items[tgt.into()].next.into()].prev = i;
            self.items[tgt.into()].next = i;
        }
        self.items[i.into()].parent = self.items[tgt.into()].parent;
    }

    /// Remove this item from the list (sets it to unused).
    /// If an item has this one as a parent, its parent becomes 0/nil.
    /// If this item was part of a list, cut out its circular refs.
    pub fn remove(&mut self, i: IDX) -> T {
        // # Safety: Packo assumes T to be zero-initializable.
        let mut res: T = unsafe { MaybeUninit::zeroed().assume_init() };
        let i = self.yank(i);
        self.items[i.into()].flags.set_used(false);
        core::mem::swap(&mut res, &mut self.items[i.into()].data);
        res
    }

    /// Yanks this node out of any tree or list it's currently connected to,
    /// bringing it back as an isolated root node.
    /// This is useful to unhook a node prior to reassign it to another
    /// connection.
    /// Failure returns idx 0.
    /// The node is still allocated after this operation ends successfully, it
    /// simply isolates it from the tree/list structures.
    pub fn yank(&mut self, i: IDX) -> IDX {
        if !self.valid_idx(i) {
            return 0.into();
        }
        // Search for the parent, if this was the first child, find for an
        // alternate next if present, otherwise the partent.child has to become
        // 0.
        for c in 0..C {
            if self.items[self.items[i.into()].parent.into()].child[c] == i {
                // i was firstchild of this parent:
                // if next was 0, then there were no other linked items,
                // assigning next work in both cases.
                self.items[self.items[i.into()].parent.into()].child[c] = self.items[i.into()].next;
            }
        }
        // Unparent anything that used is as a parent.
        // This brings the block of nodes back to the root.
        for x in &mut self.items {
            if x.parent == i {
                x.parent = 0.into();
            }
        }
        let x = &self.items[i.into()];
        // This means it was part of a circular list
        if x.next != 0.into() {
            let xn = x.next;
            let xp = x.prev;
            if xn == xp {
                self.items[xn.into()].prev = 0.into();
                self.items[xp.into()].next = 0.into();
            } else {
                self.items[xn.into()].prev = xp;
                self.items[xp.into()].next = xn;
            }
        }
        i
    }

    /// Iterates on all the elements regardless of their relation or hierarchy.
    /// Only used elements are returned.
    pub fn all(&self) -> impl Iterator<Item = &T> {
        self.into_iter()
    }

    /// Iterates on all the elements regardless of their relation or hierarchy.
    /// Only used elements are returned.
    /// This variant returns only `IDX`.
    pub fn all_idx(&self) -> impl Iterator<Item = IDX> {
        PackoIdxIterator {
            packo: self,
            cur: 1.into(),
        }
    }

    /// Iterates only on the root nodes, nodes that have no parent.
    pub fn roots(&self) -> impl Iterator<Item = &T> {
        self.items
            .iter()
            .skip(1)
            .filter(|x| x.flags.get_used())
            .filter(|x| x.parent == 0.into())
            .filter(|x| x.next == 0.into())
            .filter(|x| x.prev == 0.into())
            .map(|x| &x.data)
    }

    /// Iterates only on the root nodes, nodes that have no parent.
    /// This variant returns only `IDX`.
    pub fn roots_idx(&self) -> impl Iterator<Item = IDX> {
        self.items
            .iter()
            .enumerate()
            .skip(1)
            .filter(|(_, x)| x.flags.get_used())
            .filter(|(_, x)| x.parent == 0.into())
            .filter(|(_, x)| x.next == 0.into())
            .filter(|(_, x)| x.prev == 0.into())
            .filter_map(|(i, _)| i.try_into().ok())
    }

    /// Iterates on the linked list which this item is the first element of
    pub fn siblings(&self, i: IDX) -> impl Iterator<Item = &T> {
        PackoSiblingsIterator {
            packo: self,
            start: i,
            cur: i,
        }
    }

    /// Iterates on the linked list which this item is the first element of
    /// This variant returns `IDX` only.
    pub fn siblings_idx(&self, i: IDX) -> impl Iterator<Item = IDX> {
        PackoSiblingsIdxIterator {
            packo: self,
            start: i,
            cur: i,
        }
    }

    /// Iterates on the siblings childs of this node.
    /// Basically only one level of the tree given `i` being the parent idx.
    pub fn childs(&self, i: IDX) -> impl Iterator<Item = &T> {
        self.childsc(i, 0)
    }

    /// Iterates on the siblings childs of this node.
    /// Basically only one level of the tree given `i` being the parent idx.
    /// This variant uses the category selector. For a simple version of C=1
    /// use [`Packo::childs`].
    pub fn childsc(&self, i: IDX, c: usize) -> impl Iterator<Item = &T> {
        let cur = self.childc(i, c);
        PackoSiblingsIterator {
            packo: self,
            start: cur,
            cur,
        }
    }

    /// Iterates on the siblings childs of this node.
    /// This variant returns `IDX` only.
    pub fn childs_idx(&self, i: IDX) -> impl Iterator<Item = IDX> {
        self.childsc_idx(i, 0)
    }

    /// Iterates on the siblings childs of this node.
    /// This variant returns `IDX` only.
    pub fn childsc_idx(&self, i: IDX, c: usize) -> impl Iterator<Item = IDX> {
        let cur = self.childc(i, c);
        PackoSiblingsIdxIterator {
            packo: self,
            start: cur,
            cur,
        }
    }

    pub fn iter(&self) -> PackoIterator<'_, S, T, C, IDX> {
        <&Self as IntoIterator>::into_iter(self)
    }

    fn valid_idx(&self, i: IDX) -> bool {
        if i.into() == 0 {
            return false;
        }
        if i.into() >= S {
            return false;
        }
        if !self.items[i.into()].flags.get_used() {
            return false;
        }
        true
    }

    #[allow(clippy::unused_self)]
    fn valid_cat(&self, c: usize) -> bool {
        if c >= C {
            return false;
        }
        true
    }
}

impl<const S: usize, T, const C: usize, IDX: IDXTraits> Default for Packo<S, T, C, IDX> {
    fn default() -> Self {
        const { assert!(S > 1, "Packo's size S, need S to be > 1") }
        // # Safety: Packo assumes T is zero-initializable.
        #[allow(clippy::uninit_assumed_init)]
        Self {
            dirty: unsafe { MaybeUninit::zeroed().assume_init() },
            items: unsafe { MaybeUninit::zeroed().assume_init() },
        }
    }
}

impl<const S: usize, T, const C: usize, IDX: IDXTraits> Display for Packo<S, T, C, IDX> {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        f.write_str(" IDX  Nxt  Prv  Par ")?;
        for c in 0..C {
            write!(f, "  C{c}")?;
        }
        writeln!(f)?;
        for (i, x) in self.items.iter().enumerate() {
            if !self.items[i].flags.get_used() {
                continue;
            }
            write!(f, "{:4} {:4} {:4} {:4} ", i, x.next, x.prev, x.parent)?;
            for c in 0..C {
                write!(f, "{:4}", x.child[c])?;
            }
            writeln!(f)?;
        }
        Ok(())
    }
}

impl<const S: usize, T, const C: usize, IDX: IDXTraits> core::ops::Index<IDX>
    for Packo<S, T, C, IDX>
{
    type Output = T;

    fn index(&self, mut i: IDX) -> &Self::Output {
        if i.into() >= S {
            i = 0.into();
        }
        if !self.items[i.into()].flags.get_used() {
            i = 0.into();
        }
        &self.items[i.into()].data
    }
}

impl<const S: usize, T, const C: usize, IDX: IDXTraits> core::ops::IndexMut<IDX>
    for Packo<S, T, C, IDX>
{
    fn index_mut(&mut self, i: IDX) -> &mut Self::Output {
        if i.into() == 0 {
            // Return dirty slot for writes at 0 (as if > /dev/null)
            return &mut self.dirty;
        }
        if i.into() >= S {
            // Return dirty slot for out of bounds
            return &mut self.dirty;
        }
        if !self.items[i.into()].flags.get_used() {
            // Unused slot will write to the dirty one
            return &mut self.dirty;
        }
        &mut self.items[i.into()].data
    }
}

impl<'a, const S: usize, T, const C: usize, IDX: IDXTraits> IntoIterator
    for &'a Packo<S, T, C, IDX>
{
    type Item = &'a T;

    type IntoIter = PackoIterator<'a, S, T, C, IDX>;

    fn into_iter(self) -> Self::IntoIter {
        PackoIterator {
            packo: self,
            cur: 1.into(),
        }
    }
}

/// Default iterator for [Packo].
///
/// Iterates all the nodes that are marked as used, regardless of sibling or
/// hierarchy.
/// Items can be normally added with [`Packo::insert`].
pub struct PackoIterator<'a, const S: usize, T, const C: usize, IDX: IDXTraits> {
    packo: &'a Packo<S, T, C, IDX>,
    cur: IDX,
}

impl<'a, const S: usize, T, const C: usize, IDX: IDXTraits> Iterator
    for PackoIterator<'a, S, T, C, IDX>
{
    type Item = &'a T;

    fn next(&mut self) -> Option<Self::Item> {
        while (self.cur.into()) < S && !self.packo.items[self.cur.into()].flags.get_used() {
            self.cur += 1.into();
        }
        if self.cur.into() >= S {
            return None;
        }
        let res = &self.packo.items[self.cur.into()].data;
        self.cur += 1.into();
        Some(res)
    }
}

/// Iterator based on `IDX`.
pub struct PackoIdxIterator<'a, const S: usize, T, const C: usize, IDX: IDXTraits> {
    packo: &'a Packo<S, T, C, IDX>,
    cur: IDX,
}

impl<const S: usize, T, const C: usize, IDX: IDXTraits> Iterator
    for PackoIdxIterator<'_, S, T, C, IDX>
{
    type Item = IDX;

    fn next(&mut self) -> Option<Self::Item> {
        while (self.cur.into()) < S && !self.packo.items[self.cur.into()].flags.get_used() {
            self.cur += 1.into();
        }
        if self.cur.into() >= S {
            return None;
        }
        let res = self.cur;
        self.cur += 1.into();
        Some(res)
    }
}

/// Iterator for [Packo] siblings.
///
/// Given the starting `IDX` keeps iterating in the linked list connected for
/// the siblings of this node. The siblings can be added with [`Packo::append`].
pub struct PackoSiblingsIterator<'a, const S: usize, T, const C: usize, IDX: IDXTraits> {
    packo: &'a Packo<S, T, C, IDX>,
    start: IDX,
    cur: IDX,
}

impl<'a, const S: usize, T, const C: usize, IDX: IDXTraits> Iterator
    for PackoSiblingsIterator<'a, S, T, C, IDX>
{
    type Item = &'a T;

    fn next(&mut self) -> Option<Self::Item> {
        if self.cur.into() == 0 {
            return None;
        }
        if self.cur.into() >= S {
            return None;
        }
        if !self.packo.items[self.cur.into()].flags.get_used() {
            return None;
        }
        let res = &self.packo.items[self.cur.into()];
        self.cur = res.next;
        // Reached the end: cur matches the first child after the first time
        if self.cur == self.start {
            self.cur = 0.into();
        }
        Some(&res.data)
    }
}

/// Iterator for [Packo] siblings but yields the `IDX` only.
pub struct PackoSiblingsIdxIterator<'a, const S: usize, T, const C: usize, IDX: IDXTraits> {
    packo: &'a Packo<S, T, C, IDX>,
    start: IDX,
    cur: IDX,
}

impl<const S: usize, T, const C: usize, IDX: IDXTraits> Iterator
    for PackoSiblingsIdxIterator<'_, S, T, C, IDX>
{
    type Item = IDX;

    fn next(&mut self) -> Option<Self::Item> {
        if self.cur == 0.into() {
            return None;
        }
        if self.cur.into() >= S {
            return None;
        }
        if !self.packo.items[self.cur.into()].flags.get_used() {
            return None;
        }
        let res = self.cur;
        self.cur = self.packo.items[res.into()].next;
        // Reached the end: cur matches the first child after the first time
        if self.cur == self.start {
            self.cur = 0.into();
        }
        Some(res)
    }
}

#[derive(Clone, Copy, Default)]
struct Flags(u8);

impl Flags {
    const USED: u8 = 1;

    fn get_used(self) -> bool {
        self.0 & Self::USED > 0
    }

    fn set_used(&mut self, b: bool) {
        if b {
            self.0 |= Self::USED;
        } else {
            self.0 &= !Self::USED;
        }
    }
}

struct Tracko<const S: usize, T, const C: usize, IDX: IDXTraits> {
    flags: Flags,
    next: IDX,
    prev: IDX,
    parent: IDX,
    child: [IDX; C],
    data: T,
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_flags() {
        let mut f = Flags::default();
        assert!(!f.get_used());
        f.set_used(true);
        f.set_used(true);
        assert!(f.get_used());
        f.set_used(false);
        f.set_used(false);
        assert!(!f.get_used());
    }

    #[test]
    fn test_zero_stays_zero() {
        let mut p = Packo::<2, u8>::default();
        assert_eq!(p[0], 0);
        p[0] = 2;
        assert_eq!(p[0], 0);
    }

    #[test]
    fn test_out_of_index_gets_zero() {
        let mut p = Packo::<2, u8>::default();
        p[1] = 1;
        assert_eq!(p[1], 0);
        assert_eq!(p[2], 0);
        p[55] = 1;
        assert_eq!(p[55], 0);
    }

    #[test]
    fn test_change_other_elements() {
        let mut p = Packo::<3, u8>::default();
        p.insert(0);
        p.insert(0);

        assert_eq!(p[1], 0);
        p[1] = 2;
        assert_eq!(p[1], 2);
        assert_eq!(p[2], 0);
        p[2] = 4;
        assert_eq!(p[2], 4);
    }

    #[test]
    fn find_all_match() {
        let mut p = Packo::<3, u8>::default();
        let first = p.insert(0);
        assert_eq!(*p.all().next().unwrap(), 0);
        assert_eq!(p.all_idx().next().unwrap(), first);

        let second = p.insert(0);
        // No items available anymore, return index 0 (out of memory)
        assert_eq!(p.insert(0), 0);
        assert!(!p.all().any(|x| *x == 1));
        p[first] = 1;
        p[second] = 2;
        assert_eq!(p.all().find(|x| **x == 1).unwrap(), &1);
        assert_eq!(p.all_idx().find(|x| *x == first).unwrap(), first);
        assert_eq!(p.remove(first), 1);
        assert!(!p.all().any(|x| *x == 1));
        assert!(!p.all_idx().any(|x| x == first));
        p[second] = 1;
        p.reset();
        assert!(!p.all().any(|x| *x == 1));
        assert!(!p.all_idx().any(|x| x == second));
    }

    #[test]
    fn check_zero_states() {
        let mut p = Packo::<10, u8>::default();
        p.remove(0);
        p.remove(20);
        assert_eq!(p.childc(0, 1), 0);
        assert_eq!(p.child(0), 0);
        assert_eq!(p.child(1), 0);
        assert_eq!(p.child(20), 0);
        assert_eq!(p.parent(0), 0);
        assert_eq!(p.parent(1), 0);
        assert_eq!(p.parent(20), 0);
        assert_eq!(p.next(0), 0);
        assert_eq!(p.next(1), 0);
        assert_eq!(p.next(20), 0);
        assert_eq!(p.prev(0), 0);
        assert_eq!(p.prev(2), 0);
        assert_eq!(p.prev(20), 0);
        assert_eq!(p.append(0, 0), 0);
        assert_eq!(p.append(2, 0), 0);
        assert_eq!(p.append(20, 0), 0);
        assert_eq!(p.nestc(0, 0, 1), 0);
        assert_eq!(p.nest(0, 0), 0);
        assert_eq!(p.nest(2, 0), 0);
        assert_eq!(p.nest(20, 0), 0);
        assert_eq!(p.childs(0).next(), None);
        assert_eq!(p.childs(2).next(), None);
        assert_eq!(p.childs(20).next(), None);
        assert_eq!(p.childs_idx(0).next(), None);
        assert_eq!(p.childs_idx(2).next(), None);
        assert_eq!(p.childs_idx(20).next(), None);
    }

    #[test]
    fn tree() {
        let mut p = Packo::<10, u8>::default();
        let p1 = p.insert(1);
        assert_eq!(p.nestc(p1, 33, 1), 0);
        let c1 = p.nest(p1, 33);
        let c2 = p.nest(p1, 44);
        let c3 = p.append(c2, 55);
        assert_eq!(p.child(p1), c1);
        assert_eq!(p.next(c1), c2);
        assert_eq!(p.prev(c1), c3);
        assert_eq!(p.next(c2), c3);
        assert_eq!(p.prev(c2), c1);
        assert_eq!(p.next(c3), c1);
        assert_eq!(p.prev(c3), c2);

        let mut childs = p.childs(p1);
        assert_eq!(*childs.next().unwrap(), 33);
        assert_eq!(*childs.next().unwrap(), 44);
        assert_eq!(*childs.next().unwrap(), 55);
        drop(childs);

        let mut childs_idx = p.childs_idx(p1);
        assert_eq!(childs_idx.next().unwrap(), c1);
        assert_eq!(childs_idx.next().unwrap(), c2);
        assert_eq!(childs_idx.next().unwrap(), c3);
        drop(childs_idx);

        let mut childs = p.siblings(c1);
        assert_eq!(*childs.next().unwrap(), 33);
        assert_eq!(*childs.next().unwrap(), 44);
        assert_eq!(*childs.next().unwrap(), 55);
        drop(childs);

        let mut childs_idx = p.siblings_idx(c1);
        assert_eq!(childs_idx.next().unwrap(), c1);
        assert_eq!(childs_idx.next().unwrap(), c2);
        assert_eq!(childs_idx.next().unwrap(), c3);
        drop(childs_idx);

        let mut roots = p.roots();
        assert_eq!(*roots.next().unwrap(), 1);
        assert_eq!(roots.next(), None);
        drop(roots);

        let mut roots_idx = p.roots_idx();
        assert_eq!(roots_idx.next().unwrap(), p1);
        assert_eq!(roots_idx.next(), None);
        drop(roots_idx);

        assert_eq!(p.remove(c2), 44);

        assert_eq!(p.child(p1), c1);
        assert_eq!(p.next(c1), c3);
        assert_eq!(p.prev(c1), c3);
        assert_eq!(p.next(c3), c1);
        assert_eq!(p.prev(c3), c1);

        assert_eq!(p.remove(c3), 55);

        assert_eq!(p.child(p1), c1);
        assert_eq!(p.next(c1), 0);
        assert_eq!(p.prev(c1), 0);

        assert_eq!(p.remove(c1), 33);

        assert_eq!(p.child(p1), 0);

        let c1 = p.nest(p1, 99);
        assert_eq!(p.parent(c1), p1);
        assert_eq!(p.remove(p1), 1);
        assert_eq!(p.parent(c1), 0);

        assert_eq!(p.remove(p1), 0);
        assert_eq!(p[p1], 0);
    }

    #[test]
    fn test_filled_up_packo() {
        let mut p = Packo::<3, u8>::default();
        let p1 = p.insert(1);
        let c1 = p.nest(p1, 2);
        assert_eq!(p.append(c1, 3), 0);
        assert_eq!(p.nest(p1, 3), 0);
        assert_eq!(p.insert(3), 0);
    }

    #[test]
    fn test_iters_max() {
        let mut p = Packo::<3, u8>::default();
        p.insert(1);
        p.insert(1);
        assert_eq!(p.all().count(), 2);
        assert_eq!(p.siblings(1).count(), 1);
        assert_eq!(p.all_idx().count(), 2);
        assert_eq!(p.siblings_idx(1).count(), 1);
    }

    #[test]
    fn test_iters_holes() {
        let mut p = Packo::<5, u8>::default();
        let d = p.insert(0);
        let p1 = p.insert(1);
        let c1 = p.nest(p1, 1);
        assert_ne!(p.append(c1, 1), 0);
        p.remove(c1);
        p.remove(d);
        assert_eq!(p.all().count(), 2);
        assert_eq!(p.siblings(p1).count(), 1);
        assert_eq!(p.all_idx().count(), 2);
        assert_eq!(p.siblings_idx(p1).count(), 1);
    }

    use core::fmt::{Debug, Write};

    struct WS<const S: usize> {
        len: usize,
        bytes: [u8; S],
    }

    impl<const S: usize> Debug for WS<S> {
        fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
            f.write_str("\"")?;
            f.write_str(str::from_utf8(&self.bytes[..self.len]).unwrap())?;
            f.write_str("\"")?;
            Ok(())
        }
    }

    impl<const S: usize> Default for WS<S> {
        fn default() -> Self {
            Self {
                len: 0,
                bytes: [0; S],
            }
        }
    }

    impl<const S: usize> Write for WS<S> {
        fn write_str(&mut self, s: &str) -> core::fmt::Result {
            for b in s.bytes() {
                assert!(self.len < S, "out of memory");
                self.bytes[self.len] = b;
                self.len += 1;
            }
            Ok(())
        }
    }

    impl<const S: usize> PartialEq<&str> for WS<S> {
        fn eq(&self, other: &&str) -> bool {
            let this = str::from_utf8(&self.bytes[..self.len]).unwrap();
            this == *other
        }
    }

    #[test]
    fn test_display_empty() {
        let p = Packo::<4, u8>::default();
        let mut w = WS::<100>::default();
        write!(w, "{p}").unwrap();
        assert_eq!(w, " IDX  Nxt  Prv  Par   C0\n");
    }

    #[test]
    fn test_display_one() {
        let mut p = Packo::<4, u8>::default();
        p.insert(1);
        let mut w = WS::<100>::default();
        write!(w, "{p}").unwrap();
        assert_eq!(w, " IDX  Nxt  Prv  Par   C0\n   1    0    0    0    0\n");
    }

    #[test]
    fn test_display_nest() {
        let mut p = Packo::<4, u8>::default();
        let i = p.insert(1);
        p.nest(i, 2);
        let mut w = WS::<100>::default();
        write!(w, "{p}").unwrap();
        assert_eq!(
            w,
            " IDX  Nxt  Prv  Par   C0\n   1    0    0    0    2\n   2    0    0    1    0\n"
        );
    }

    #[test]
    fn test_display_list() {
        let mut p = Packo::<4, u8>::default();
        let i = p.insert(1);
        p.append(i, 2);
        let mut w = WS::<100>::default();
        write!(w, "{p}").unwrap();
        assert_eq!(
            w,
            " IDX  Nxt  Prv  Par   C0\n   1    2    2    0    0\n   2    1    1    0    0\n"
        );
    }
}