#pragma once
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <vector>
#include <cstdint>

// Hand-rolled rigid-body physics — sequential-impulse solver, SAT boxes.
// V1 shapes: Sphere, Box (OBB), Plane (infinite, static-only).

namespace phys {

enum class Shape : uint8_t { Sphere, Box, Plane };

struct Body {
    Shape shape = Shape::Sphere;

    // Sphere: half_extents.x = radius (y, z unused).
    // Box:    half_extents = half-widths along local X/Y/Z.
    // Plane:  plane_normal + plane_offset define n·x = offset.
    glm::vec3 half_extents{0.5f};
    glm::vec3 plane_normal{0.0f, 1.0f, 0.0f};
    float     plane_offset = 0.0f;

    glm::vec3 position{0.0f};
    glm::quat orientation{1.0f, 0.0f, 0.0f, 0.0f};
    glm::vec3 linear_velocity{0.0f};
    glm::vec3 angular_velocity{0.0f};
    glm::vec3 force_accum{0.0f};
    glm::vec3 torque_accum{0.0f};

    float inv_mass = 1.0f;              // 0 => static or kinematic
    glm::mat3 inv_inertia_local{0.0f};  // diag in local space
    glm::mat3 inv_inertia_world{0.0f};  // cached each step

    float restitution     = 0.2f;
    float friction        = 0.5f;
    float linear_damping  = 0.02f;
    float angular_damping = 0.05f;

    bool is_static = false;
    bool sleeping  = false;
    float sleep_timer = 0.0f;

    bool alive  = true;
    int  handle = -1;  // stable handle = index into world.bodies_
};

struct Contact {
    int a = -1;
    int b = -1;
    glm::vec3 point{0.0f};        // world-space contact point
    glm::vec3 normal{0.0f, 1.0f, 0.0f};  // from A toward B
    float penetration = 0.0f;

    // Solver scratch (recomputed each step)
    glm::vec3 rA{0.0f}, rB{0.0f};     // offsets from COM to point
    glm::vec3 tangent1{0.0f}, tangent2{0.0f};
    float normal_mass   = 0.0f;
    float tangent_mass1 = 0.0f;
    float tangent_mass2 = 0.0f;
    float bias = 0.0f;
    float normal_impulse  = 0.0f;
    float tangent_impulse1 = 0.0f;
    float tangent_impulse2 = 0.0f;
};

class World {
public:
    glm::vec3 gravity{0.0f, -9.81f, 0.0f};
    float fixed_dt = 1.0f / 60.0f;
    int solver_iterations = 8;
    bool enabled = false;

    // Water: buoyancy samples the actual Gerstner wave surface each step.
    bool  water_enabled = false;
    float water_level      = 0.0f;
    float water_amplitude  = 0.3f;
    float water_wavelength = 8.0f;
    float water_speed      = 1.0f;
    float water_time       = 0.0f;   // pushed from Engine each frame
    float water_amp_scale  = 1.0f;   // storminess multiplier
    float water_density    = 1.0f;
    float water_drag_linear  = 1.5f;
    float water_drag_angular = 0.8f;

    // Wind-driven surface current (applied to submerged bodies).
    glm::vec2 wind_dir_xz   = glm::vec2(1.0f, 0.0f);
    float wind_speed        = 0.0f;
    float current_coef      = 0.6f;

    // Directional water flow (independent of wind — rivers, waterfalls).
    glm::vec2 water_flow_dir = glm::vec2(0.0f, 0.0f);
    float     water_flow_speed = 0.0f;

    // Gerstner sampler — mirrors the water shader exactly.
    float SampleWaterY(float x, float z) const;

    int  AddBody(const Body& b);
    void RemoveBody(int handle);
    Body* Get(int handle);
    const Body* Get(int handle) const;

    // Drives the fixed-timestep loop. Call each frame with real delta.
    void Step(float real_dt);

    // Iterate live bodies; callback receives handle and body ref.
    template <typename F>
    void ForEach(F&& fn) {
        for (auto& b : bodies_) if (b.alive) fn(b.handle, b);
    }

private:
    std::vector<Body> bodies_;
    std::vector<Contact> contacts_;
    std::vector<Contact> prev_contacts_;  // for warm-starting across frames
    float accumulator_ = 0.0f;

    void IntegrateForces(float dt);
    void DetectCollisions();
    void WarmStart();
    void PrepareSolver(float dt);
    void SolveVelocities();
    void IntegrateVelocities(float dt);
    void UpdateSleep(float dt);

    // Pairwise tests that push contacts into contacts_.
    void TestPair(int ia, int ib);
    void SpherePlane(const Body& s, const Body& p, int is, int ip);
    void SphereSphere(const Body& a, const Body& b, int ia, int ib);
    void SphereBox(const Body& s, const Body& box, int is, int ib);
    void BoxPlane(const Body& box, const Body& p, int ib, int ip);
    void BoxBox(const Body& a, const Body& b, int ia, int ib);
};

}  // namespace phys
