use crate::com::{Blocking, Collision, Velocity, Position};
use crate::types::{NPhase,BPhase};
use crate::game::MyGame;

use nalgebra::{Isometry2, Vector2};
use ncollide2d::broad_phase::{
    broad_phase::BroadPhase,
    broad_phase::BroadPhaseProxyHandle,
    BroadPhaseInterferenceHandler,
};
use specs::{Join,RunNow};

struct InterferenceHandler<'a> {
    collisions: specs::WriteStorage<'a, Collision>,
}

impl<'a> BroadPhaseInterferenceHandler<specs::Entity> for InterferenceHandler<'a> {
    fn is_interference_allowed(&mut self, a: &specs::Entity, b: &specs::Entity) -> bool {
        // Prevent self-collision.
        *a != *b
    }

    fn interference_started(&mut self, a: &specs::Entity, b: &specs::Entity) {
        self.collisions.get_mut(*a).map (|r| r.has_collision = true );
        self.collisions.get_mut(*b).map (|r| r.has_collision = true );
    }

    fn interference_stopped(&mut self, _: &specs::Entity, _: &specs::Entity) {
    }
}


struct Collide;

impl<'a> specs::System<'a> for Collide {
    type SystemData = (
        specs::Entities<'a>,
        specs::ReadStorage<'a, Position>,
        specs::ReadStorage<'a, Velocity>,
        specs::ReadStorage<'a, Blocking>,
        specs::WriteStorage<'a, Collision>,
        specs::WriteExpect<'a, BPhase>,
        specs::WriteExpect<'a, NPhase>,
    );

    fn run(&mut self, (entities, position, velocity, blocking, mut collisions, mut bf, _narrow): Self::SystemData) {
        let _: Vec<()> = (&mut collisions).join().map( |c| c.has_collision = false ).collect();
        let handles: Vec<BroadPhaseProxyHandle> =
            (&entities, &position, &blocking).join().map( |(e, pos, bl)| {
                let np = if let Some(vel) = velocity.get(e) {
                    pos.moved(vel)
                } else {
                    pos.clone()
                };
                bf.create_proxy(
                    { let v = bl.volume.aabb(&Isometry2::new(Vector2::new(np.x, np.y), nalgebra::zero()));
                      println!("{:?}", v);
                      v
                    },
                    e,
                )
        }).collect();

        bf.update(&mut InterferenceHandler {
            collisions: collisions,
        });

        bf.remove(&handles, &mut |_, _| {});
    }
}

struct Intersection;

impl<'a> specs::System<'a> for Intersection {
    type SystemData = (
        specs::Entities<'a>,
        specs::ReadStorage<'a, Position>,
        specs::ReadStorage<'a, Velocity>,
        specs::ReadStorage<'a, Blocking>,
        specs::WriteStorage<'a, Collision>,
    );

    fn run(&mut self, (entity, position, velocity, blocking, mut collision): Self::SystemData) {
        let mut spacemap = std::collections::HashMap::new();
        for (e, pos, _) in (&entity, &position, &blocking).join() {
            spacemap.insert(pos.to_grid(), e);
        }

        for (pos, vel, col) in (&position, &velocity, &mut collision).join() {
            if let Some(_) = spacemap.get(&pos.moved(vel).to_grid()) {
                col.has_collision = true;
            }
        }
    }
}

struct Physics;

impl <'a> specs::System<'a> for Physics {
    type SystemData = (
        specs::ReadStorage<'a, Velocity>,
        specs::ReadStorage<'a, Collision>,
        specs::WriteStorage<'a, Position>,
    );

    fn run(&mut self, (velocity, collision, mut position): Self::SystemData) {
        for (vel, col, pos) in (&velocity, &collision, &mut position).join() {
            if !col.has_collision {
                pos.x += vel.dx;
                pos.y += vel.dy;
            }
        }
    }
}

struct ResetCollision;

impl<'a> specs::System<'a> for ResetCollision {
    type SystemData =
        specs::WriteStorage<'a, Collision>;

    fn run(&mut self, mut collision: Self::SystemData) {
        for mut e in (&mut collision).join() {
            e.has_collision = false;
        }
    }
}

pub fn systems(game: &mut MyGame) {
    Collide.run_now(&game.world);
    // Intersection.run_now(&game.world.res);
    Physics.run_now(&game.world);
    ResetCollision.run_now(&game.world);
}