linear-sim 0.10.5

use derive_more::From;
use either::Either;

#[cfg(feature = "derive_serdes")]
use serde::{Deserialize, Serialize};

use crate::collision::Proximity;
use crate::object;
use crate::math::*;

use super::{Constraint, Impulses, PostImpulses};

/// Distance criterea for contacts and collisions (TOI contacts).
///
/// Continuous collision detection will aim to return the TOI for when the objects are
/// at a distance approximately `0.5 * CONTACT_DISTANCE` in order to prevent
/// intersection due to numerical error.
///
/// Post-impulse position correction will also move objects apart to a distance of
/// `0.5 * CONTACT_DISTANCE`.
pub const CONTACT_DISTANCE : f64 = 0.005;   // 5mm
/// Velocity for determination of whether a normal impulse should be applied
pub const RESTING_VELOCITY : f64 = (1.0/120.0) * CONTACT_DISTANCE;

/// A contact constraint
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Debug, PartialEq, From)]
pub struct Contact {
  pub normal   : Unit3 <f64>,
  pub distance : f64
}

impl Contact {
  #[inline]
  pub const fn new (normal : Unit3 <f64>, distance : f64) -> Self {
    Contact { normal, distance }
  }

  pub(crate) fn constrain_velocity_with_restitution (&self,
    mut object_a     : Either <&object::Static, &mut object::Dynamic>,
    object_b         : &mut object::Dynamic,
    pseudovelocity_a : Option <Vector3 <f64>>,
    pseudovelocity_b : Vector3 <f64>,
    // 0.0 for a resting contact
    restitution      : Normalized <f64>
  ) -> Option <Impulses> {
    let velocity_effective_a = match &object_a {
      Either::Left (_) => {
        debug_assert!(pseudovelocity_a.is_none());
        Vector3::zero()
      }
      Either::Right (object_a) => object_a.derivatives.velocity +
        pseudovelocity_a.unwrap_or (Vector3::zero())
    };
    let velocity_effective_b        = object_b.derivatives.velocity + pseudovelocity_b;
    let velocity_effective_relative = velocity_effective_a - velocity_effective_b;
    let velocity_normal_component   = velocity_effective_relative.dot (*self.normal);
    let mut impulses        = None;
    let mut mass_relative_a = 0.0;
    let mut mass_relative_b = 0.0;
    if velocity_normal_component < 0.0 {
      let velocity_normal = velocity_normal_component * *self.normal;
      // normal impulse
      let impulse_normal = velocity_normal + *restitution * velocity_normal;
      let impulse_normal_magnitude2 = impulse_normal.magnitude_squared();
      if impulse_normal_magnitude2 < RESTING_VELOCITY * RESTING_VELOCITY {
        return None
      }
      let (impulse_normal_a, impulse_normal_b) = match &mut object_a {
        Either::Right (object_a) => {
          let mass_combined    = object_a.mass + object_b.mass;
          mass_relative_a      = object_a.mass.mass() * mass_combined.mass_reciprocal();
          mass_relative_b      = object_b.mass.mass() * mass_combined.mass_reciprocal();
          let impulse_normal_a = -mass_relative_b * impulse_normal;
          let impulse_normal_b = mass_relative_a * impulse_normal;
          object_a.derivatives.velocity += impulse_normal_a;
          object_b.derivatives.velocity += impulse_normal_b;
          (impulse_normal_a, impulse_normal_b)
        }
        Either::Left (_) => {
          object_b.derivatives.velocity += impulse_normal;
          (Vector3::zero(), impulse_normal)
        }
      };
      // friction (tangent) impulse
      let (impulse_tangent_a, impulse_tangent_b) = {
        let velocity_tangent = velocity_effective_relative - velocity_normal;
        let velocity_tangent_magnitude2 = velocity_tangent.magnitude_squared();
        if velocity_tangent_magnitude2 > 0.0 {
          let friction_a = match &object_a {
            Either::Left  (x) => x.material.friction,
            Either::Right (x) => x.material.friction
          };
          let friction_coefficient = friction_a * object_b.material.friction;
          let impulse_tangent_potential_magnitude2 =
            *friction_coefficient * impulse_normal_magnitude2;
          let impulse_tangent = if impulse_tangent_potential_magnitude2 <
            velocity_tangent_magnitude2
          {
            (impulse_tangent_potential_magnitude2.sqrt()
            / velocity_tangent_magnitude2.sqrt()) * velocity_tangent
          } else {
            velocity_tangent
          };
          match object_a {
            Either::Right (object_a) => {
              let impulse_tangent_a = -mass_relative_b * impulse_tangent;
              let impulse_tangent_b = mass_relative_a * impulse_tangent;
              object_a.derivatives.velocity += impulse_tangent_a;
              object_b.derivatives.velocity += impulse_tangent_b;
              (impulse_tangent_a, impulse_tangent_b)
            }
            Either::Left (_) => {
              object_b.derivatives.velocity += impulse_tangent;
              (Vector3::zero(), impulse_tangent)
            }
          }
        } else {
          (Vector3::zero(), Vector3::zero())
        }
      };
      impulses = Some (Impulses {
        impulse_normal_a, impulse_normal_b,
        impulse_tangent_a, impulse_tangent_b
      });
    }
    impulses
  }
}

impl Constraint for Contact {
  /// Resting contact: restitution == 0.0
  fn constrain_velocity (&self,
    object_a         : Either <&object::Static, &mut object::Dynamic>,
    object_b         : &mut object::Dynamic,
    pseudovelocity_a : Option <Vector3 <f64>>,
    pseudovelocity_b : Vector3 <f64>
  ) -> Option <Impulses> {
    self.constrain_velocity_with_restitution (
      object_a, object_b, pseudovelocity_a, pseudovelocity_b, Normalized::zero())
  }

  /// Queries current proximity and applies pseudovelocity impulse at `T-t_reverse` to
  /// correct position.
  fn constrain_position (&mut self,
    t_reverse        : f64,
    mut object_a     : Either <&object::Static, &mut object::Dynamic>,
    object_b         : &mut object::Dynamic,
    pseudovelocity_a : Option <&mut Vector3 <f64>>,
    pseudovelocity_b : &mut Vector3 <f64>
  ) -> Option <PostImpulses> {
    let t_forward     = -t_reverse;
    let mut impulses  = None;
    let mut proximity = match &object_a {
      Either::Left  (object_a) => Proximity::query (*object_a, object_b),
      Either::Right (object_a) => Proximity::query (*object_a, object_b)
    };
    log::trace!(distance=proximity.distance; "post impulse contact @ T==1.0");
    if cfg!(debug_assertions) {
      approx::assert_abs_diff_eq!(proximity.normal.cross (proximity.half_axis),
        Vector3::zero());
    }
    let normal = proximity.normal;
    if proximity.distance < 0.0 {
      let a_to_b = -proximity.distance * *proximity.normal;
      let pseudo_impulse = (a_to_b + 0.5 * CONTACT_DISTANCE * *proximity.normal)
        / t_forward;
      let (pseudo_impulse_a, pseudo_impulse_b) = match &mut object_a {
        Either::Left  (object_a) => {
          object::lerp (object_b, *pseudovelocity_b, t_reverse);
          let pseudo_impulse_b = -pseudo_impulse;
          *pseudovelocity_b += pseudo_impulse_b;
          object::lerp (object_b, *pseudovelocity_b, t_forward);
          proximity = Proximity::query (*object_a, object_b);
          (Vector3::zero(), pseudo_impulse_b)
        }
        Either::Right (object_a) => {
          let pseudovelocity_a = pseudovelocity_a.unwrap();
          object::lerp (*object_a, *pseudovelocity_a, t_reverse);
          object::lerp (object_b, *pseudovelocity_b, t_reverse);
          let mass_combined    = object_a.mass + object_b.mass;
          let mass_relative_a  = object_a.mass.mass() * mass_combined.mass_reciprocal();
          let mass_relative_b  = object_b.mass.mass() * mass_combined.mass_reciprocal();
          let pseudo_impulse_a = mass_relative_b * pseudo_impulse;
          let pseudo_impulse_b = -mass_relative_a * pseudo_impulse;
          *pseudovelocity_a += pseudo_impulse_a;
          *pseudovelocity_b += pseudo_impulse_b;
          object::lerp (*object_a, *pseudovelocity_a, t_forward);
          object::lerp (object_b, *pseudovelocity_b, t_forward);
          proximity = Proximity::query (*object_a, object_b);
          (pseudo_impulse_a, pseudo_impulse_b)
        }
      };
      log::trace!(distance=proximity.distance; "position corrected distance @ T==1.0");
      debug_assert!(proximity.distance >= 0.0);
      if cfg!(debug_assertions) {
        approx::assert_relative_eq!(*proximity.normal, *normal, epsilon=0.000_000_1);
        approx::assert_abs_diff_eq!(
          proximity.distance, 0.5 * CONTACT_DISTANCE, epsilon=0.000_000_001);
      }
      impulses = Some (PostImpulses { pseudo_impulse_a, pseudo_impulse_b });
    }
    *self = Contact { normal: proximity.normal, distance: proximity.distance };
    impulses
  }
}

impl Eq for Contact { }
#[expect(clippy::non_canonical_partial_ord_impl)]
impl PartialOrd for Contact {
  /// 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 Contact {
  fn cmp (&self, _other : &Self) -> std::cmp::Ordering {
    // TODO: compiler hint ?
    unreachable!()
  }
}

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