#include "physics.h"

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

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);
}

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;
    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;
    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_static = 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) {
    Collider* c = &world->colliders[index];
    if (c->type == COLLIDER_SPHERE) {
        c->sphere.center = position;
    }
}

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, size;
    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, diff.x), 0, 0};
        else if (dy > dz) { normal->x = 0; normal->y = copysignf(1, diff.y); normal->z = 0; }
        else { normal->x = 0; normal->y = 0; normal->z = copysignf(1, 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;
}

void physics_step(PhysicsWorld* world, float dt) {
    (void)world;
    (void)dt;
}

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