#include "demo3d_sim.h"

#include <math.h>

#define DIST_EPSILON 0.03125f

typedef struct {
    f32 fraction;
    pxl8_vec3 normal;
    bool all_solid;
    bool start_solid;
} trace_result;

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

    i32 node_id = 0;
    while (node_id >= 0) {
        const demo3d_bsp_node* node = &bsp->nodes[node_id];
        const demo3d_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);
}

static i32 bsp_contents_from(const demo3d_bsp* bsp, i32 node_id, pxl8_vec3 pos) {
    while (node_id >= 0) {
        const demo3d_bsp_node* node = &bsp->nodes[node_id];
        const demo3d_bsp_plane* plane = &bsp->planes[node->plane_id];
        f32 d = pxl8_vec3_dot(pos, plane->normal) - plane->dist;
        node_id = node->children[d < 0 ? 1 : 0];
    }
    i32 leaf_idx = -(node_id + 1);
    if (leaf_idx < 0 || (u32)leaf_idx >= bsp->num_leafs) return -1;
    return bsp->leafs[leaf_idx].contents;
}

static bool bsp_recursive_trace(const demo3d_bsp* bsp, i32 node_id,
                                 f32 p1f, f32 p2f,
                                 pxl8_vec3 p1, pxl8_vec3 p2,
                                 trace_result* tr) {
    if (node_id < 0) {
        i32 leaf_idx = -(node_id + 1);
        if (leaf_idx >= 0 && (u32)leaf_idx < bsp->num_leafs &&
            bsp->leafs[leaf_idx].contents == -1) {
            tr->start_solid = true;
        } else {
            tr->all_solid = false;
        }
        return true;
    }

    const demo3d_bsp_node* node = &bsp->nodes[node_id];
    const demo3d_bsp_plane* plane = &bsp->planes[node->plane_id];

    f32 t1 = pxl8_vec3_dot(p1, plane->normal) - plane->dist;
    f32 t2 = pxl8_vec3_dot(p2, plane->normal) - plane->dist;

    if (t1 >= 0 && t2 >= 0)
        return bsp_recursive_trace(bsp, node->children[0], p1f, p2f, p1, p2, tr);
    if (t1 < 0 && t2 < 0)
        return bsp_recursive_trace(bsp, node->children[1], p1f, p2f, p1, p2, tr);

    i32 side;
    f32 frac;
    if (t1 < 0) {
        frac = (t1 + DIST_EPSILON) / (t1 - t2);
        side = 1;
    } else {
        frac = (t1 - DIST_EPSILON) / (t1 - t2);
        side = 0;
    }
    if (frac < 0) frac = 0;
    if (frac > 1) frac = 1;

    f32 midf = p1f + (p2f - p1f) * frac;
    pxl8_vec3 mid = {
        p1.x + frac * (p2.x - p1.x),
        p1.y + frac * (p2.y - p1.y),
        p1.z + frac * (p2.z - p1.z),
    };

    if (!bsp_recursive_trace(bsp, node->children[side], p1f, midf, p1, mid, tr))
        return false;

    if (bsp_contents_from(bsp, node->children[side ^ 1], mid) != -1)
        return bsp_recursive_trace(bsp, node->children[side ^ 1], midf, p2f, mid, p2, tr);

    if (tr->all_solid)
        return false;

    if (midf < tr->fraction) {
        tr->fraction = midf;
        if (side == 0) {
            tr->normal = plane->normal;
        } else {
            tr->normal.x = -plane->normal.x;
            tr->normal.y = -plane->normal.y;
            tr->normal.z = -plane->normal.z;
        }
    }
    return false;
}

static trace_result bsp_trace_line(const demo3d_bsp* bsp, pxl8_vec3 from, pxl8_vec3 to) {
    trace_result tr = { .fraction = 1.0f, .all_solid = true };
    if (!bsp || bsp->num_nodes == 0) {
        tr.all_solid = false;
        return tr;
    }
    bsp_recursive_trace(bsp, 0, 0.0f, 1.0f, from, to, &tr);
    if (tr.all_solid) {
        tr.fraction = 0.0f;
        tr.start_solid = true;
    }
    return tr;
}

bool demo3d_bsp_point_solid(const demo3d_bsp* bsp, pxl8_vec3 pos) {
    if (!bsp) return false;
    if (bsp->bounds_max_x > bsp->bounds_min_x &&
        (pos.x < bsp->bounds_min_x || pos.x >= bsp->bounds_max_x ||
         pos.z < bsp->bounds_min_z || pos.z >= bsp->bounds_max_z))
        return false;
    i32 leaf = bsp_find_leaf(bsp, pos);
    if (leaf < 0 || (u32)leaf >= bsp->num_leafs) return true;
    return bsp->leafs[leaf].contents == -1;
}

static void trace_offsets(const demo3d_bsp* bsp, pxl8_vec3 from, pxl8_vec3 to,
                          f32 radius, f32* out_frac, pxl8_vec3* out_normal) {
    f32 d = radius * 0.7071f;
    pxl8_vec3 offsets[9] = {
        {0, 0, 0},
        {radius, 0, 0}, {-radius, 0, 0},
        {0, 0, radius}, {0, 0, -radius},
        {d, 0, d}, {d, 0, -d},
        {-d, 0, d}, {-d, 0, -d},
    };

    *out_frac = 1.0f;
    *out_normal = (pxl8_vec3){0, 0, 0};

    for (i32 i = 0; i < 9; i++) {
        pxl8_vec3 s = { from.x + offsets[i].x, from.y + offsets[i].y, from.z + offsets[i].z };
        pxl8_vec3 e = { to.x + offsets[i].x, to.y + offsets[i].y, to.z + offsets[i].z };
        trace_result tr = bsp_trace_line(bsp, s, e);
        if (tr.fraction < *out_frac && !tr.start_solid) {
            *out_frac = tr.fraction;
            *out_normal = tr.normal;
        }
    }
}

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

    f32 frac;
    pxl8_vec3 normal;
    trace_offsets(bsp, from, to, radius, &frac, &normal);
    if (frac >= 1.0f) return to;

    pxl8_vec3 delta = { to.x - from.x, to.y - from.y, to.z - from.z };
    pxl8_vec3 hit = {
        from.x + delta.x * frac,
        from.y + delta.y * frac,
        from.z + delta.z * frac,
    };

    f32 remaining = 1.0f - frac;
    if (remaining <= 0.0f) return hit;

    f32 backoff = pxl8_vec3_dot(delta, normal) * remaining;
    pxl8_vec3 slide_target = {
        hit.x + delta.x * remaining - normal.x * backoff,
        hit.y + delta.y * remaining - normal.y * backoff,
        hit.z + delta.z * remaining - normal.z * backoff,
    };

    f32 slide_frac;
    pxl8_vec3 slide_normal;
    trace_offsets(bsp, hit, slide_target, radius, &slide_frac, &slide_normal);
    if (slide_frac >= 1.0f) return slide_target;

    pxl8_vec3 slide_delta = {
        slide_target.x - hit.x,
        slide_target.y - hit.y,
        slide_target.z - hit.z,
    };
    return (pxl8_vec3){
        hit.x + slide_delta.x * slide_frac,
        hit.y + slide_delta.y * slide_frac,
        hit.z + slide_delta.z * slide_frac,
    };
}

static const demo3d_bsp* sim_bsp_at(const demo3d_sim_world* world, f32 x, f32 z) {
    i32 cx = (i32)floorf(x / world->chunk_size);
    i32 cz = (i32)floorf(z / world->chunk_size);
    i32 dx = cx - world->center_cx + 1;
    i32 dz = cz - world->center_cz + 1;
    if (dx < 0 || dx > 2 || dz < 0 || dz > 2) return NULL;
    return world->chunks[dz * 3 + dx];
}

static f32 bsp_terrain_height(const demo3d_bsp* bsp, f32 x, f32 z) {
    if (!bsp || !bsp->heightfield || bsp->heightfield_cell_size <= 0) return -1e9f;
    f32 lx = (x - bsp->heightfield_ox) / bsp->heightfield_cell_size;
    f32 lz = (z - bsp->heightfield_oz) / bsp->heightfield_cell_size;
    i32 ix = (i32)floorf(lx);
    i32 iz = (i32)floorf(lz);
    if (ix < 0 || ix >= bsp->heightfield_w - 1 || iz < 0 || iz >= bsp->heightfield_h - 1) return -1e9f;
    f32 fx = lx - ix;
    f32 fz = lz - iz;
    i32 w = bsp->heightfield_w;
    f32 h00 = bsp->heightfield[iz * w + ix];
    f32 h10 = bsp->heightfield[iz * w + ix + 1];
    f32 h01 = bsp->heightfield[(iz + 1) * w + ix];
    f32 h11 = bsp->heightfield[(iz + 1) * w + ix + 1];
    f32 h0 = h00 + (h10 - h00) * fx;
    f32 h1 = h01 + (h11 - h01) * fx;
    return h0 + (h1 - h0) * fz;
}

static f32 sim_terrain_height(const demo3d_sim_world* world, f32 x, f32 z) {
    const demo3d_bsp* bsp = sim_bsp_at(world, x, z);
    return bsp_terrain_height(bsp, x, z);
}

static void sim_trace_offsets(const demo3d_sim_world* world, pxl8_vec3 from, pxl8_vec3 to,
                              f32 radius, f32* out_frac, pxl8_vec3* out_normal) {
    f32 d = radius * 0.7071f;
    pxl8_vec3 offsets[9] = {
        {0, 0, 0},
        {radius, 0, 0}, {-radius, 0, 0},
        {0, 0, radius}, {0, 0, -radius},
        {d, 0, d}, {d, 0, -d},
        {-d, 0, d}, {-d, 0, -d},
    };

    *out_frac = 1.0f;
    *out_normal = (pxl8_vec3){0, 0, 0};

    for (i32 i = 0; i < 9; i++) {
        pxl8_vec3 s = { from.x + offsets[i].x, from.y + offsets[i].y, from.z + offsets[i].z };
        pxl8_vec3 e = { to.x + offsets[i].x, to.y + offsets[i].y, to.z + offsets[i].z };
        const demo3d_bsp* bsp_s = sim_bsp_at(world, s.x, s.z);
        const demo3d_bsp* bsp_e = sim_bsp_at(world, e.x, e.z);
        if (bsp_s) {
            trace_result tr = bsp_trace_line(bsp_s, s, e);
            if (tr.fraction < *out_frac && !tr.start_solid) {
                *out_frac = tr.fraction;
                *out_normal = tr.normal;
            }
        }
        if (bsp_e && bsp_e != bsp_s) {
            bool inside_bounds = !(bsp_e->bounds_max_x > bsp_e->bounds_min_x) ||
                (s.x >= bsp_e->bounds_min_x && s.x < bsp_e->bounds_max_x &&
                 s.z >= bsp_e->bounds_min_z && s.z < bsp_e->bounds_max_z);
            if (inside_bounds) {
                trace_result tr = bsp_trace_line(bsp_e, s, e);
                if (tr.fraction < *out_frac && !tr.start_solid) {
                    *out_frac = tr.fraction;
                    *out_normal = tr.normal;
                }
            }
        }
    }
}

pxl8_vec3 demo3d_sim_trace(const demo3d_sim_world* world, pxl8_vec3 from, pxl8_vec3 to, f32 radius, f32 height) {
    (void)height;
    if (!world || world->chunk_size <= 0) return to;

    f32 frac;
    pxl8_vec3 normal;
    sim_trace_offsets(world, from, to, radius, &frac, &normal);
    if (frac >= 1.0f) return to;

    pxl8_vec3 delta = { to.x - from.x, to.y - from.y, to.z - from.z };
    pxl8_vec3 hit = {
        from.x + delta.x * frac,
        from.y + delta.y * frac,
        from.z + delta.z * frac,
    };

    f32 remaining = 1.0f - frac;
    if (remaining <= 0.0f) return hit;

    f32 backoff = pxl8_vec3_dot(delta, normal) * remaining;
    pxl8_vec3 slide_target = {
        hit.x + delta.x * remaining - normal.x * backoff,
        hit.y + delta.y * remaining - normal.y * backoff,
        hit.z + delta.z * remaining - normal.z * backoff,
    };

    f32 slide_frac;
    pxl8_vec3 slide_normal;
    sim_trace_offsets(world, hit, slide_target, radius, &slide_frac, &slide_normal);
    if (slide_frac >= 1.0f) return slide_target;

    pxl8_vec3 slide_delta = {
        slide_target.x - hit.x,
        slide_target.y - hit.y,
        slide_target.z - hit.z,
    };
    return (pxl8_vec3){
        hit.x + slide_delta.x * slide_frac,
        hit.y + slide_delta.y * slide_frac,
        hit.z + slide_delta.z * slide_frac,
    };
}

bool demo3d_sim_check_ground(const demo3d_sim_world* world, pxl8_vec3 pos, f32 radius) {
    if (!world || world->chunk_size <= 0) return true;

    f32 th = sim_terrain_height(world, pos.x, pos.z);
    if (th > -1e8f && pos.y - th < 2.0f) return true;

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

    return result.y > down.y;
}

void demo3d_sim_move_player(demo3d_sim_entity* ent, const demo3d_input_msg* input,
                          const demo3d_sim_world* world, const demo3d_sim_config* cfg, f32 dt) {
    if (!ent || !input || !cfg) return;
    if (!(ent->flags & DEMO3D_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 & DEMO3D_SIM_FLAG_GROUNDED) != 0;

    if (grounded && (input->buttons & 1)) {
        ent->vel.y = cfg->jump_velocity;
        ent->flags &= ~DEMO3D_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
    };

    if (grounded) {
        f32 th_dest = sim_terrain_height(world, target.x, target.z);
        if (th_dest > -1e8f) target.y = th_dest;
    }

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

    f32 th = sim_terrain_height(world, new_pos.x, new_pos.z);
    if (th > -1e8f && new_pos.y < th) {
        new_pos.y = th;
        if (ent->vel.y < 0) ent->vel.y = 0;
    }

    ent->pos = new_pos;

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

void demo3d_sim_integrate(demo3d_sim_entity* ent, const demo3d_sim_world* world,
                        const demo3d_sim_config* cfg, f32 dt) {
    if (!ent || !cfg) return;
    if (!(ent->flags & DEMO3D_SIM_FLAG_ALIVE)) return;
    if (ent->flags & DEMO3D_SIM_FLAG_PLAYER) return;

    bool grounded = (ent->flags & DEMO3D_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
    };

    if (grounded) {
        f32 th_dest = sim_terrain_height(world, target.x, target.z);
        if (th_dest > -1e8f) target.y = th_dest;
    }

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

    f32 th2 = sim_terrain_height(world, new_pos.x, new_pos.z);
    if (th2 > -1e8f && new_pos.y < th2) {
        new_pos.y = th2;
        if (ent->vel.y < 0.0f) ent->vel.y = 0.0f;
    }

    ent->pos = new_pos;

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