#include "pxl8_sim.h"

#include <math.h>

static i32 bsp_find_leaf(const pxl8_bsp* bsp, pxl8_vec3 pos) {
    if (!bsp || bsp->num_nodes == 0) return -1;

    i32 node_id = 0;
    while (node_id >= 0) {
        const pxl8_bsp_node* node = &bsp->nodes[node_id];
        const pxl8_bsp_plane* plane = &bsp->planes[node->plane_id];
        f32 dist = pxl8_vec3_dot(pos, plane->normal) - plane->dist;
        node_id = node->children[dist < 0 ? 1 : 0];
    }

    return -(node_id + 1);
}

bool pxl8_bsp_point_solid(const pxl8_bsp* bsp, pxl8_vec3 pos) {
    i32 leaf = bsp_find_leaf(bsp, pos);
    if (leaf < 0 || (u32)leaf >= bsp->num_leafs) return true;
    return bsp->leafs[leaf].contents == -1;
}

static bool bsp_point_clear(const pxl8_bsp* bsp, f32 x, f32 y, f32 z, f32 radius) {
    if (pxl8_bsp_point_solid(bsp, (pxl8_vec3){x, y, z})) return false;
    if (pxl8_bsp_point_solid(bsp, (pxl8_vec3){x + radius, y, z})) return false;
    if (pxl8_bsp_point_solid(bsp, (pxl8_vec3){x - radius, y, z})) return false;
    if (pxl8_bsp_point_solid(bsp, (pxl8_vec3){x, y, z + radius})) return false;
    if (pxl8_bsp_point_solid(bsp, (pxl8_vec3){x, y, z - radius})) return false;
    return true;
}

pxl8_vec3 pxl8_bsp_trace(const pxl8_bsp* bsp, pxl8_vec3 from, pxl8_vec3 to, f32 radius) {
    if (!bsp || bsp->num_nodes == 0) return to;

    if (bsp_point_clear(bsp, to.x, to.y, to.z, radius)) {
        return to;
    }

    bool x_ok = bsp_point_clear(bsp, to.x, from.y, from.z, radius);
    bool y_ok = bsp_point_clear(bsp, from.x, to.y, from.z, radius);
    bool z_ok = bsp_point_clear(bsp, from.x, from.y, to.z, radius);

    pxl8_vec3 result = from;
    if (x_ok) result.x = to.x;
    if (y_ok) result.y = to.y;
    if (z_ok) result.z = to.z;

    return result;
}

pxl8_vec3 pxl8_sim_trace(const pxl8_sim_world* world, pxl8_vec3 from, pxl8_vec3 to, f32 radius, f32 height) {
    (void)height;
    if (!world) return to;

    if (world->bsp) {
        return pxl8_bsp_trace(world->bsp, from, to, radius);
    }

    return to;
}

bool pxl8_sim_check_ground(const pxl8_sim_world* world, pxl8_vec3 pos, f32 radius) {
    if (!world) return true;
    if (!world->bsp) return true;

    pxl8_vec3 down = {pos.x, pos.y - 2.0f, pos.z};
    pxl8_vec3 result = pxl8_sim_trace(world, pos, down, radius, 0.0f);

    return result.y > down.y;
}

void pxl8_sim_move_player(pxl8_sim_entity* ent, const pxl8_input_msg* input,
                          const pxl8_sim_world* world, const pxl8_sim_config* cfg, f32 dt) {
    if (!ent || !input || !cfg) return;
    if (!(ent->flags & PXL8_SIM_FLAG_ALIVE)) return;

    ent->yaw -= input->look_dx * 0.008f;
    f32 new_pitch = ent->pitch - input->look_dy * 0.008f;
    if (new_pitch > cfg->max_pitch) new_pitch = cfg->max_pitch;
    if (new_pitch < -cfg->max_pitch) new_pitch = -cfg->max_pitch;
    ent->pitch = new_pitch;

    f32 sin_yaw = sinf(ent->yaw);
    f32 cos_yaw = cosf(ent->yaw);

    f32 input_len = sqrtf(input->move_x * input->move_x + input->move_y * input->move_y);

    pxl8_vec3 move_dir = {0, 0, 0};
    f32 target_speed = 0.0f;

    if (input_len > 0.0f) {
        f32 nx = input->move_x / input_len;
        f32 ny = input->move_y / input_len;
        move_dir.x = cos_yaw * nx - sin_yaw * ny;
        move_dir.z = -sin_yaw * nx - cos_yaw * ny;
        target_speed = cfg->move_speed;
    }

    ent->vel.x = move_dir.x * target_speed;
    ent->vel.z = move_dir.z * target_speed;

    bool grounded = (ent->flags & PXL8_SIM_FLAG_GROUNDED) != 0;

    if (grounded && (input->buttons & 1)) {
        ent->vel.y = cfg->jump_velocity;
        ent->flags &= ~PXL8_SIM_FLAG_GROUNDED;
        grounded = false;
    }

    if (!grounded) {
        ent->vel.y -= cfg->gravity * dt;
    }

    pxl8_vec3 target = {
        ent->pos.x + ent->vel.x * dt,
        ent->pos.y + ent->vel.y * dt,
        ent->pos.z + ent->vel.z * dt
    };

    pxl8_vec3 new_pos = pxl8_sim_trace(world, ent->pos, target, cfg->player_radius, cfg->player_height);

    if (ent->vel.y < 0 && new_pos.y > target.y + 0.01f) {
        f32 hi = new_pos.y;
        f32 lo = target.y;
        for (i32 i = 0; i < 8; i++) {
            f32 mid = (hi + lo) * 0.5f;
            pxl8_vec3 test = {new_pos.x, mid, new_pos.z};
            pxl8_vec3 result = pxl8_sim_trace(world, new_pos, test, cfg->player_radius, cfg->player_height);
            if (result.y > mid + 0.01f) {
                lo = mid;
            } else {
                hi = mid;
            }
        }
        new_pos.y = hi;
    }

    ent->pos = new_pos;

    if (pxl8_sim_check_ground(world, ent->pos, cfg->player_radius)) {
        ent->flags |= PXL8_SIM_FLAG_GROUNDED;
        if (ent->vel.y < 0) ent->vel.y = 0;
    } else {
        ent->flags &= ~PXL8_SIM_FLAG_GROUNDED;
    }
}

void pxl8_sim_integrate(pxl8_sim_entity* ent, const pxl8_sim_world* world,
                        const pxl8_sim_config* cfg, f32 dt) {
    if (!ent || !cfg) return;
    if (!(ent->flags & PXL8_SIM_FLAG_ALIVE)) return;
    if (ent->flags & PXL8_SIM_FLAG_PLAYER) return;

    bool grounded = (ent->flags & PXL8_SIM_FLAG_GROUNDED) != 0;

    ent->vel.y -= cfg->gravity * dt;

    if (grounded) {
        f32 speed = sqrtf(ent->vel.x * ent->vel.x + ent->vel.z * ent->vel.z);
        if (speed > 0.0f) {
            f32 drop = speed * cfg->friction * dt;
            f32 new_speed = speed - drop;
            if (new_speed < 0.0f) new_speed = 0.0f;
            f32 scale = new_speed / speed;
            ent->vel.x *= scale;
            ent->vel.z *= scale;
        }
    }

    pxl8_vec3 target = {
        ent->pos.x + ent->vel.x * dt,
        ent->pos.y + ent->vel.y * dt,
        ent->pos.z + ent->vel.z * dt
    };

    pxl8_vec3 new_pos = pxl8_sim_trace(world, ent->pos, target, cfg->player_radius, cfg->player_height);

    if (ent->vel.y < 0 && new_pos.y > target.y + 0.01f) {
        f32 hi = new_pos.y;
        f32 lo = target.y;
        for (i32 i = 0; i < 8; i++) {
            f32 mid = (hi + lo) * 0.5f;
            pxl8_vec3 test = {new_pos.x, mid, new_pos.z};
            pxl8_vec3 result = pxl8_sim_trace(world, new_pos, test, cfg->player_radius, cfg->player_height);
            if (result.y > mid + 0.01f) {
                lo = mid;
            } else {
                hi = mid;
            }
        }
        new_pos.y = hi;
    }

    ent->pos = new_pos;

    if (pxl8_sim_check_ground(world, ent->pos, cfg->player_radius)) {
        ent->flags |= PXL8_SIM_FLAG_GROUNDED;
        if (ent->vel.y < 0.0f) ent->vel.y = 0.0f;
    } else {
        ent->flags &= ~PXL8_SIM_FLAG_GROUNDED;
    }
}