#include "pxl8_sim.h"

#include <math.h>

static usize vxl_world_to_local(f32 x, i32 chunk_coord) {
    f32 chunk_base = chunk_coord * PXL8_VXL_WORLD_CHUNK_SIZE;
    f32 local_world = x - chunk_base;
    i32 local_voxel = (i32)floorf(local_world / PXL8_VXL_SCALE);
    if (local_voxel < 0) return 0;
    if (local_voxel >= PXL8_VXL_CHUNK_SIZE) return PXL8_VXL_CHUNK_SIZE - 1;
    return (usize)local_voxel;
}

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

bool pxl8_vxl_point_solid(const pxl8_vxl_chunk* chunk, i32 cx, i32 cy, i32 cz,
                          f32 x, f32 y, f32 z) {
    if (!chunk) return false;

    usize lx = vxl_world_to_local(x, cx);
    usize ly = vxl_world_to_local(y, cy);
    usize lz = vxl_world_to_local(z, cz);

    return pxl8_vxl_block_get(chunk, (i32)lx, (i32)ly, (i32)lz) != PXL8_VXL_BLOCK_AIR;
}

static bool vxl_point_clear(const pxl8_vxl_chunk* chunk, i32 cx, i32 cy, i32 cz,
                            f32 x, f32 y, f32 z, f32 radius) {
    if (pxl8_vxl_point_solid(chunk, cx, cy, cz, x - radius, y, z)) return false;
    if (pxl8_vxl_point_solid(chunk, cx, cy, cz, x + radius, y, z)) return false;
    if (pxl8_vxl_point_solid(chunk, cx, cy, cz, x, y - radius, z)) return false;
    if (pxl8_vxl_point_solid(chunk, cx, cy, cz, x, y + radius, z)) return false;
    if (pxl8_vxl_point_solid(chunk, cx, cy, cz, x, y, z - radius)) return false;
    if (pxl8_vxl_point_solid(chunk, cx, cy, cz, x, y, z + radius)) return false;
    return true;
}

pxl8_vec3 pxl8_vxl_trace(const pxl8_vxl_chunk* chunk, i32 cx, i32 cy, i32 cz,
                         pxl8_vec3 from, pxl8_vec3 to, f32 radius) {
    if (!chunk) return to;

    if (vxl_point_clear(chunk, cx, cy, cz, to.x, to.y, to.z, radius)) {
        return to;
    }

    bool x_ok = vxl_point_clear(chunk, cx, cy, cz, to.x, from.y, from.z, radius);
    bool y_ok = vxl_point_clear(chunk, cx, cy, cz, from.x, to.y, from.z, radius);
    bool z_ok = vxl_point_clear(chunk, cx, cy, cz, 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) {
    if (!world) return to;

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

    if (world->vxl) {
        return pxl8_vxl_trace(world->vxl, world->vxl_cx, world->vxl_cy, world->vxl_cz,
                              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 && !world->vxl) return true;

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

    return result.y > down.y;
}

void pxl8_sim_move_player(pxl8_sim_entity* ent, const pxl8_input_msg* input,
                          const pxl8_sim_world* world, f32 dt) {
    if (!ent || !input) 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 > PXL8_SIM_MAX_PITCH) new_pitch = PXL8_SIM_MAX_PITCH;
    if (new_pitch < -PXL8_SIM_MAX_PITCH) new_pitch = -PXL8_SIM_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 = PXL8_SIM_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 = PXL8_SIM_JUMP_VELOCITY;
        ent->flags &= ~PXL8_SIM_FLAG_GROUNDED;
        grounded = false;
    }

    if (!grounded) {
        ent->vel.y -= PXL8_SIM_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, PXL8_SIM_PLAYER_RADIUS);
    ent->pos = new_pos;

    if (pxl8_sim_check_ground(world, ent->pos, PXL8_SIM_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, f32 dt) {
    if (!ent) 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 -= PXL8_SIM_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 * PXL8_SIM_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, PXL8_SIM_PLAYER_RADIUS);
    ent->pos = new_pos;

    if (pxl8_sim_check_ground(world, ent->pos, PXL8_SIM_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;
    }
}