#include "physics.h"
#include "../include/engine_api.h"

#include <stdlib.h>
#include <math.h>
#include <float.h>

static void move_collider(Collider* c, Vec3 delta);

PhysicsWorld* physics_create(void) {
    PhysicsWorld* w = (PhysicsWorld*)calloc(1, sizeof(PhysicsWorld));
    w->gravity_enabled = 1;
    w->gravity = (Vec3){0, -9.81f, 0};
    return w;
}

void physics_destroy(PhysicsWorld* world) {
    free(world);
}

static void init_body(RigidBody* b) {
    b->velocity = (Vec3){0, 0, 0};
    b->acceleration = (Vec3){0, 0, 0};
    b->mass = 1.0f;
    b->restitution = 0.5f;
    b->friction = 0.3f;
    b->is_kinematic = 0;
}

uint32_t physics_add_aabb(PhysicsWorld* world, uint32_t entity, Vec3 min, Vec3 max) {
    uint32_t idx = world->collider_count++;
    Collider* c = &world->colliders[idx];
    c->entity_id = entity;
    c->type = COLLIDER_AABB;
    c->aabb.min = min;
    c->aabb.max = max;
    c->is_trigger = 0;
    c->is_static = 0;
    init_body(&c->body);
    return idx;
}

uint32_t physics_add_sphere(PhysicsWorld* world, uint32_t entity, Vec3 center, float radius) {
    uint32_t idx = world->collider_count++;
    Collider* c = &world->colliders[idx];
    c->entity_id = entity;
    c->type = COLLIDER_SPHERE;
    c->sphere.center = center;
    c->sphere.radius = radius;
    c->is_trigger = 0;
    c->is_static = 0;
    init_body(&c->body);
    return idx;
}

uint32_t physics_add_plane(PhysicsWorld* world, uint32_t entity, Vec3 normal, float distance) {
    uint32_t idx = world->collider_count++;
    Collider* c = &world->colliders[idx];
    c->entity_id = entity;
    c->type = COLLIDER_PLANE;
    vec3_normalize(&normal, &c->plane.normal);
    c->plane.distance = distance;
    c->is_trigger = 0;
    c->is_static = 1;
    init_body(&c->body);
    c->body.mass = 0.0f;
    c->body.is_kinematic = 1;
    return idx;
}

void physics_remove_collider(PhysicsWorld* world, uint32_t index) {
    if (index < world->collider_count) {
        world->colliders[index] = world->colliders[--world->collider_count];
    }
}

void physics_update_collider(PhysicsWorld* world, uint32_t index, Vec3 position) {
    if (!world || index >= world->collider_count) return;
    Collider* c = &world->colliders[index];
    if (c->type == COLLIDER_SPHERE) {
        c->sphere.center = position;
    } else if (c->type == COLLIDER_AABB) {
        Vec3 center = {
            (c->aabb.min.x + c->aabb.max.x) * 0.5f,
            (c->aabb.min.y + c->aabb.max.y) * 0.5f,
            (c->aabb.min.z + c->aabb.max.z) * 0.5f
        };
        Vec3 delta;
        vec3_sub(&position, &center, &delta);
        move_collider(c, delta);
    }
}

static int aabb_vs_aabb(const Collider* a, const Collider* b, Vec3* normal, float* depth) {
    Vec3 diff;
    Vec3 centerA = {(a->aabb.min.x + a->aabb.max.x) * 0.5f, (a->aabb.min.y + a->aabb.max.y) * 0.5f, (a->aabb.min.z + a->aabb.max.z) * 0.5f};
    Vec3 centerB = {(b->aabb.min.x + b->aabb.max.x) * 0.5f, (b->aabb.min.y + b->aabb.max.y) * 0.5f, (b->aabb.min.z + b->aabb.max.z) * 0.5f};
    vec3_sub(&centerA, &centerB, &diff);
    Vec3 half_a, half_b;
    vec3_sub(&a->aabb.max, &a->aabb.min, &half_a);
    vec3_scale(&half_a, 0.5f, &half_a);
    vec3_sub(&b->aabb.max, &b->aabb.min, &half_b);
    vec3_scale(&half_b, 0.5f, &half_b);

    float dx = fabsf(diff.x) - (half_a.x + half_b.x);
    float dy = fabsf(diff.y) - (half_a.y + half_b.y);
    float dz = fabsf(diff.z) - (half_a.z + half_b.z);

    if (dx < 0 && dy < 0 && dz < 0) {
        *depth = -fmaxf(fmaxf(dx, dy), dz);
        if (dx > dy && dx > dz) { *normal = (Vec3){copysignf(1.0f, diff.x), 0, 0}; }
        else if (dy > dz) { normal->x = 0; normal->y = copysignf(1.0f, diff.y); normal->z = 0; }
        else { normal->x = 0; normal->y = 0; normal->z = copysignf(1.0f, diff.z); }
        return 1;
    }
    return 0;
}

static int sphere_vs_sphere(const Collider* a, const Collider* b, Vec3* normal, float* depth) {
    Vec3 diff;
    vec3_sub(&a->sphere.center, &b->sphere.center, &diff);
    float dist = vec3_length(&diff);
    float radii = a->sphere.radius + b->sphere.radius;

    if (dist < radii) {
        *depth = radii - dist;
        if (dist > 0.0001f) {
            vec3_normalize(&diff, normal);
        } else {
            *normal = (Vec3){0, 1, 0};
        }
        return 1;
    }
    return 0;
}

static int sphere_vs_aabb(const Collider* sphere, const Collider* aabb, Vec3* normal, float* depth) {
    Vec3 closest;
    closest.x = fmaxf(aabb->aabb.min.x, fminf(sphere->sphere.center.x, aabb->aabb.max.x));
    closest.y = fmaxf(aabb->aabb.min.y, fminf(sphere->sphere.center.y, aabb->aabb.max.y));
    closest.z = fmaxf(aabb->aabb.min.z, fminf(sphere->sphere.center.z, aabb->aabb.max.z));

    Vec3 diff;
    vec3_sub(&sphere->sphere.center, &closest, &diff);
    float dist_sq = vec3_dot(&diff, &diff);

    if (dist_sq < sphere->sphere.radius * sphere->sphere.radius) {
        float dist = sqrtf(dist_sq);
        *depth = sphere->sphere.radius - dist;
        if (dist > 0.0001f) {
            vec3_normalize(&diff, normal);
        } else {
            *normal = (Vec3){0, 1, 0};
        }
        return 1;
    }
    return 0;
}

static int sphere_vs_plane(const Collider* sphere, const Collider* plane, Vec3* normal, float* depth) {
    float dist = vec3_dot(&sphere->sphere.center, &plane->plane.normal) - plane->plane.distance;
    if (dist < sphere->sphere.radius) {
        *depth = sphere->sphere.radius - dist;
        *normal = plane->plane.normal;
        return 1;
    }
    return 0;
}

int physics_check_collision(const Collider* a, const Collider* b, Vec3* normal, float* depth) {
    if (a->type == COLLIDER_AABB && b->type == COLLIDER_AABB)
        return aabb_vs_aabb(a, b, normal, depth);
    if (a->type == COLLIDER_SPHERE && b->type == COLLIDER_SPHERE)
        return sphere_vs_sphere(a, b, normal, depth);
    if (a->type == COLLIDER_SPHERE && b->type == COLLIDER_AABB)
        return sphere_vs_aabb(a, b, normal, depth);
    if (a->type == COLLIDER_AABB && b->type == COLLIDER_SPHERE) {
        int hit = sphere_vs_aabb(b, a, normal, depth);
        if (hit) { normal->x = -normal->x; normal->y = -normal->y; normal->z = -normal->z; }
        return hit;
    }
    if (a->type == COLLIDER_SPHERE && b->type == COLLIDER_PLANE)
        return sphere_vs_plane(a, b, normal, depth);
    if (a->type == COLLIDER_PLANE && b->type == COLLIDER_SPHERE) {
        int hit = sphere_vs_plane(b, a, normal, depth);
        if (hit) { normal->x = -normal->x; normal->y = -normal->y; normal->z = -normal->z; }
        return hit;
    }
    return 0;
}

int physics_raycast(PhysicsWorld* world, Vec3 origin, Vec3 direction, float max_dist,
                    uint32_t* hit_entity, Vec3* hit_point, Vec3* hit_normal) {
    vec3_normalize(&direction, &direction);
    float closest_t = max_dist;
    int hit = 0;

    for (uint32_t i = 0; i < world->collider_count; i++) {
        Collider* c = &world->colliders[i];
        if (c->type == COLLIDER_SPHERE) {
            Vec3 oc;
            vec3_sub(&origin, &c->sphere.center, &oc);
            float a = vec3_dot(&direction, &direction);
            float b = 2.0f * vec3_dot(&oc, &direction);
            float cc = vec3_dot(&oc, &oc) - c->sphere.radius * c->sphere.radius;
            float disc = b * b - 4 * a * cc;
            if (disc >= 0) {
                float t = (-b - sqrtf(disc)) / (2 * a);
                if (t > 0 && t < closest_t) {
                    closest_t = t;
                    *hit_entity = c->entity_id;
                    Vec3 scaled;
                    vec3_scale(&direction, t, &scaled);
                    vec3_add(&origin, &scaled, hit_point);
                    vec3_sub(hit_point, &c->sphere.center, hit_normal);
                    vec3_normalize(hit_normal, hit_normal);
                    hit = 1;
                }
            }
        }
    }
    return hit;
}

static int body_is_movable(Collider* c) {
    return !c->is_static && !c->body.is_kinematic && c->body.mass > 0.0f;
}

static float body_inv_mass(Collider* c) {
    if (c->is_static || c->body.is_kinematic || c->body.mass <= 0.0f) return 0.0f;
    return 1.0f / c->body.mass;
}

static void move_collider(Collider* c, Vec3 delta) {
    if (c->type == COLLIDER_SPHERE) {
        vec3_add(&c->sphere.center, &delta, &c->sphere.center);
    } else if (c->type == COLLIDER_AABB) {
        vec3_add(&c->aabb.min, &delta, &c->aabb.min);
        vec3_add(&c->aabb.max, &delta, &c->aabb.max);
    }
}

RigidBody* physics_get_body(PhysicsWorld* world, uint32_t index) {
    if (!world || index >= world->collider_count) return NULL;
    return &world->colliders[index].body;
}

void physics_apply_force(PhysicsWorld* world, uint32_t index, Vec3 force) {
    RigidBody* b = physics_get_body(world, index);
    if (!b || b->mass <= 0.0f) return;
    Vec3 scaled;
    vec3_scale(&force, 1.0f / b->mass, &scaled);
    vec3_add(&b->acceleration, &scaled, &b->acceleration);
}

void physics_apply_impulse(PhysicsWorld* world, uint32_t index, Vec3 impulse) {
    RigidBody* b = physics_get_body(world, index);
    if (!b || b->mass <= 0.0f) return;
    Vec3 scaled;
    vec3_scale(&impulse, 1.0f / b->mass, &scaled);
    vec3_add(&b->velocity, &scaled, &b->velocity);
}

void physics_step(PhysicsWorld* world, float dt) {
    if (!world || world->collider_count == 0 || dt <= 0.0f) return;

    const float linear_damping = 0.995f;

    // 1. 积分动态刚体
    for (uint32_t i = 0; i < world->collider_count; i++) {
        Collider* c = &world->colliders[i];
        if (!body_is_movable(c)) continue;

        RigidBody* b = &c->body;
        if (world->gravity_enabled) {
            vec3_add(&b->acceleration, &world->gravity, &b->acceleration);
        }

        Vec3 dv;
        vec3_scale(&b->acceleration, dt, &dv);
        vec3_add(&b->velocity, &dv, &b->velocity);
        b->acceleration = (Vec3){0, 0, 0};

        vec3_scale(&b->velocity, linear_damping, &b->velocity);

        Vec3 dp;
        vec3_scale(&b->velocity, dt, &dp);
        move_collider(c, dp);
    }

    // 2. 碰撞检测与响应
    for (uint32_t i = 0; i < world->collider_count; i++) {
        Collider* a = &world->colliders[i];
        for (uint32_t j = i + 1; j < world->collider_count; j++) {
            Collider* b = &world->colliders[j];
            if (a->is_static && b->is_static) continue;

            Vec3 normal;
            float depth = 0.0f;
            if (!physics_check_collision(a, b, &normal, &depth)) continue;

            // 触发器只报告，不解决
            if (a->is_trigger || b->is_trigger) continue;

            float inv_a = body_inv_mass(a);
            float inv_b = body_inv_mass(b);
            float total_inv = inv_a + inv_b;
            if (total_inv <= 0.0f) continue;

            // 位置修正（防止沉入）
            float percent = 0.5f; // 修正比例
            Vec3 correction;
            vec3_scale(&normal, depth * percent / total_inv, &correction);
            if (inv_a > 0.0f) {
                Vec3 da; vec3_scale(&correction, inv_a, &da);
                da.x = -da.x; da.y = -da.y; da.z = -da.z;
                move_collider(a, da);
            }
            if (inv_b > 0.0f) {
                Vec3 db; vec3_scale(&correction, inv_b, &db);
                move_collider(b, db);
            }

            // 冲量响应
            Vec3 rel_v;
            vec3_sub(&a->body.velocity, &b->body.velocity, &rel_v);
            float vel_along_normal = vec3_dot(&rel_v, &normal);
            if (vel_along_normal > 0.0f) continue; // 正在分离

            float e = fminf(a->body.restitution, b->body.restitution);
            float j = -(1.0f + e) * vel_along_normal / total_inv;

            Vec3 impulse;
            vec3_scale(&normal, j, &impulse);
            if (inv_a > 0.0f) {
                Vec3 va; vec3_scale(&impulse, inv_a, &va);
                vec3_add(&a->body.velocity, &va, &a->body.velocity);
            }
            if (inv_b > 0.0f) {
                Vec3 vb; vec3_scale(&impulse, inv_b, &vb);
                vec3_sub(&b->body.velocity, &vb, &b->body.velocity);
            }

            // 摩擦力
            vec3_sub(&a->body.velocity, &b->body.velocity, &rel_v);
            Vec3 tangent;
            Vec3 vn; vec3_scale(&normal, vec3_dot(&rel_v, &normal), &vn);
            vec3_sub(&rel_v, &vn, &tangent);
            float tan_len = vec3_length(&tangent);
            if (tan_len > 0.0001f) {
                vec3_scale(&tangent, 1.0f / tan_len, &tangent);
                float jt = -vec3_dot(&rel_v, &tangent) / total_inv;
                float mu = sqrtf(a->body.friction * a->body.friction + b->body.friction * b->body.friction);
                if (jt > j * mu) jt = j * mu;
                if (jt < -j * mu) jt = -j * mu;
                Vec3 friction_impulse;
                vec3_scale(&tangent, jt, &friction_impulse);
                if (inv_a > 0.0f) {
                    Vec3 va; vec3_scale(&friction_impulse, inv_a, &va);
                    vec3_add(&a->body.velocity, &va, &a->body.velocity);
                }
                if (inv_b > 0.0f) {
                    Vec3 vb; vec3_scale(&friction_impulse, inv_b, &vb);
                    vec3_sub(&b->body.velocity, &vb, &b->body.velocity);
                }
            }
        }
    }
}

void physics_set_gravity(PhysicsWorld* world, Vec3 gravity) {
    world->gravity = gravity;
}