use fpnum::*;
use integral_geometry::{
    Point, Size, GridIndex
};

type Index = u16;

#[derive(PartialEq, Eq, Clone, Copy, Debug)]
struct PhysicsData {
    position: FPPoint,
    velocity: FPPoint,
}

#[derive(PartialEq, Eq, Clone, Copy, Debug)]
struct CollisionData {
    bounds: CircleBounds
}

#[derive(PartialEq, Eq, Clone, Copy, Debug)]
struct ContactData {
    elasticity: FPNum,
    friction: FPNum
}

pub struct PhysicsCollection {
    positions: Vec<FPPoint>,
    velocities: Vec<FPPoint>
}

impl PhysicsCollection {
    fn push(&mut self, data: PhysicsData) {
        self.positions.push(data.position);
        self.velocities.push(data.velocity);
    }

    fn iter_mut_pos(&mut self) -> impl Iterator<Item = (&mut FPPoint, &FPPoint)> {
        self.positions.iter_mut().zip(self.velocities.iter())
    }
}

pub struct JoinedData {
    physics: PhysicsData,
    collision: CollisionData,
    contact: ContactData
}

pub struct World {
    enabled_physics: PhysicsCollection,
    disabled_physics: Vec<PhysicsData>,

    collision: Vec<CollisionData>,
    grid: Grid,

    physics_cleanup: Vec<PhysicsData>,
    collision_output: Vec<(Index, Index)>
}

#[derive(PartialEq, Eq, Clone, Copy, Debug)]
struct CircleBounds {
    center: FPPoint,
    radius: FPNum
}

impl CircleBounds {
    fn intersects(&self, other: &CircleBounds) -> bool {
        (other.center - self.center).is_in_range(self.radius + other.radius)
    }
}

fn fppoint_round(point: &FPPoint) -> Point {
    Point::new(point.x().round() as i32, point.y().round() as i32)
}

struct GridBin {
    refs: Vec<Index>,
    static_entries: Vec<CircleBounds>,
    dynamic_entries: Vec<CircleBounds>
}

impl GridBin {
    fn new() -> Self {
        Self {
            refs: vec![],
            static_entries: vec![],
            dynamic_entries: vec![]
        }
    }
}

const GRID_BIN_SIZE: usize = 256;

struct Grid {
    bins: Vec<GridBin>,
    space_size: Size,
    bins_count: Size,
    index: GridIndex
}

impl Grid {
    fn new(size: Size) -> Self {
        assert!(size.is_power_of_two());
        let bins_count =
            Size::new(size.width / GRID_BIN_SIZE,
                      size.height / GRID_BIN_SIZE);

        Self {
            bins: (0..bins_count.area()).map(|_| GridBin::new()).collect(),
            space_size: size,
            bins_count,
            index: Size::square(GRID_BIN_SIZE).to_grid_index()
        }
    }

    fn bin_index(&self, position: &FPPoint) -> Point {
        self.index.map(fppoint_round(position))
    }

    fn lookup_bin(&mut self, position: &FPPoint) -> &mut GridBin {
        let index = self.bin_index(position);
        &mut self.bins[index.x as usize * self.bins_count.width + index.y as usize]
    }

    fn insert_static(&mut self, index: Index, position: &FPPoint, bounds: &CircleBounds) {
        self.lookup_bin(position).static_entries.push(*bounds)
    }

    fn insert_dynamic(&mut self, index: Index, position: &FPPoint, bounds: &CircleBounds) {
        self.lookup_bin(position).dynamic_entries.push(*bounds)
    }

    fn check_collisions(&self, collisions: &mut Vec<(Index, Index)>) {
        for bin in &self.bins {
            for bounds in &bin.dynamic_entries {
                for other in &bin.dynamic_entries {
                    if bounds.intersects(other) && bounds != other {
                        collisions.push((0, 0))
                    }
                }

                for other in &bin.static_entries {
                    if bounds.intersects(other) {
                        collisions.push((0, 0))
                    }
                }
            }
        }
    }
}

impl World {
    pub fn step(&mut self, time_step: FPNum) {
        for (pos, vel) in self.enabled_physics.iter_mut_pos() {
            *pos += *vel
        }

        self.grid.check_collisions(&mut self.collision_output);
    }

    pub fn add_gear(&mut self, data: JoinedData) {
        if data.physics.velocity == FPPoint::zero() {
            self.disabled_physics.push(data.physics);
            self.grid.insert_static(0, &data.physics.position, &data.collision.bounds);
        } else {
            self.enabled_physics.push(data.physics);
            self.grid.insert_dynamic(0, &data.physics.position, &data.collision.bounds);
        }
    }
}

#[cfg(test)]
mod tests {

}