extern crate nalgebra as na;
extern crate ncollide;

use na::{Isometry2, Point2, Vector2};
use ncollide::shape::Cuboid;
use ncollide::query::ContactPrediction;
use ncollide::narrow_phase::{ContactDispatcher, DefaultContactDispatcher};

fn main() {
    let cuboid_a = Cuboid::new(Vector2::new(10.0, 10.0));
    let cuboid_b = Cuboid::new(Vector2::new(300.0, 1.5));

    let pos_b = Isometry2::new(Vector2::new(5.0, 0.0), 1.5);

    let dispatcher = DefaultContactDispatcher::<Point2<f32>, Isometry2<f32>>::new();
    let mut algorithm = dispatcher.get_contact_algorithm(&cuboid_a, &cuboid_b).unwrap();

    let prediction = ContactPrediction::new(0.0, 0.0, 0.0);

    let mut p = Vector2::new(0.0, 0.0);
    let mut angle = 0.0;

    for i in 1..1000000 {
        p.x += 0.0001;
        angle += 0.005;

        let pos_a = Isometry2::new(p, angle);

        algorithm.update(&dispatcher, &pos_a, &cuboid_a, &pos_b, &cuboid_b, &prediction);

        let mut contacts = Vec::new();
        algorithm.contacts(&mut contacts);

        for contact in contacts.iter() {
            p -= contact.normal.unwrap() * contact.depth;
        }

        //println!("{}", i);
    }
}