use fpnum::*;

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

#[derive(PartialEq, Eq, Clone, Copy, Debug)]

pub struct PhysicsData {

    pub position: FPPoint,

    pub velocity: FPPoint,

}

impl GearData for PhysicsData {}

impl PhysicsData {

    pub fn new(position: FPPoint, velocity: FPPoint) -> Self {

        Self { position, velocity }

    }

}

pub struct DynamicPhysicsCollection {

    gear_ids: Vec<GearId>,

    positions: Vec<FPPoint>,

    velocities: Vec<FPPoint>,

}

impl DynamicPhysicsCollection {

    fn new() -> Self {

        Self {

            gear_ids: Vec::new(),

            positions: Vec::new(),

            velocities: Vec::new(),

        }

    }

    fn len(&self) -> usize {

        self.gear_ids.len()

    }

    fn push(&mut self, gear_id: GearId, physics: PhysicsData) {

        self.gear_ids.push(gear_id);

        self.positions.push(physics.position);

        self.velocities.push(physics.velocity);

    }

    fn remove(&mut self, gear_id: GearId) {

        if let Some(index) = self.gear_ids.iter().position(|id| *id == gear_id) {

            self.gear_ids.swap_remove(index);

            self.positions.swap_remove(index);

            self.velocities.swap_remove(index);

        }

    }

    fn iter_pos_update(&mut self) -> impl Iterator<Item = (GearId, (&mut FPPoint, &FPPoint))> {

        self.gear_ids

            .iter()

            .cloned()

            .zip(self.positions.iter_mut().zip(self.velocities.iter()))

    }

}

pub struct StaticPhysicsCollection {

    gear_ids: Vec<GearId>,

    positions: Vec<FPPoint>,

}

impl StaticPhysicsCollection {

    fn new() -> Self {

        Self {

            gear_ids: Vec::new(),

            positions: Vec::new(),

        }

    }

    fn push(&mut self, gear_id: GearId, physics: PhysicsData) {

        self.gear_ids.push(gear_id);

        self.positions.push(physics.position);

    }

    fn remove(&mut self, gear_id: GearId) {

        if let Some(index) = self.gear_ids.iter().position(|id| *id == gear_id) {

            self.gear_ids.swap_remove(index);

            self.positions.swap_remove(index);

        }

    }

}

pub struct PhysicsProcessor {

    dynamic_physics: DynamicPhysicsCollection,

    static_physics: StaticPhysicsCollection,

    physics_cleanup: Vec<GearId>,

    position_updates: PositionUpdates,

}

pub struct PositionUpdates {

    pub gear_ids: Vec<GearId>,

    pub shifts: Vec<(FPPoint, FPPoint)>,

}

impl PositionUpdates {

    pub fn new(capacity: usize) -> Self {

        Self {

            gear_ids: Vec::with_capacity(capacity),

            shifts: Vec::with_capacity(capacity),

        }

    }

    pub fn push(&mut self, gear_id: GearId, old_position: &FPPoint, new_position: &FPPoint) {

        self.gear_ids.push(gear_id);

        self.shifts.push((*old_position, *new_position));

    }

    pub fn iter(&self) -> impl Iterator<Item = (GearId, &FPPoint, &FPPoint)> {

        self.gear_ids

            .iter()

            .cloned()

            .zip(self.shifts.iter())

            .map(|(id, (from, to))| (id, from, to))

    }

    pub fn clear(&mut self) {

        self.gear_ids.clear();

        self.shifts.clear();

    }

}

impl PhysicsProcessor {

    pub fn new() -> Self {

        Self {

            dynamic_physics: DynamicPhysicsCollection::new(),

            static_physics: StaticPhysicsCollection::new(),

            physics_cleanup: Vec::new(),

            position_updates: PositionUpdates::new(0),

        }

    }

        self.position_updates.clear();

        for (gear_id, (pos, vel)) in self.dynamic_physics.iter_pos_update() {

            let old_pos = *pos;

            if !vel.is_zero() {

                self.position_updates.push(gear_id, &old_pos, pos)

            } else {

                self.physics_cleanup.push(gear_id)

            }

        }

        &self.position_updates

    }

    pub fn push(&mut self, gear_id: GearId, physics_data: PhysicsData) {

        if physics_data.velocity.is_zero() {

            self.static_physics.push(gear_id, physics_data);

        } else {

            self.dynamic_physics.push(gear_id, physics_data);

        }

    }

}

impl GearDataProcessor<PhysicsData> for PhysicsProcessor {

    fn add(&mut self, gear_id: GearId, gear_data: PhysicsData) {

        if gear_data.velocity.is_zero() {

            self.static_physics.push(gear_id, gear_data);

        } else {

            self.dynamic_physics.push(gear_id, gear_data);

        }

    }

    fn remove(&mut self, gear_id: GearId) {

        self.static_physics.remove(gear_id);

        self.dynamic_physics.remove(gear_id)

    }

}