/** 3d rigid body */

module re.phys.rigid3d;

version (physics) {
    import std.math;
    import std.format;
    import std.typecons;
    import std.container.array;

    import re.ecs.component;
    import re.ecs.updatable;
    import re.math;
    import re.time;
    import re.core;
    import re.ng.manager;
    import re.ng.scene;
    import re.phys.collider;
    import re.util.dual_map;
    import re.util.newtonphys;
    import re.phys.newton3d;
    import newton = bindbc.newton;

    /// represents a manager for physics bodies
    class PhysicsManager3D : Manager {
        /// the maximum number of collisions to support in this world
        public int max_collisions = 1024;
        /// position correction iterations used by physics engine
        public int pos_correction_iterations = 20;
        /// velocity correction iterations used by physics engine
        public int vel_correction_iterations = 40;
        /// gravity in the physics world
        public Vector3 gravity = Vector3(0.0f, -9.80665f, 0.0f); // earth's gravity

        /// the internal newton physics world
        private newton.NewtonWorld* world;

        /// some cursed dlib stuff
        private NewtonWorldManager manager;

        /// physics target timestep
        private float _timestep;
        /// used to track time to keep physics timestep fixed
        private float _phys_time = 0;

        private PhysicsBody3D[NewtonRigidBody] _bodies;

        /// the total number of bodies in this physics world
        @property public size_t body_count() {
            return newton.NewtonWorldGetBodyCount(world);
        }

        override void setup() {
            world = newton.NewtonCreate();
            manager = new NewtonWorldManager(world);

            // set to iterative (1) mode https://newtondynamics.com/wiki/index.php/NewtonSetSolverIterations
            newton.NewtonSetSolverIterations(world, 1);

            // world.gravity = convert_vec3(gravity);
            // _timestep = 1f / Core.target_fps; // set target _timestep
        }

        override void destroy() {
            newton.NewtonDestroyAllBodies(world);
            newton.NewtonDestroy(world);
        }

        override void update() {
            // step the simulation
            _phys_time += Time.delta_time;
            // TODO: should this be while?
            if (_phys_time >= _timestep) {
                _phys_time -= _timestep;

                // // sync FROM bodies: physical properties (mass, inertia)
                // // sync TO bodies: transforms, momentum
                // foreach (comp; _bodies.byValue()) {
                //     NewtonRigidBody bod = comp._phys_body;

                //     // sync properties -> physics engine
                //     if (abs(bod.mass - comp.mass) > float.epsilon) {
                //         bod.mass = comp.mass;
                //         bod.invMass = 1f / comp.mass;
                //         // TODO: update mass contribtion of shapes?
                //         // foreach (shape; body_comp._phys_shapes) {

                //         // }
                //     }
                //     // TODO: sync inertia? right now it's automatically set from mass

                //     // sync velocity
                //     bod.linearVelocity = convert_vec3(comp.velocity);
                //     bod.angularVelocity = convert_vec3(comp.angular_velocity);
                //     bod.maxSpeed = comp.max_speed;

                //     // apply forces and impulses
                //     // these are queued up, then we apply them all to the object
                //     foreach (force; comp._forces) {
                //         bod.applyForceAtPos(convert_vec3(force.value),
                //                 convert_vec3(force.pos) + bod.position);
                //     }
                //     comp._forces.removeBack(cast(uint) comp._forces.length);
                //     foreach (impulse; comp._impulses) {
                //         bod.applyImpulse(convert_vec3(impulse.value),
                //                 convert_vec3(impulse.pos) + bod.position);
                //     }
                //     comp._impulses.removeBack(cast(uint) comp._impulses.length);
                //     foreach (torque; comp._torques) {
                //         bod.applyTorque(convert_vec3(torque));
                //     }
                //     comp._torques.removeBack(cast(uint) comp._torques.length);
                // }

                // // sync options to world
                // world.gravity = convert_vec3(gravity);

                // world.update(_timestep);
                newton.NewtonUpdate(world, _timestep);

                // foreach (comp; _bodies.byValue()) {
                //     NewtonRigidBody bod = comp._phys_body;

                //     // sync physics engine -> components

                //     // sync position
                //     auto bod_pos = bod.position;
                //     comp.transform.position = convert_vec3(bod_pos);

                //     // sync rotation/orientation
                //     auto bod_rot = bod.orientation;
                //     comp.transform.orientation = convert_quat(bod_rot);

                //     // sync velocity
                //     comp.velocity = convert_vec3(bod.linearVelocity);
                //     comp.angular_velocity = convert_vec3(bod.angularVelocity);
                // }
            }
        }

        /// register all colliders in this body
        private void register_colliders(PhysicsBody3D body_comp) {
            // add colliders to the physics world
            auto bod = body_comp._phys_body;

            auto box_colliders = body_comp.entity.get_components!BoxCollider();

            foreach (box; box_colliders) {
                auto box_size = box.size;
                auto box_origin = box.offset + box.size * 0.5;

                // auto offset_mat = newton_matrix();
                // newton.GetId
                // offset_mat.posit = box_origin;
                // newton.NewtonCreateBox

                // auto box_geom = New!(geom.GeomBox)(world, convert_vec3(box.size));
                // auto shape = world.addShapeComponent(bod, box_geom,
                //         convert_vec3(box.offset), bod.mass);
                // body_comp._shapes[shape] = box;
            }
        }

        /// unregister all colliders in this body
        private void unregister_colliders(PhysicsBody3D body_comp) {
            // import std.range : front;
            // import std.algorithm : countUntil, remove;

            // // we need to remove from the world each collider that we internally have registered to that body
            // foreach (shape; body_comp._shapes.byKey()) {
            //     world.shapeComponents.removeFirst(shape);
            // }

            // // then clear the internal collider list
            // body_comp._shapes.clear();
        }

        // private NewtonRigidBody createDynamicBody(NewtonCollisionShape shape, float mass) {
        //     NewtonRigidBody b = New!NewtonRigidBody(shape, mass, this, this);
        //     b.dynamic = true;
        //     // TODO: store a list of bodies
        //     return b;
        // }

        // private NewtonRigidBody createStaticBody(NewtonCollisionShape shape) {
        //     auto b = createDynamicBody(shape, 0.0f);
        //     b.dynamic = false;
        //     return b;
        // }

        /// registers a body
        private void register(PhysicsBody3D body_comp) {
            // NewtonRigidBody bod;
            // switch (body_comp._body_type) {
            // case PhysicsBody3D.BodyType.Static:
            //     bod = world.addStaticBody(convert_vec3(body_comp.transform.position));
            //     break;
            // case PhysicsBody3D.BodyType.Dynamic:
            //     bod = world.addDynamicBody(convert_vec3(body_comp.transform.position),
            //         body_comp.mass);
            //     break;
            // default:
            //     assert(0);
            // }

            // bod.rotation = convert_quat(body_comp.transform.orientation);

            // // update registration
            // body_comp._phys_body = bod;
            // _bodies[bod] = body_comp;

            // // add colliders
            // register_colliders(body_comp);

            // // sync properties
            // sync_properties(body_comp);

            // // mark as synced
            // body_comp.physics_synced = true;
        }

        /// unregisters a body
        private void unregister(PhysicsBody3D body_comp) {
            // // remove colliders
            // unregister_colliders(body_comp);

            // auto bod = body_comp._phys_body;

            // // remove body
            // switch (body_comp._body_type) {
            // case PhysicsBody3D.BodyType.Static:
            //     world.staticBodies.removeFirst(bod);
            //     break;
            // case PhysicsBody3D.BodyType.Dynamic:
            //     world.dynamicBodies.removeFirst(bod);
            //     break;
            // default:
            //     assert(0);
            // }

            // // mark as unsynced
            // body_comp.physics_synced = false;

            // // clear registration
            // _bodies.remove(bod);
            // body_comp._phys_body = null;
        }

        /// sync a body's colliders in the physics engine, necessary when shapes change
        private void sync_colliders(PhysicsBody3D body_comp) {
            unregister_colliders(body_comp);
            register_colliders(body_comp);
        }

        /// synchronize the transform from body to physics engine
        private void sync_transform(PhysicsBody3D body_comp) {
            // sync position
            body_comp._phys_body.position = convert_vec3(body_comp.transform.position);

            // sync rotation
            body_comp._phys_body.rotation = convert_quat(body_comp.transform.orientation);
        }

        /// sync physical properties from body to physics engine
        private void sync_properties(PhysicsBody3D body_comp) {
            auto bod = body_comp._phys_body;
            // if (body_comp.custom_gravity) {
            //     bod.useOwnGravity = true;
            //     bod.gravity = convert_vec3(body_comp.gravity);
            // }
            // bod.damping = body_comp.damping;
            // bod.bounce = body_comp.bounce;
            // bod.friction = body_comp.friction;
        }

        /// cast a ray of a given length in a given direction and return the result. null if no hits.
        public Nullable!RaycastResult raycast(Vector3 start, Vector3 direction, float dist) {
            // import std.algorithm.searching : countUntil;

            // dm_ray.CastResult cr;
            // if (world.raycast(convert_vec3(start), convert_vec3(direction), dist, cr, true, true)) {
            //     // get matching physics body
            //     auto body_comp = _bodies[cr.rbody];
            //     auto res = RaycastResult(convert_vec3(cr.point), convert_vec3(cr.normal), body_comp);
            //     // get matching collider
            //     if (cr.shape !is null) {
            //         auto col = body_comp._shapes[cr.shape];
            //         res.collider = Nullable!Collider(col);
            //     }
            //     return Nullable!RaycastResult(res);
            // }
            // no result
            return Nullable!RaycastResult.init;
        }

        /// raycast from a physics body in a certain direction, excluding the source body
        public Nullable!RaycastResult raycast_from(PhysicsBody3D source,
            Vector3 offset, Vector3 direction, float dist) {
            // we want to exclude this body, so we're temporarily going to disable it
            source._phys_body.raycastable = false;
            auto res = raycast(source.transform.position + offset, direction, dist);
            source._phys_body.raycastable = true;
            return res;
        }
    }

    public struct RaycastResult {
        Vector3 point;
        Vector3 normal;
        PhysicsBody3D pbody;
        Nullable!Collider collider;
    }

    /// represents a physics body
    abstract class PhysicsBody3D : Component {
        mixin Reflect;
        // - references to things in the physics engine
        private NewtonRigidBody _phys_body;
        // private newton.NewtonCollision*[] _phys_shapes;
        private Collider[newton.NewtonCollision* ] _shapes;

        // - physical properties
        /// object mass
        public float mass = 0;
        /// moment of inertia
        // public float inertia = 1;
        /// max speed of object
        public float max_speed = float.max;
        /// current linear velocity of object
        public Vector3 velocity = Vector3(0, 0, 0);
        /// current angular velocity of object
        public Vector3 angular_velocity = Vector3(0, 0, 0);
        /// damping amount
        public float damping = 0.5;
        /// bounce amount
        public float bounce = 0;
        /// coefficient of friction
        public float friction = 0.5;
        /// whether to use a custom gravity value
        public bool custom_gravity = false;
        /// if custom gravity is enabled, the gravity to use
        public Vector3 gravity = Vector3(0, 0, 0);

        /// whether this body is currently in sync with the physics system
        public bool physics_synced = false;

        private PhysicsManager3D _mgr;
        private BodyType _body_type;

        /// physics body mode: dynamic or static
        public enum BodyType {
            Dynamic,
            Static
        }

        // - used to queue forces and impulses to be applied by the physics engine
        private Array!VecAtPoint _forces;
        private Array!Vector3 _torques;
        private Array!VecAtPoint _impulses;

        private struct VecAtPoint {
            Vector3 value;
            Vector3 pos;
        }

        /// creates a physics body with a given mass and type
        this(float mass, BodyType type) {
            this.mass = mass;
            _body_type = type;
        }

        override void setup() {
            // register the body in the physics manager
            auto mgr = entity.scene.get_manager!PhysicsManager3D();
            assert(!mgr.isNull, "scene did not have PhysicsManager3D registered."
                    ~ "please add that to the scene before creating this component.");
            _mgr = mgr.get;
            _mgr.register(this);
        }

        /// apply an impulse to the physics body
        public void apply_impulse(Vector3 value, Vector3 pos) {
            _impulses.insertBack(VecAtPoint(value, pos));
        }

        /// apply a force to the physics body
        public void apply_force(Vector3 value, Vector3 pos) {
            _forces.insertBack(VecAtPoint(value, pos));
        }

        /// apply a torque to the physics body
        public void apply_torque(Vector3 value) {
            _torques.insertBack(value);
        }

        /// notify physics engine about new physical properties, such as gravity
        public void sync_properties() {
            _mgr.sync_properties(this);
        }

        /// used to notify the physics engine to update colliders if they have changed
        public void sync_colliders() {
            _mgr.sync_colliders(this);
        }

        /// used to notify the physics engine when transform is directly modified
        public void sync_transform() {
            _mgr.sync_transform(this);
        }

        override void destroy() {
            _mgr.unregister(this);
        }
    }

    /// a dynamic physics body that is affected by forces
    public class DynamicBody : PhysicsBody3D {
        mixin Reflect;
        this(float mass = 1f) {
            super(mass, PhysicsBody3D.BodyType.Dynamic);
        }
    }

    /// a static physics body that is not affected by forces
    public class StaticBody : PhysicsBody3D {
        mixin Reflect;
        this(float mass = 1f) {
            super(mass, PhysicsBody3D.BodyType.Static);
        }
    }

    @("phys-rigid3d-basic") unittest {
        import re.ng.scene : Scene2D;
        import re.util.test : test_scene;

        class TestScene : Scene2D {
            override void on_start() {
                add_manager(new PhysicsManager3D());

                auto nt = create_entity("block");
                nt.add_component(new DynamicBody());
            }
        }

        auto test = test_scene(new TestScene());
        test.game.run();

        // check conditions

        test.game.destroy();
    }

    @("phys-rigid3d-lifecycle") unittest {
        import re.ng.scene : Scene2D;
        import re.util.test : test_scene;
        import re.ecs.entity : Entity;

        class TestScene : Scene2D {
            private Entity nt1;

            override void on_start() {
                add_manager(new PhysicsManager3D());

                nt1 = create_entity("one");
                nt1.add_component(new DynamicBody());
                auto nt2 = create_entity("two");
                nt2.add_component(new DynamicBody());
                auto nt3 = create_entity("three");
                nt3.add_component(new DynamicBody());
            }

            public void kill_one() {
                nt1.destroy();
            }
        }

        auto test = test_scene(new TestScene());
        test.game.run();

        // check conditions
        auto mgr = test.scene.get_manager!PhysicsManager3D;
        assert(!mgr.isNull);
        assert(mgr.get.body_count == 3, "physics body count does not match");

        (cast(TestScene) test.scene).kill_one();
        assert(mgr.get.body_count == 2, "physics body was not unregistered on component destroy");

        test.game.destroy();
    }

    @("phys-rigid3d-colliders") unittest {
        import re.ng.scene : Scene2D;
        import re.util.test : test_scene;
        import re.ecs.entity : Entity;
        import std.algorithm : canFind;

        class TestScene : Scene2D {
            private Entity nt1;

            override void on_start() {
                add_manager(new PhysicsManager3D());

                nt1 = create_entity("block");
                nt1.add_component(new BoxCollider(Vector3(1, 1, 1), Vector3Zero));
                nt1.add_component(new DynamicBody());
            }

            /// inform the physics body that the colliders need to be synced
            public void reload_colliders() {
                nt1.get_component!DynamicBody().sync_colliders();
            }

            /// remove the existing box collider, and replace with a new one, then reload
            public void replace_colliders() {
                nt1.remove_component!BoxCollider();
                nt1.add_component(new BoxCollider(Vector3(2, 2, 2), Vector3Zero));
                reload_colliders();
            }
        }

        auto test = test_scene(new TestScene());
        test.game.run();

        // check conditions
        auto scn = cast(TestScene) test.scene;
        auto mgr = test.scene.get_manager!PhysicsManager3D.get;
        auto bod = test.scene.get_entity("block").get_component!DynamicBody();

        // check that colliders are registered
        assert(mgr.world.dynamicBodies.data.canFind(bod._phys_body));
        auto shape1 = bod._shapes.keys[0];
        immutable auto collider1_size_x = (cast(geom.GeomBox)(shape1.geometry)).halfSize.x;
        assert(collider1_size_x == 1,
            "collider #1 size from physics engine does not match provided collider size");

        // sync the colliders, then ensure that the registration is different
        scn.reload_colliders();
        auto shape2 = bod._shapes.keys[0];
        assert(shape1 != shape2,
            "colliders were synced, which was supposed to reset collider registration, but entry was not changed");

        // replace the colliders
        scn.replace_colliders();
        assert(bod._shapes.length > 0, "registration entry for new collider missing");
        auto shape3 = bod._shapes.keys[0];
        immutable auto collider3_size_x = (cast(geom.GeomBox)(shape3.geometry)).halfSize.x;
        assert(collider3_size_x == 2,
            "collider #3 size from physics engine does not match replaced collider size");

        test.game.destroy();
    }
}