linear-sim 0.7.0

use std;
use geometry::Aabb3;
use vec_map::VecMap;
use sorted_vec::SortedSet;
#[cfg(feature = "derive_serdes")]
use serde::{Deserialize, Serialize};

use crate::{geometry, math, object};

use super::{InternalId, ObjectPair};

/// Data for broad-phase "sort and sweep" ("sweep and prune (SAP)") collision detection
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Debug, Default)]
pub(crate) struct Broad {
  /// Each sorted axis of both static and instantaneous dynamic AABB intervals
  axes_discrete   : SortedAxes,
  /// Each sorted axis of both static and swept dynamic AABB intervals
  axes_continuous : SortedAxes,
  /// Object pairs involved in last resolve collision; when calling
  /// `overlap_pairs_continuous`, these pairs will be excluded
  resolved        : SortedSet <ObjectPair>,
  /// Dynamic objects that were modified in the last resolve collision; when calling
  /// `overlap_pairs_continuous`, only overlaps involving these objects will be
  /// considered
  modified        : SortedSet <InternalId>
}

#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Debug, Default)]
struct SortedAxes {
  axes          : [Vec <Endpoint>; 3],
  overlap_pairs : OverlapPairs,
  order_indices : OrderIndices
}

/// Min or max point of an AABB dimension
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Copy, Debug, PartialEq)]
enum Endpoint {
  Min (f64, InternalId),
  Max (f64, InternalId)
}

/// Keeps track of the ordered (min, max) endpoint indices in the sorted axes
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Debug, Default)]
struct OrderIndices {
  // static is a keyword in rust
  static_ : VecMap <[(u32, u32); 3]>,
  dynamic : VecMap <[(u32, u32); 3]>
}

/// We only track overlap pairs involving at least one dynamic object (not static vs.
/// static).
///
/// To take advantage of temporal coherence, overlap pairs should be preserved between
/// steps.
///
/// The 4th set of overlap pairs contains the pairs that overlap on all 3 axes.
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Debug, Default, Eq, PartialEq)]
struct OverlapPairs ([SortedSet <ObjectPair>; 4]);

////////////////////////////////////////////////////////////////////////////////
//  impls                                                                     //
////////////////////////////////////////////////////////////////////////////////

impl Broad {
  #[inline]
  pub(crate) fn overlaps_discrete (&self, aabb : Aabb3 <f64>) -> Vec <InternalId> {
    self.axes_discrete.overlaps (aabb)
  }

  pub(super) fn add_object_static (&mut self, aabb : Aabb3 <f64>, key : object::Key) {
    let id = InternalId::new_static (key);
    self.axes_discrete.insert (aabb, id);
    self.axes_continuous.insert (aabb, id);
  }
  pub(super) fn add_object_dynamic (&mut self,
    aabb_discrete   : Aabb3 <f64>,
    aabb_continuous : Aabb3 <f64>,
    key             : object::Key
  ) {
    let id = InternalId::new_dynamic (key);
    self.axes_discrete.insert (aabb_discrete, id);
    self.axes_continuous.insert (aabb_continuous, id);
  }
  pub(super) fn remove_object (&mut self, id : InternalId) {
    // ensures this object does not exist in the resolved ids
    debug_assert!(self.resolved.is_empty());
    self.axes_discrete.remove (id);
    self.axes_continuous.remove (id);
  }
  #[inline]
  pub(super) fn get_aabb_static (&self, key : object::Key) -> Aabb3 <f64> {
    let id = InternalId::new_static (key);
    debug_assert_eq!(
      self.axes_discrete.get_aabb (id), self.axes_continuous.get_aabb (id));
    self.axes_discrete.get_aabb (id)
  }
  #[inline]
  pub(super) fn get_aabb_dynamic_discrete (&self, key : object::Key) -> Aabb3 <f64> {
    let id = InternalId::new_dynamic (key);
    self.axes_discrete.get_aabb (id)
  }
  #[inline]
  pub(super) fn get_aabb_dynamic_continuous (&self, key : object::Key) -> Aabb3 <f64> {
    let id = InternalId::new_dynamic (key);
    self.axes_continuous.get_aabb (id)
  }
  #[inline]
  pub(super) fn update_aabb_static (&mut self, aabb : Aabb3 <f64>, key : object::Key) {
    let id = InternalId::new_static (key);
    self.axes_discrete.update (aabb, id);
    self.axes_continuous.update (aabb, id);
  }
  #[inline]
  pub(super) fn update_aabb_dynamic_discrete (&mut self,
    aabb : Aabb3 <f64>,
    key  : object::Key
  ) {
    let id = InternalId::new_dynamic (key);
    self.axes_discrete.update (aabb, id);
  }
  #[inline]
  pub(super) fn update_aabb_dynamic_continuous (&mut self,
    aabb : Aabb3 <f64>,
    key  : object::Key
  ) {
    let id = InternalId::new_dynamic (key);
    self.axes_continuous.update (aabb, id);
  }

  #[inline]
  pub(super) fn add_resolved (&mut self, object_pair : ObjectPair) {
    use sorted_vec::FindOrInsert;
    match self.resolved.find_or_insert (object_pair) {
      FindOrInsert::Inserted (_) => {}
      FindOrInsert::Found    (_) => unreachable!()
    }
  }

  #[inline]
  pub(super) fn set_modified (&mut self, modified : SortedSet <InternalId>) {
    self.modified = modified;
  }

  /// Update dynamic AABBs
  #[inline]
  pub(super) fn begin_step (&mut self,
    objects_dynamic : &VecMap <object::Dynamic>,
    _step            : u64
  ) {
    for (i, object) in objects_dynamic.iter() {
      let key        = object::Key::from (i);
      let id         = InternalId::new_dynamic (key);
      let old_aabb   = self.axes_discrete.get_aabb (id);
      let new_aabb   = object.aabb_dilated();
      let swept_aabb = Aabb3::union (old_aabb, new_aabb);
      self.update_aabb_dynamic_discrete (new_aabb, key);
      self.update_aabb_dynamic_continuous (swept_aabb, key);
    }
  }

  pub(super) fn overlap_pairs_continuous (&mut self,
    overlap_pairs : &mut Vec <ObjectPair>,
    iter          : u64
  ) {
    debug_assert!(overlap_pairs.is_empty());
    if self.resolved.is_empty() && iter == 0 {
      // collect all overlaps on initial iteration
      overlap_pairs.extend (self.axes_continuous.overlap_pairs.0[3].iter().copied());
    } else if !self.resolved.is_empty() {
      // on subsequent iterations, only collect overlaps involving resolved
      // dynamic objects, and clear the list of resolved objects when finished
      for pair in self.axes_continuous.overlap_pairs.0[3].iter().copied() {
        if self.resolved.contains (&pair) {
          continue
        }
        let (id_a, id_b) = pair.into();
        debug_assert_eq!(id_b.kind(), object::Kind::Dynamic);
        if self.modified.binary_search_by_key (&id_b, |x| *x).is_ok() ||
          id_a.kind() == object::Kind::Dynamic &&
          self.modified.binary_search_by_key (&id_a, |x| *x).is_ok()
        {
          overlap_pairs.push (pair);
        }
      }
      self.resolved.clear();
      self.modified.clear();
    }
  }
}

impl SortedAxes {
  fn get_aabb (&self, id : InternalId) -> Aabb3 <f64> {
    let &[
      (index_min_x, index_max_x),
      (index_min_y, index_max_y),
      (index_min_z, index_max_z)
    ] = self.order_indices.get (id);
    let min = [
      self.axes[0][index_min_x as usize].point(),
      self.axes[1][index_min_y as usize].point(),
      self.axes[2][index_min_z as usize].point()
    ].into();
    let max = [
      self.axes[0][index_max_x as usize].point(),
      self.axes[1][index_max_y as usize].point(),
      self.axes[2][index_max_z as usize].point()
    ].into();
    Aabb3::with_minmax_unchecked (min, max)
  }
  /// Overlap query
  fn overlaps (&self, aabb : Aabb3 <f64>) -> Vec <InternalId> {
    let mut out     = SortedSet::new();
    let mut collect = SortedSet::new();
    for (i, axis) in self.axes.iter().enumerate() {
      let min       = aabb.min().0[i];
      let max       = aabb.max().0[i];
      let index_min = axis.binary_search_by (|p| p.point().partial_cmp (&min).unwrap())
        .unwrap_or_else (|i| i);
      collect.clear();
      let mut mins = vec![];
      for other_endpoint in axis.iter().skip (index_min) {
        // TODO: currently we traverse the axis from the given min to the end of
        // the axis; we could also traverse from the start of the axis until the
        // given max; a possible optimization would be to choose whichever
        // direction is shorter
        let (other_point, other_id) = other_endpoint.pair();
        if other_point > min {
          if other_point < max {
            if i == 0 {
              // could be inserted twice if both endpoints are found
              let _result = collect.find_or_insert (other_id);
            } else if out.binary_search (&other_id).is_ok() {
              debug_assert!(!out.is_empty());
              // could be inserted twice if both endpoints are found
              let _result = collect.find_or_insert (other_id);
            }
          } else if other_endpoint.is_min() {
            mins.push (other_id);
          } else if !mins.contains (&other_id) {
            if i == 0 {
              let _result = collect.find_or_insert (other_id);
            } else if out.binary_search (&other_id).is_ok() {
              debug_assert!(!out.is_empty());
              // could be inserted twice if both endpoints are found
              let _result = collect.find_or_insert (other_id);
            }
          }
        }
      }
      std::mem::swap (&mut out, &mut collect);
      if out.is_empty() {
        break
      }
    }
    out.into_vec()
  }
  fn insert (&mut self, aabb : Aabb3 <f64>, id : InternalId) {
    let mins  = aabb.min().0.into_array().map (|s| Endpoint::Min (s, id));
    let maxes = aabb.max().0.into_array().map (|s| Endpoint::Max (s, id));
    let mut order_indices = [(u32::MAX, u32::MAX); 3];
    for (i, axis) in self.axes.iter_mut().enumerate() {
      // TODO: currently we traverse the axis from the inserted min to the end
      // of the axis; we could also traverse from the start of the axis until
      // the inserted max; a possible optimization would be to choose whichever
      // direction is shorter
      let axis3 = math::Axis3::from_repr (i as u8).unwrap();
      // find location and insert min/max
      let min       = mins[i];
      let index_min = axis.binary_search_by (|p| p.partial_cmp (&min).unwrap())
        .unwrap_or_else (|i| i);
      axis.insert (index_min, min);
      let max       = maxes[i];
      let index_max = axis[index_min+1..]
        .binary_search_by (|p| p.partial_cmp (&max).unwrap())
        .unwrap_or_else (|i| i) + index_min + 1;
      axis.insert (index_max, max);
      order_indices[i] = (index_min as u32, index_max as u32);
      // for endpoints between min/max, collect overlaps and increment order
      // indices by one
      let mut min_overlapped = vec![];
      for endpoint in axis[index_min+1..index_max].iter() {
        let (_, other_id) = endpoint.pair();
        if id.kind() == object::Kind::Dynamic ||
          other_id.kind() == object::Kind::Dynamic
        {
          if endpoint.is_min() {
            min_overlapped.push (other_id);
            self.overlap_pairs.insert (axis3, (id, other_id).into());
          } else if !min_overlapped.contains (&other_id) {
            self.overlap_pairs.insert (axis3, (id, other_id).into());
          }
        }
        let order_indices = self.order_indices.get_mut (other_id);
        match endpoint {
          Endpoint::Min (..) => order_indices[i].0 += 1,
          Endpoint::Max (..) => order_indices[i].1 += 1
        }
      }
      // for endpoints after max, collect overlaps and increment indices by two
      let mut mins = vec![];
      for endpoint in axis[index_max+1..].iter() {
        let (_, other_id) = endpoint.pair();
        if id.kind() == object::Kind::Dynamic ||
          other_id.kind() == object::Kind::Dynamic
        {
          if endpoint.is_min() {
            mins.push (other_id);
          } else if !min_overlapped.contains (&other_id) &&
            !mins.contains (&other_id)
          {
            self.overlap_pairs.insert (axis3, (id, other_id).into());
          }
        }
        let order_indices = self.order_indices.get_mut (other_id);
        match endpoint {
          Endpoint::Min (..) => order_indices[i].0 += 2,
          Endpoint::Max (..) => order_indices[i].1 += 2
        }
      }
      debug_assert!(axis.is_sorted());
    }
    self.order_indices.insert (id, order_indices);
    if cfg!(debug_assertions) {
      self.verify_order_indices();
    }
    debug_assert_eq!(self.sweep(), self.overlap_pairs);
  }
  fn update (&mut self, aabb : Aabb3 <f64>, id : InternalId) {
    let order_indices = *self.order_indices.get (id);
    log::trace!(id:?; "sorted axes update");
    for (i, axis) in self.axes.iter_mut().enumerate() {
      let axis3 = math::Axis3::from_repr (i as u8).unwrap();
      let min_new = aabb.min().0[i];
      let max_new = aabb.max().0[i];
      let (index_min, index_max) = order_indices[i];
      let min_old = axis[index_min as usize].point();
      let max_old = axis[index_max as usize].point();
      let mut new_index_min = index_min as usize;
      let mut new_index_max = index_max as usize;
      axis[new_index_max].set_point (max_new);
      axis[new_index_min].set_point (min_new);
      // first perform operations that add overlaps (move min down or max up),
      // followed by operations that remove overlaps (move min up or max down)
      // TODO: doing things this way means that overlaps may be added only to be
      // removed when the other endpoint is moved; is it possible to avoid this
      // by looking at the old/new endpoints?
      if min_old > min_new {
        // sort min down: only add overlaps
        loop {
          if new_index_min == 0 {
            break
          }
          let index_left = new_index_min - 1;
          let other_endpoint = axis[index_left];
          let (other_point, other_id) = other_endpoint.pair();
          if min_new < other_point {
            if other_endpoint.is_max() && (
              id.kind() == object::Kind::Dynamic ||
              other_id.kind() == object::Kind::Dynamic
            ) {
              // add overlap
              self.overlap_pairs.insert (axis3, (id, other_id).into());
            }
            // update order indices
            self.order_indices.get_mut (id)[i].0 -= 1;
            match other_endpoint {
              Endpoint::Min (..) =>
                self.order_indices.get_mut (other_id)[i].0 += 1,
              Endpoint::Max (..) =>
                self.order_indices.get_mut (other_id)[i].1 += 1
            }
            // swap
            axis.swap (new_index_min, index_left);
            new_index_min = index_left;
          } else {
            break
          }
        }
      }
      if max_new > max_old {
        // sort max up: only add overlaps
        loop {
          if new_index_max == axis.len() - 1 {
            break
          }
          let index_right = new_index_max + 1;
          let other_endpoint = axis[index_right];
          let (other_point, other_id) = axis[index_right].pair();
          if max_new > other_point {
            if other_endpoint.is_min() && (
              id.kind() == object::Kind::Dynamic ||
              other_id.kind() == object::Kind::Dynamic
            ) {
              // add overlap
              self.overlap_pairs.insert (axis3, (id, other_id).into());
            }
            // update order indices
            match other_endpoint {
              Endpoint::Min (..) =>
                self.order_indices.get_mut (other_id)[i].0 -= 1,
              Endpoint::Max (..) =>
                self.order_indices.get_mut (other_id)[i].1 -= 1
            }
            // swap
            axis.swap (new_index_max, index_right);
            new_index_max = index_right;
          } else {
            break
          }
        }
      }
      if min_new > min_old {
        // sort min up: only remove overlaps
        loop {
          if new_index_min == axis.len() - 1 {
            break
          }
          let index_right = new_index_min + 1;
          let other_endpoint = axis[index_right];
          let (other_point, other_id) = axis[index_right].pair();
          if min_new > other_point {
            if other_endpoint.is_max() && (
              id.kind() == object::Kind::Dynamic ||
              other_id.kind() == object::Kind::Dynamic
            ) {
              // remove overlap
              self.overlap_pairs.remove (axis3, (id, other_id).into());
            }
            // update order indices
            match other_endpoint {
              Endpoint::Min (..) =>
                self.order_indices.get_mut (other_id)[i].0 -= 1,
              Endpoint::Max (..) =>
                self.order_indices.get_mut (other_id)[i].1 -= 1
            }
            // swap
            axis.swap (new_index_min, index_right);
            new_index_min = index_right;
          } else {
            break
          }
        }
      }
      if max_old > max_new {
        // sort max down: only remove overlaps
        loop {
          if new_index_max == 0 {
            break
          }
          let index_left = new_index_max - 1;
          let other_endpoint = axis[index_left];
          let (other_point, other_id) = other_endpoint.pair();
          if max_new < other_point {
            if other_endpoint.is_min() && (
              id.kind() == object::Kind::Dynamic ||
              other_id.kind() == object::Kind::Dynamic
            ) {
              // remove overlap
              self.overlap_pairs.remove (axis3, (id, other_id).into());
            }
            // update order indices
            match other_endpoint {
              Endpoint::Min (..) =>
                self.order_indices.get_mut (other_id)[i].0 += 1,
              Endpoint::Max (..) =>
                self.order_indices.get_mut (other_id)[i].1 += 1
            }
            // swap
            axis.swap (new_index_max, index_left);
            new_index_max = index_left;
          } else {
            break
          }
        }
      }
      self.order_indices.get_mut (id)[i] =
        (new_index_min as u32, new_index_max as u32);
      log::trace!(index=i, axis:?; "sorted axis update after");
      //debug_assert!(axis.is_sorted());
    }
    /*
    if cfg!(debug_assertions) {
      self.verify_order_indices();
    }
    debug_assert_eq!(self.sweep(), self.overlap_pairs);
    debug_assert_eq!(self.aabb_check(), self.overlap_pairs.0[3]);
    */
  }
  fn remove (&mut self, id : InternalId) {
    let order_indices = self.order_indices.remove (id);
    for (i, axis) in self.axes.iter_mut().enumerate() {
      let (index_min, index_max) = order_indices[i];
      // for endpoints between min/max, decrement order indices by one
      for endpoint in axis[index_min as usize+1..index_max as usize].iter() {
        let (_, other_id) = endpoint.pair();
        let order_indices = self.order_indices.get_mut (other_id);
        match endpoint {
          Endpoint::Min (..) => order_indices[i].0 -= 1,
          Endpoint::Max (..) => order_indices[i].1 -= 1
        }
      }
      // for endpoints after max, decrement order indices by two
      for endpoint in axis[index_max as usize+1..].iter() {
        let (_, other_id) = endpoint.pair();
        let order_indices = self.order_indices.get_mut (other_id);
        match endpoint {
          Endpoint::Min (..) => order_indices[i].0 -= 2,
          Endpoint::Max (..) => order_indices[i].1 -= 2
        }
      }
      // remove endpoints
      axis.remove (index_max as usize);
      axis.remove (index_min as usize);
    }
    self.overlap_pairs.remove_id (id);
    if cfg!(debug_assertions) {
      self.verify_order_indices();
    }
    debug_assert_eq!(self.sweep(), self.overlap_pairs);
  }
  #[expect(clippy::needless_range_loop)]
  fn verify_order_indices (&self) {
    for (index, indices) in self.order_indices.static_.iter() {
      let id = InternalId::new_static (object::Key::from (index));
      for i in 0..3 {
        let min = self.axes[i][indices[i].0 as usize];
        debug_assert_eq!(min.id(), id, "axis[{i}], id: {id:?}");
        debug_assert!(min.is_min(), "axis[{i}], id: {id:?}");
        let max = self.axes[i][indices[i].1 as usize];
        debug_assert_eq!(max.id(), id, "axis[{i}], id: {id:?}");
        debug_assert!(max.is_max(), "axis[{i}], id: {id:?}");
      }
    }
    for (index, indices) in self.order_indices.dynamic.iter() {
      let id = InternalId::new_dynamic (object::Key::from (index));
      for i in 0..3 {
        let min = self.axes[i][indices[i].0 as usize];
        debug_assert_eq!(min.id(), id, "axis[{i}], id: {id:?}");
        debug_assert!(min.is_min(), "axis[{i}], id: {id:?}");
        let max = self.axes[i][indices[i].1 as usize];
        debug_assert_eq!(max.id(), id, "axis[{i}], id: {id:?}");
        debug_assert!(max.is_max(), "axis[{i}], id: {id:?}");
      }
    }
  }
  fn sweep (&self) -> OverlapPairs {
    let mut overlaps = OverlapPairs::default();
    for (i, axis) in self.axes.iter().enumerate() {
      let axis3 = math::Axis3::from_repr (i as u8).unwrap();
      let mut open : Vec <InternalId> = vec![];
      for endpoint in axis.iter() {
        let id = endpoint.id();
        if endpoint.is_min() {
          for open_id in open.iter() {
            if open_id.kind() == object::Kind::Dynamic ||
              id.kind() == object::Kind::Dynamic
            {
              overlaps.insert (axis3, (*open_id, id).into());
            }
          }
          open.push (id);
        } else {
          open.remove (open.iter().position (|x| x == &id).unwrap());
        }
      }
    }
    overlaps
  }
  /// Do pair-wise overlap check on all AABBs
  #[expect(dead_code)]
  fn aabb_check (&self) -> SortedSet <ObjectPair> {
    let mut overlaps = vec![];
    for i in self.order_indices.static_.keys() {
      let id_static   = InternalId::new_static (object::Key::from (i));
      let aabb_static = self.get_aabb (id_static);
      for j in self.order_indices.dynamic.keys() {
        let id_dynamic   = InternalId::new_dynamic (object::Key::from (j));
        let aabb_dynamic = self.get_aabb (id_dynamic);
        if aabb_static.intersects (aabb_dynamic) {
          overlaps.push (ObjectPair::from ((id_static, id_dynamic)));
        }
      }
    }
    for (n, i) in self.order_indices.dynamic.keys().enumerate() {
      let id_a   = InternalId::new_dynamic (object::Key::from (i));
      let aabb_a = self.get_aabb (id_a);
      for (m, j) in self.order_indices.dynamic.keys().enumerate() {
        if m >= n {
          break
        }
        let id_b   = InternalId::new_dynamic (object::Key::from (j));
        let aabb_b = self.get_aabb (id_b);
        if aabb_a.intersects (aabb_b) {
          overlaps.push (ObjectPair::from ((id_a, id_b)));
        }
      }
    }
    SortedSet::from_unsorted (overlaps)
  }
}

impl OrderIndices {
  fn get (&self, id : InternalId) -> &[(u32, u32); 3] {
    self.get_map (id).get (id.key().index()).unwrap()
  }

  fn get_mut (&mut self, id : InternalId) -> &mut [(u32, u32); 3] {
    self.get_map_mut (id).get_mut (id.key().index()).unwrap()
  }

  fn insert (&mut self, id : InternalId, indices : [(u32, u32); 3]) {
    let result = self.get_map_mut (id).insert (id.key().index(), indices);
    debug_assert!(result.is_none());
  }

  fn remove (&mut self, id : InternalId) -> [(u32, u32); 3] {
    self.get_map_mut (id).remove (id.key().index()).unwrap()
  }

  fn get_map (&self, id : InternalId) -> &VecMap <[(u32, u32); 3]> {
    match id.kind() {
      object::Kind::Static   => &self.static_,
      object::Kind::Dynamic  => &self.dynamic,
      object::Kind::Nodetect => unreachable!()
    }
  }

  fn get_map_mut (&mut self, id : InternalId) -> &mut VecMap <[(u32, u32); 3]> {
    match id.kind() {
      object::Kind::Static   => &mut self.static_,
      object::Kind::Dynamic  => &mut self.dynamic,
      object::Kind::Nodetect => unreachable!()
    }
  }
}

impl OverlapPairs {
  fn insert (&mut self, axis : math::Axis3, pair : ObjectPair) {
    let component_index = axis.component();
    let result = self.0[component_index].find_or_insert (pair);
    debug_assert!(result.is_inserted());
    let mut overlap_all = true;
    for i in 0..3 {
      if i != component_index && self.0[i].binary_search (&pair).is_err() {
        overlap_all = false;
        break
      }
    }
    if overlap_all {
      let result = self.0[3].find_or_insert (pair);
      debug_assert!(result.is_inserted());
    }
  }

  fn remove (&mut self, axis : math::Axis3, pair : ObjectPair) {
    let component_index = axis.component();
    let result = self.0[component_index].remove_item (&pair);
    debug_assert!(result.is_some());
    self.0[3].remove_item (&pair);
  }

  fn remove_id (&mut self, id : InternalId) {
    for overlaps in self.0.iter_mut() {
      overlaps.retain (|ObjectPair (id_a, id_b)| id_a != &id && id_b != &id);
    }
  }
}

impl Endpoint {
  const fn pair (self) -> (f64, InternalId) {
    match self {
      Endpoint::Min (point, id) | Endpoint::Max (point, id) => (point, id)
    }
  }

  const fn point (self) -> f64 {
    match self {
      Endpoint::Min (point, _) | Endpoint::Max (point, _) => point
    }
  }

  const fn id (self) -> InternalId {
    match self {
      Endpoint::Min (_, id) | Endpoint::Max (_, id) => id
    }
  }

  const fn is_min (&self) -> bool {
    match self {
      Endpoint::Min (..) => true,
      Endpoint::Max (..) => false
    }
  }

  const fn is_max (&self) -> bool {
    match self {
      Endpoint::Min (..) => false,
      Endpoint::Max (..) => true
    }
  }

  const fn set_point (&mut self, point : f64) {
    match self {
      Endpoint::Min (p, _) | Endpoint::Max (p, _) => *p = point
    }
  }
}

impl PartialOrd for Endpoint {
  fn partial_cmp (&self, other : &Endpoint) -> Option <std::cmp::Ordering> {
    self.point().partial_cmp (&other.point())
  }
}

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

  #[test]
  fn add_object() {
    let mut broad = Broad::default();
    let aabb1 = Aabb3::with_minmax_unchecked (
      [0.0, 0.0, 0.0].into(), [2.0, 2.0, 2.0].into());
    let aabb2 = Aabb3::with_minmax_unchecked (
      [1.0, 1.0, -2.0].into(), [3.0, 3.0, -1.0].into());
    let aabb3 = Aabb3::with_minmax_unchecked (
      [2.0, 2.0, -3.0].into(), [3.0, 3.0, 0.0].into());
    broad.add_object_static (aabb1, 0u32.into());
    broad.add_object_dynamic (aabb2, aabb2, 0u32.into());
    broad.add_object_dynamic (aabb3, aabb3, 1u32.into());
    println!("{broad:?}");
  }

  #[test]
  fn remove_object() {
    let mut broad = Broad::default();
    let aabb1 = Aabb3::with_minmax_unchecked (
      [0.0, 0.0, 0.0].into(), [2.0, 2.0, 2.0].into());
    let aabb2 = Aabb3::with_minmax_unchecked (
      [1.0, 1.0, -2.0].into(), [3.0, 3.0, -1.0].into());
    let aabb3 = Aabb3::with_minmax_unchecked (
      [2.0, 2.0, -3.0].into(), [3.0, 3.0, 0.0].into());
    broad.add_object_static (aabb1, 0u32.into());
    broad.add_object_dynamic (aabb2, aabb2, 0u32.into());
    broad.add_object_dynamic (aabb3, aabb3, 1u32.into());
    broad.remove_object (InternalId::new_dynamic (0u32.into()));
    println!("{broad:?}");
  }
}