use crate::dynamics::ccd::RigidBodyMotion;
use crate::dynamics::RigidBody;
use crate::math::{Isometry, Real};
use parry::motion::{ConstantLinearVelocityRigidMotion, RigidMotion};

#[derive(Copy, Clone, Debug)]
pub struct CCDData {
    pub position0: Isometry<Real>,
    pub time: Real,
}

impl CCDData {
    pub fn new(body: &RigidBody, dt: Real) -> Self {
        Self {
            position0: body.position_before_integration,
            time: 0.0,
        }
    }

    pub fn advance(&mut self, dt: Real, body: &RigidBody, time_ratio: Real) {
        let motion = self.motion(dt, body, 0.0);
        self.position0 = motion.position_at_time(time_ratio);
    }

    pub fn validate_advancement(&mut self, body: &RigidBody) {
        self.position0 = body.position
    }

    pub fn clamp_advancement(&self, body: &mut RigidBody) {
        body.position.translation = self.position0.translation;
    }

    pub fn motion(&self, dt: Real, body: &RigidBody, time_origin: Real) -> RigidBodyMotion {
        if body.is_ccd_enabled(dt) {
            let p0 = Isometry::from_parts(self.position0.translation, body.position.rotation);
            let motion = ConstantLinearVelocityRigidMotion::new(time_origin, p0, body.linvel);
            RigidBodyMotion::RigidLinear(motion)
        } else {
            RigidBodyMotion::Static(body.position)
        }
    }

    pub fn safe_position(&self, body: &RigidBody) -> Isometry<Real> {
        Isometry::from_parts(self.position0.translation, body.position.rotation)
    }
}
