linear-sim 0.7.0

use std;
use stash::Stash;
use vec_map::VecMap;
#[cfg(feature = "derive_serdes")]
use serde::{Deserialize, Serialize};

use crate::{constraint, event, math, object};
use super::{CONTACT_DISTANCE, InternalId, ObjectPair, Proximity};

pub (crate) mod group;
pub (crate) use self::group::Group;

#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Debug, PartialEq)]
pub struct Contact {
  pub constraint : constraint::Planar
}

/// A "TOI contact".
///
/// A pair of objects together with a contact plane and non-negative restitution
/// value.
///
/// A resting contact will have a restitution value of 0.0 and a colliding
/// contact will usually have a positive non-zero restitution value.
///
/// ⚠ Note that the `PartialOrd` and `Ord` implementations are only so
/// that contacts can be sorted in the narrow TOI list. Attempting to compare
/// contacts will always panic.
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Debug, PartialEq)]
pub struct Colliding {
  pub contact     : Contact,
  pub restitution : math::Normalized <f64>
}

/// Persistent contact group manager
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Debug)]
pub (crate) struct Manager {
  // NOTE: we put this field in an option so we can take and iterate over groups
  // without borrowing the manager
  pub contact_groups            : Option <Stash <Group>>,
  /// Number of contacts for static objects.
  ///
  /// Entries are only occupied when the number of contacts is greater than
  /// zero.
  object_contacts_static        : VecMap <u32>,
  /// Group key and number of contacts for dynamic objects
  object_group_contacts_dynamic : VecMap <(group::KeyType, u32)>
}

impl Manager {
  pub(super) fn get_group (&self, key : group::KeyType) -> Option <&Group> {
    self.contact_groups.as_ref().unwrap().get (key as usize)
  }
  pub(super) fn get_group_mut (&mut self, key : group::KeyType) -> Option <&mut Group> {
    self.contact_groups.as_mut().unwrap().get_mut (key as usize)
  }
  pub(super) fn remove_group (&mut self, key : group::KeyType) -> Option <Group> {
    self.contact_groups.as_mut().unwrap().take (key as usize)
  }
  pub(super) fn add_contact (&mut self, object_pair : ObjectPair, contact : Contact) {
    let (object_id_a, object_id_b) = object_pair.into();
    let group = match (
      self.get_group_key (object_id_a), self.get_group_key (object_id_b)
    ) {
      (Some (group_a), None) => {
        // dynamic object b joins existing group a
        self.increment_contact (object_id_a);
        self.assign_dynamic_group_key (object_id_b, group_a);
        self.get_group_mut (group_a).unwrap()
      }
      (None, Some (group_b)) => {
        // object a joins existing group b
        if object_id_a.kind() == object::Kind::Dynamic {
          self.assign_dynamic_group_key (object_id_a, group_b);
        } else {
          self.increment_contact (object_id_a);
        }
        self.increment_contact (object_id_b);
        self.get_group_mut (group_b).unwrap()
      }
      (Some (group_a), Some (group_b)) => {
        self.increment_contact (object_id_a);
        self.increment_contact (object_id_b);
        if group_a == group_b {
          self.get_group_mut (group_a).unwrap()
        } else {
          // merge groups
          let group_b = self.remove_group (group_b).unwrap();
          for (pair, _) in group_b.contacts.iter() {
            let (id_a, id_b) = (*pair).into();
            if id_a.kind() == object::Kind::Dynamic {
              self.change_dynamic_group_key (id_a, group_a);
            }
            self.change_dynamic_group_key (id_b, group_a);
          }
          let group_a = self.get_group_mut (group_a).unwrap();
          group_a.contacts.extend (group_b.contacts);
          group_a
        }
      }
      (None, None) => {
        // new group
        let new_group_index = self.contact_groups.as_mut().unwrap()
          .put (Group::default());
        debug_assert!(new_group_index < group::KeyType::MAX as usize);
        let new_group_key = new_group_index as group::KeyType;
        if object_id_a.kind() == object::Kind::Dynamic {
          self.assign_dynamic_group_key (object_id_a, new_group_key);
        } else {
          self.increment_contact (object_id_a);
        }
        self.assign_dynamic_group_key (object_id_b, new_group_key);
        self.get_group_mut (new_group_key).unwrap()
      }
    };
    group.contacts.push ((object_pair, contact));
  }

  /// Remove the object if it belongs to a persistent contact group.
  ///
  /// Returns true if the object was found and removed, otherwise returns false if the
  /// object was not in a contact group.
  #[must_use]
  pub(super) fn remove_object (&mut self, object_id : InternalId) -> bool {
    if let Some (group_key) = self.get_group_key (object_id) {
      debug_assert_eq!(object_id.kind(), object::Kind::Dynamic);
      let group   = self.get_group_mut (group_key).unwrap();
      let removed = group.remove_object (object_id);
      let empty   = group.contacts.is_empty();
      if empty {
        let _ = self.remove_group (group_key).unwrap();
      }
      for id in removed {
        self.decrement_contact (id);
      }
      self.object_group_contacts_dynamic.remove (object_id.key().index()).unwrap();
      true
    } else if object_id.kind() == object::Kind::Static {
      let index = object_id.key().index();
      if let Some (mut contact_count) = self.object_contacts_static.remove (index) {
        debug_assert!(contact_count > 0);
        let mut empty = vec![];
        let mut contact_groups = self.contact_groups.take().unwrap();
        for (i, group) in contact_groups.iter_mut() {
          let removed = group.remove_object (object_id);
          if group.contacts.is_empty() {
            empty.push (i);
          }
          contact_count -= removed.len() as u32;
          for id in removed {
            self.decrement_contact (id);
          }
          if contact_count == 0 {
            break
          }
        }
        for i in empty {
          contact_groups.take (i);
        }
        self.contact_groups = Some (contact_groups);
        true
      } else {
        false
      }
    } else {
      false
    }
  }

  pub(super) fn output_contacts (&self, output : &mut Vec <event::Output>) {
    for group in self.contact_groups.as_ref().unwrap().values() {
      for (object_pair, contact) in group.contacts.iter().cloned() {
        let (id_a, id_b) = object_pair.into();
        output.push (event::Contact {
          object_id_a: id_a.into(),
          object_id_b: id_b.into(),
          contact
        }.into())
      }
    }
  }

  /// Takes list of contact indices to remove.
  ///
  /// List should be non-empty. Note that the modified group may no longer be
  /// connected and `group.partition()` should be called to get connected
  /// components.
  pub(super) fn remove_contacts (&mut self, group : &mut Group, remove_list : &[u32]) {
    debug_assert!(!remove_list.is_empty());
    group.contacts = group.contacts.drain (..).enumerate().filter_map (
      |(i, contact@(object_pair, _))|
      if remove_list.contains (&(i as u32)) {
        let (object_id_a, object_id_b) = object_pair.into();
        self.decrement_contact (object_id_a);
        self.decrement_contact (object_id_b);
        None
      } else {
        Some (contact)
      }
    ).collect();
  }

  /// Get the contact count for an object
  pub(super) fn get_contact_count (&self, object_id : InternalId) -> Option <u32> {
    let index = object_id.key().index();
    match object_id.kind() {
      object::Kind::Static  => self.object_contacts_static.get (index).copied(),
      object::Kind::Dynamic => self.object_group_contacts_dynamic.get (index)
        .map (|g| g.1),
      object::Kind::Nodetect => unreachable!()
    }
  }

  /// Get the group key for dynamic object, if it exists, otherwise returns None
  /// if the object is static
  pub(super) fn get_group_key (&self, object_id : InternalId)
    -> Option <group::KeyType>
  {
    let index = object_id.key().index();
    match object_id.kind() {
      // static objects can be members of more than one group
      object::Kind::Static  => None,
      object::Kind::Dynamic => self.object_group_contacts_dynamic.get (index)
        .map (|g| g.0),
      object::Kind::Nodetect => unreachable!()
    }
  }

  /// Change existing group key
  pub(super) fn change_dynamic_group_key (&mut self,
    object_id : InternalId, group_key : group::KeyType
  ) {
    match object_id.kind() {
      object::Kind::Static  => unreachable!(),
      object::Kind::Dynamic => {
        let index = object_id.key().index();
        self.object_group_contacts_dynamic[index].0 = group_key;
      }
      object::Kind::Nodetect => unimplemented!()
    }
  }

  /// Assign a new group key to a dynamic object
  fn assign_dynamic_group_key (&mut self,
    object_id : InternalId, group_key : group::KeyType
  ) {
    let index = object_id.key().index();
    match object_id.kind() {
      object::Kind::Static  => unreachable!("call increment_contact instead"),
      object::Kind::Dynamic =>
        assert!(self.object_group_contacts_dynamic.insert (index, (group_key, 1))
          .is_none()),
      object::Kind::Nodetect => unimplemented!()
    }
  }

  /// Increment the contact counter.
  ///
  /// Adds the contact counter if this is the first contact of a static object.
  fn increment_contact (&mut self, object_id : InternalId) {
    let index = object_id.key().index();
    match object_id.kind() {
      object::Kind::Static  =>
        *self.object_contacts_static.entry (index).or_insert (0) += 1,
      object::Kind::Dynamic => self.object_group_contacts_dynamic[index].1 += 1,
      object::Kind::Nodetect => unimplemented!()
    }
  }

  /// Reduce the contact count for the given object ID
  fn decrement_contact (&mut self, object_id : InternalId) {
    let index = object_id.key().index();
    match object_id.kind() {
      object::Kind::Dynamic => {
        let count = &mut self.object_group_contacts_dynamic.get_mut (index).unwrap().1;
        *count -= 1;
        if *count == 0 {
          self.object_group_contacts_dynamic.remove (index);
        }
      }
      object::Kind::Static  => {
        let count = self.object_contacts_static.get_mut (index).unwrap();
        *count -= 1;
        if *count == 0 {
          self.object_contacts_static.remove (index);
        }
      }
      object::Kind::Nodetect => unreachable!()
    }
  }
}

impl Default for Manager {
  fn default() -> Self {
    Manager {
      contact_groups:                Some (Stash::default()),
      object_contacts_static:        VecMap::default(),
      object_group_contacts_dynamic: VecMap::default()
    }
  }
}

impl TryFrom <Proximity> for Contact {
  type Error = ();
  fn try_from (proximity : Proximity) -> Result <Self, Self::Error> {
    if proximity.distance >= 0.0 && proximity.distance < CONTACT_DISTANCE {
      Ok (Contact { constraint: proximity.into() })
    } else {
      Err (())
    }
  }
}

impl std::ops::Deref for Colliding {
  type Target = Contact;
  fn deref (&self) -> &Contact {
    &self.contact
  }
}
impl Eq for Colliding { }
#[expect(clippy::non_canonical_partial_ord_impl)]
impl PartialOrd for Colliding {
  /// Panics: should not be called; this trait is implemented so that narrow TOI
  /// results can be automatically be sorted
  fn partial_cmp (&self, _rhs : &Self) -> Option <std::cmp::Ordering> {
    // TODO: compiler hint ?
    unreachable!()
  }
}
impl Ord for Colliding {
  fn cmp (&self, _other : &Self) -> std::cmp::Ordering {
    // TODO: compiler hint ?
    unreachable!()
  }
}