use na;
use na::Translate;
use math::{Scalar, Point, Vect, Isometry};
use utils::data::hash_map::HashMap;
use utils::data::hash::UintTWHash;
use entities::bounding_volume::{self, BoundingVolume};
use entities::partitioning::BoundingVolumeInterferencesCollector;
use entities::shape::CompositeShape;
use entities::inspection::Repr;
use entities::inspection;
use queries::geometry::Contact;
use narrow_phase::{CollisionDetector, CollisionDispatcher, CollisionAlgorithm};


/// Collision detector between a concave shape and another shape.
pub struct CompositeShapeRepr<P: Point, M> {
    prediction:    <P::Vect as Vect>::Scalar,
    sub_detectors: HashMap<usize, CollisionAlgorithm<P, M>, UintTWHash>,
    to_delete:     Vec<usize>,
    interferences: Vec<usize>
}

impl<P: Point, M> CompositeShapeRepr<P, M> {
    /// Creates a new collision detector between a concave shape and another shape.
    pub fn new(prediction: <P::Vect as Vect>::Scalar) -> CompositeShapeRepr<P, M> {
        CompositeShapeRepr {
            prediction:    prediction,
            sub_detectors: HashMap::new_with_capacity(5, UintTWHash::new()),
            to_delete:     Vec::new(),
            interferences: Vec::new()
        }
    }
}

impl<P, M> CompositeShapeRepr<P, M>
    where P:  Point,
          P::Vect: Translate<P>,
          M: Isometry<P, P::Vect> {
    fn do_update(&mut self,
                 dispatcher: &CollisionDispatcher<P, M>,
                 m1:         &M,
                 g1:         &CompositeShape<P, M>,
                 m2:         &M,
                 g2:         &Repr<P, M>,
                 swap:       bool) {
        // Find new collisions
        let ls_m2    = na::inv(m1).expect("The transformation `m1` must be inversible.") * *m2;
        let ls_aabb2 = bounding_volume::aabb(g2, &ls_m2).loosened(self.prediction);

        {
            let mut visitor = BoundingVolumeInterferencesCollector::new(&ls_aabb2, &mut self.interferences);
            g1.bvt().visit(&mut visitor);
        }

        for i in self.interferences.iter() {
            let mut detector = None;

            g1.map_part_at(*i, &mut |_, g1| {
                let r1 = g1.repr();
                let r2 = g2.repr();

                if swap {
                    detector = dispatcher.get_collision_algorithm(&r2, &r1)
                }
                else {
                    detector = dispatcher.get_collision_algorithm(&r1, &r2)
                }
            });

            match detector {
                Some(detector) => {
                    let _ = self.sub_detectors.insert_or_replace(*i, detector, false);
                },
                None => { }
            }
        }

        self.interferences.clear();

        // Update all collisions
        for detector in self.sub_detectors.elements_mut().iter_mut() {
            let key = detector.key;
            if ls_aabb2.intersects(g1.aabb_at(key)) {
                g1.map_transformed_part_at(m1, key, &mut |m1, g1| {
                    if swap {
                        assert!(detector.value.update(dispatcher, m2, g2, m1, g1), "The shape was no longer valid.");
                    }
                    else {
                        assert!(detector.value.update(dispatcher, m1, g1, m2, g2), "The shape was nolonger valid.");
                    }
                });
            }
            else 

        }

        // Remove outdated sub detectors
        for i in self.to_delete.iter() {
            self.sub_detectors.remove(i);
        }

        self.to_delete.clear();
    }
}

/// Collision detector between a shape and a concave shape.
pub struct ReprCompositeShape<P: Point, M> {
    sub_detector: CompositeShapeRepr<P, M>
}

impl<P: Point, M> ReprCompositeShape<P, M> {
    /// Creates a new collision detector between a shape and a concave shape.
    pub fn new(prediction: <P::Vect as Vect>::Scalar) -> ReprCompositeShape<P, M> {
        ReprCompositeShape {
            sub_detector: CompositeShapeRepr::new(prediction)
        }
    }
}

impl<P, M> CollisionDetector<P, M> for CompositeShapeRepr<P, M>
    where P: Point,
          P::Vect: Translate<P>,
          M: Isometry<P, P::Vect> {
    fn update(&mut self,
              d:  &CollisionDispatcher<P, M>,
              ma: &M,
              a:  &Repr<P, M>,
              mb: &M,
              b:  &Repr<P, M>)
              -> bool {
        if let Some(cs) = inspection::maybe_as_composite_shape(a) {
            self.do_update(d, ma, cs, mb, b, false);

            true
        }
        else {
            false
        }
    }

    fn num_colls(&self) -> usize {
        let mut res = 0;

        for detector in self.sub_detectors.elements().iter() {
            res = res + detector.value.num_colls()
        }

        res
    }

    fn colls(&self, out: &mut Vec<Contact<P>>) {
        for detector in self.sub_detectors.elements().iter() {
            detector.value.colls(out);
        }
    }
}

impl<P, M> CollisionDetector<P, M> for ReprCompositeShape<P, M>
    where P: Point,
          P::Vect: Translate<P>,
          M: Isometry<P, P::Vect> {
    fn update(&mut self,
              d:  &CollisionDispatcher<P, M>,
              ma: &M,
              a:  &Repr<P, M>,
              mb: &M,
              b:  &Repr<P, M>)
              -> bool {
        if let Some(cs) = inspection::maybe_as_composite_shape(b) {
            self.sub_detector.do_update(d, mb, cs, ma, a, true);

            true
        }
        else {
            false
        }
    }

    fn num_colls(&self) -> usize {
        self.sub_detector.num_colls()
    }

    fn colls(&self, out: &mut Vec<Contact<P>>) {
        self.sub_detector.colls(out)
    }
}

{
                // FIXME: ask the detector if it wants to be removed or not
                self.to_delete.push(key);
            }