use crate::common::{GearData, GearDataLookup, GearDataProcessor, GearId, Millis};
use fpnum::*;

#[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) -> u16 {
        self.gear_ids.push(gear_id);
        self.positions.push(physics.position);
        self.velocities.push(physics.velocity);

        (self.gear_ids.len() - 1) as u16
    }

    fn remove(&mut self, index: usize) -> Option<GearId> {
        self.gear_ids.swap_remove(index);
        self.positions.swap_remove(index);
        self.velocities.swap_remove(index);

        self.gear_ids.get(index).cloned()
    }

    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) -> u16 {
        self.gear_ids.push(gear_id);
        self.positions.push(physics.position);

        (self.gear_ids.len() - 1) as u16
    }

    fn remove(&mut self, index: usize) -> Option<GearId> {
        self.gear_ids.swap_remove(index);
        self.positions.swap_remove(index);

        self.gear_ids.get(index).cloned()
    }
}

pub struct PhysicsProcessor {
    gear_lookup: GearDataLookup<bool>,
    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 {
            gear_lookup: GearDataLookup::new(),
            dynamic_physics: DynamicPhysicsCollection::new(),
            static_physics: StaticPhysicsCollection::new(),
            physics_cleanup: Vec::new(),
            position_updates: PositionUpdates::new(0),
        }
    }

    pub fn process(&mut self, time_step: Millis) -> &PositionUpdates {
        let fp_step = time_step.to_fixed();
        self.position_updates.clear();
        for (gear_id, (pos, vel)) in self.dynamic_physics.iter_pos_update() {
            let old_pos = *pos;
            *pos += *vel * fp_step;
            if !vel.is_zero() {
                self.position_updates.push(gear_id, &old_pos, pos)
            } else {
                self.physics_cleanup.push(gear_id)
            }
        }
        &self.position_updates
    }
}

impl GearDataProcessor<PhysicsData> for PhysicsProcessor {
    fn add(&mut self, gear_id: GearId, gear_data: PhysicsData) {
        let is_dynamic = !gear_data.velocity.is_zero();
        let index = if is_dynamic {
            self.dynamic_physics.push(gear_id, gear_data)
        } else {
            self.static_physics.push(gear_id, gear_data)
        };

        self.gear_lookup.add(gear_id, index, is_dynamic);
    }

    fn remove(&mut self, gear_id: GearId) {
        if let Some(entry) = self.gear_lookup.get(gear_id) {
            let relocated_gear_id = if *entry.value() {
                self.dynamic_physics.remove(entry.index() as usize)
            } else {
                self.static_physics.remove(entry.index() as usize)
            };

            if let Some(id) = relocated_gear_id {
                let index = entry.index();
                self.gear_lookup[id].set_index(index);
            }
        }
    }

    fn get(&mut self, gear_id: GearId) -> Option<PhysicsData> {
        if let Some(entry) = self.gear_lookup.get(gear_id) {
            let data = if *entry.value() {
                PhysicsData {
                    position: self.dynamic_physics.positions[entry.index() as usize],
                    velocity: self.dynamic_physics.velocities[entry.index() as usize],
                }
            } else {
                PhysicsData {
                    position: self.static_physics.positions[entry.index() as usize],
                    velocity: FPPoint::zero(),
                }
            };
            Some(data)
        } else {
            None
        }
    }
}