use raidillon_core::scene::Scene;
use raidillon_ecs::components::RigidBodyComponent;
use raidillon_ecs::Transform;
use raidillon_engine::EngineResources;
use raidillon_engine::system::System;
use raidillon_physics::Physics;
use raidillon_platform::PlatformContext;

/// Do physics calculations and apply to world.
#[derive(Default)]
pub struct PhysicsSystem;

impl System for PhysicsSystem {
    fn load_world(&mut self, res: &mut EngineResources, scene: &mut Scene) {
        let p = Physics::default();
        scene.resources.insert(p);
    }

    fn fixed_update(&mut self, res: &mut EngineResources, scene: &mut Scene) {
        let pctx = res.get::<PlatformContext>().expect("PlatformContext missing").clone();
        let physics = scene.resources.get_mut::<Physics>().expect("Physics missing");
        physics.step(pctx.time_ctx.fixed_dt);

        // apply calculations to dynamic bodies
        let mut query = scene.world.query::<(&mut Transform, &RigidBodyComponent)>();
        for (_ent, (tr, rb_component)) in query.iter() {
            if let Some(body) = physics.get_rigid_body(rb_component.0) {
                let pos = body.position();
                let translation = Physics::rapier_translation_to_glam(&pos.translation.vector);
                let rotation = Physics::rapier_rotation_to_glam(&pos.rotation);
                tr.translation = translation;
                tr.rotation = rotation;
            }
        }
    }
}