use crate::math::{Isometry, Real, Vector};
use parry::motion::{ConstantLinearVelocityRigidMotion, ConstantVelocityRigidMotion, RigidMotion};

pub enum RigidBodyMotion {
    RigidLinear(ConstantLinearVelocityRigidMotion),
    RigidNonlinear(ConstantVelocityRigidMotion),
    Static(Isometry<Real>),
}

impl RigidBodyMotion {
    pub fn is_static_or_linear(&self) -> bool {
        match self {
            RigidBodyMotion::RigidLinear(_) | RigidBodyMotion::Static(_) => true,
            _ => false,
        }
    }

    pub fn linvel(&self) -> Vector<Real> {
        match self {
            RigidBodyMotion::RigidLinear(m) => m.velocity,
            RigidBodyMotion::RigidNonlinear(m) => m.linvel,
            RigidBodyMotion::Static(_m) => Vector::zeros(),
        }
    }
}

impl RigidMotion for RigidBodyMotion {
    fn position_at_time(&self, t: Real) -> Isometry<Real> {
        match self {
            RigidBodyMotion::RigidLinear(m) => m.position_at_time(t),
            RigidBodyMotion::RigidNonlinear(m) => m.position_at_time(t),
            RigidBodyMotion::Static(m) => m.position_at_time(t),
        }
    }
}
