use super::RigidBodyMotion;
use crate::data::Coarena;
use crate::dynamics::ccd::CCDData;
use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle};
use crate::geometry::{Collider, ColliderHandle};
use crate::math::{Isometry, Real, Vector};
use crate::utils::WCross;
use parry::motion::{RigidMotion, RigidMotionComposition};
use parry::query::{QueryDispatcher, TOIStatus};
use std::collections::HashMap;

#[derive(Copy, Clone, Debug)]
pub struct TOIEntry {
    pub toi: Real,
    pub c1: ColliderHandle,
    pub b1: RigidBodyHandle,
    pub c2: ColliderHandle,
    pub b2: RigidBodyHandle,
    pub is_intersection: bool,
    pub timestamp: usize,
}

impl TOIEntry {
    fn new(
        toi: Real,
        c1: ColliderHandle,
        b1: RigidBodyHandle,
        c2: ColliderHandle,
        b2: RigidBodyHandle,
        is_intersection: bool,
        timestamp: usize,
    ) -> Self {
        Self {
            toi,
            c1,
            b1,
            c2,
            b2,
            is_intersection,
            timestamp,
        }
    }

    pub fn try_from_colliders<QD: ?Sized + QueryDispatcher>(
        params: &IntegrationParameters,
        query_dispatcher: &QD,
        ch1: ColliderHandle,
        ch2: ColliderHandle,
        c1: &Collider,
        c2: &Collider,
        b1: &RigidBody,
        b2: &RigidBody,
        is_intersection: bool,
        frozen1: Option<Real>,
        frozen2: Option<Real>,
        end_time: Real,
        body_params: &Coarena<CCDData>,
    ) -> Option<Self> {
        let linvel1 = if frozen1.is_some() {
            na::zero()
        } else {
            b1.linvel
        };

        let linvel2 = if frozen2.is_some() {
            na::zero()
        } else {
            b2.linvel
        };

        let vel12 = linvel2 - linvel1;
        let thickness = c1.shape().ccd_thickness() + c2.shape().ccd_thickness();

        if params.dt() * vel12.norm() < thickness {
            return None;
        }

        let body_params1 = body_params.get(c1.parent)?;
        let body_params2 = body_params.get(c2.parent)?;

        let target_distance = 0.0;

        // Compute the TOI.
        let mut motion1 = body_params1.motion(params.dt(), b1, 0.0);
        let mut motion2 = body_params2.motion(params.dt(), b2, 0.0);

        if let Some(t) = frozen1 {
            let pos = motion1.position_at_time(t);
            motion1 = RigidBodyMotion::Static(pos);
        }

        if let Some(t) = frozen2 {
            let pos = motion2.position_at_time(t);
            motion2 = RigidBodyMotion::Static(pos);
        }

        let toi;
        let mut pos1 = Isometry::identity();
        let mut pos12_at_contact = Isometry::identity();

        if motion1.is_static_or_linear() && motion2.is_static_or_linear() {
            pos1 = motion1.position_at_time(0.0) * c1.position_wrt_parent();
            let pos2 = motion2.position_at_time(0.0) * c2.position_wrt_parent();
            let pos12 = pos1.inv_mul(&pos2);

            let res_toi = query_dispatcher
                .time_of_impact(
                    &pos12,
                    &vel12,
                    c1.shape(),
                    c2.shape(),
                    end_time,
                    target_distance,
                )
                .ok();
            toi = res_toi??;
            let pos1_at_contact = motion1.position_at_time(toi.toi) * c1.position_wrt_parent();
            let pos2_at_contact = motion2.position_at_time(toi.toi) * c2.position_wrt_parent();
            pos12_at_contact = pos1_at_contact.inv_mul(&pos2_at_contact);
        } else {
            let motion1 = motion1.prepend_transformation(*c1.position_wrt_parent());
            let motion2 = motion2.prepend_transformation(*c2.position_wrt_parent());
            let motion12 = motion1.inv_mul(&motion2);

            toi = query_dispatcher
                .nonlinear_time_of_impact(
                    &motion12,
                    c1.shape(),
                    c2.shape(),
                    end_time,
                    target_distance,
                )
                .ok()??;
        }

        let local_vel12 = pos1.inverse_transform_vector(&vel12);

        if toi.toi == 0.0 {
            let res_contact = query_dispatcher
                .contact(
                    &pos12_at_contact,
                    c1.shape(),
                    c2.shape(),
                    params.prediction_distance,
                )
                .ok();
            let contact = res_contact??;

            let dot = contact.normal1.dot(&local_vel12);
            let ccd_threshold = thickness + contact.dist;

            if -dot * params.dt() < ccd_threshold {
                return None;
            }
        }

        let toi = toi.toi;
        Some(Self::new(
            toi,
            ch1,
            c1.parent(),
            ch2,
            c2.parent(),
            is_intersection,
            0,
        ))
    }
}

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

impl Ord for TOIEntry {
    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
        self.partial_cmp(other).unwrap()
    }
}

impl PartialEq for TOIEntry {
    fn eq(&self, other: &Self) -> bool {
        self.toi == other.toi
    }
}

impl Eq for TOIEntry {}
