#include "phys.h"
#include "sim_snapshot.h"
#include "sim_ent.h"
#include "math.h"
#include "scratch.h"
#include "space.h"

GLOBAL struct {
    /* Constants */
    struct math_spring_result contact_softness;
    struct math_spring_result mouse_joint_linear_softness;
    struct math_spring_result mouse_joint_angular_softness;
} G = ZI, DEBUG_ALIAS(G, G_phys);

/* ========================== *
 * Startup
 * ========================== */

struct phys_startup_receipt phys_startup(void)
{
    /* Initialize constants */
    const f32 substep_dt = (1.f / ((f32)SIM_TICKS_PER_SECOND * (f32)SIM_PHYSICS_SUBSTEPS));

    const f32 contact_frequency = (1.f / substep_dt) / 8.f;
    const f32 contact_damping_ratio = 10;
    G.contact_softness = math_spring(contact_frequency, contact_damping_ratio, substep_dt);

    const f32 mouse_joint_linear_frequency = 5.0f;
    const f32 mouse_joint_linear_damping_ratio = 0.7f;
    const f32 mouse_joint_angular_frequency = 0.5f;
    const f32 mouse_joint_angular_damping_ratio = 0.1f;
    G.mouse_joint_linear_softness = math_spring(mouse_joint_linear_frequency, mouse_joint_linear_damping_ratio, substep_dt);
    G.mouse_joint_angular_softness = math_spring(mouse_joint_angular_frequency, mouse_joint_angular_damping_ratio, substep_dt);

    return (struct phys_startup_receipt) { 0 };
}

/* ========================== *
 * Contact
 * ========================== */

struct phys_collision_data_array phys_create_and_update_contacts(struct arena *arena, struct phys_ctx *ctx, f32 elapsed_dt, u64 phys_iteration)
{
    __prof;
    struct phys_collision_data_array res = ZI;
    res.a = arena_dry_push(arena, struct phys_collision_data);
    struct sim_snapshot *ss = ctx->ss;
    struct sim_ent_lookup *contact_lookup = &ss->contact_lookup;
    struct space *space = ss->space;
#if COLLIDER_DEBUG
    struct sim_ent_lookup *debug_lookup = &ss->collision_debug_lookup;
#endif

    struct sim_ent *root = sim_ent_from_handle(ss, SIM_ENT_ROOT_HANDLE);
    u64 tick = ss->tick;

    for (u64 check0_index = 0; check0_index < ss->num_ents_reserved; ++check0_index) {
        struct sim_ent *check0 = &ss->ents[check0_index];
        if (!sim_ent_is_valid_and_active(check0)) continue;
        if (!(sim_ent_has_prop(check0, SIM_ENT_PROP_PHYSICAL_DYNAMIC) || sim_ent_has_prop(check0, SIM_ENT_PROP_PHYSICAL_KINEMATIC))) continue;
        if (check0->local_collider.count <= 0) continue;

        struct xform check0_xf = sim_ent_get_xform(check0);
        struct collider_shape check0_collider = check0->local_collider;
        struct aabb aabb = collider_aabb_from_collider(&check0_collider, check0_xf);

        struct space_iter iter = space_iter_begin_aabb(space, aabb);
        struct space_entry *space_entry;
        while ((space_entry = space_iter_next(&iter))) {
            struct sim_ent *check1 = sim_ent_from_handle(ss, space_entry->ent);
            if (check1 == check0) continue;
            if (!sim_ent_is_valid_and_active(check1)) continue;
            if (!(sim_ent_has_prop(check1, SIM_ENT_PROP_PHYSICAL_DYNAMIC) || sim_ent_has_prop(check1, SIM_ENT_PROP_PHYSICAL_KINEMATIC))) continue;
            if (check1->local_collider.count <= 0) continue;

            /* Deterministic order based on entity index */
            struct sim_ent *e0;
            struct sim_ent *e1;
            struct xform e0_xf;
            struct xform e1_xf;
            struct collider_shape e0_collider;
            struct collider_shape e1_collider;
            if (check0_index < check1->handle.idx) {
                e0 = check0;
                e1 = check1;
                e0_xf = check0_xf;
                e1_xf = sim_ent_get_xform(check1);
                e0_collider = check0_collider;
                e1_collider = check1->local_collider;
            } else {
                e0 = check1;
                e1 = check0;
                e0_xf = sim_ent_get_xform(check1);
                e1_xf = check0_xf;
                e0_collider = check1->local_collider;
                e1_collider = check0_collider;
            }

            struct sim_ent_lookup_key key = sim_ent_lookup_key_from_two_handles(e0->handle, e1->handle);
            struct sim_ent_lookup_entry *constraint_entry = sim_ent_lookup_get(contact_lookup, key);

            struct sim_ent *constraint_ent = sim_ent_nil();
            if (constraint_entry) {
                constraint_ent = sim_ent_from_handle(ss, constraint_entry->ent);
                if (sim_ent_is_valid_and_active(constraint_ent)) {
                    if (constraint_ent->contact_constraint_data.last_phys_iteration >= phys_iteration) {
                        /* Already processed constraint this iteration */
                        continue;
                    } else {
                        constraint_ent->contact_constraint_data.last_phys_iteration = phys_iteration;
                    }
                } else {
                    /* Constraint ent no longer valid, delete constraint_entry*/
                    sim_ent_lookup_remove(contact_lookup, constraint_entry);
                    constraint_entry= NULL;
                }
            }

            /* Calculate collision */
            struct collider_collision_points_result collider_res = collider_collision_points(&e0_collider, &e1_collider, e0_xf, e1_xf);

            /* Parts of algorithm are hard-coded to support 2 contact points */
            CT_ASSERT(ARRAY_COUNT(constraint_ent->contact_constraint_data.points) == 2);
            CT_ASSERT(ARRAY_COUNT(collider_res.points) == 2);

            struct phys_contact_constraint *constraint = NULL;
            if (collider_res.num_points > 0) {
                if (!sim_ent_is_valid_and_active(constraint_ent)) {
                    /* Create constraint */
                    {
                        constraint_ent = sim_ent_alloc(root);
                        constraint_ent->contact_constraint_data.e1 = e1->handle;
                        constraint_ent->contact_constraint_data.e0 = e0->handle;
                        constraint_ent->contact_constraint_data.skip_solve = sim_ent_has_prop(e0, SIM_ENT_PROP_SENSOR) || sim_ent_has_prop(e1, SIM_ENT_PROP_SENSOR)
                            || !(sim_ent_has_prop(e0, SIM_ENT_PROP_PHYSICAL_DYNAMIC) || sim_ent_has_prop(e1, SIM_ENT_PROP_PHYSICAL_DYNAMIC));
                        sim_ent_enable_prop(constraint_ent, SIM_ENT_PROP_ACTIVE);

                        /* TODO: Should we recalculate normal as more contact points are added? */
                        sim_ent_enable_prop(constraint_ent, SIM_ENT_PROP_CONTACT_CONSTRAINT);
                        sim_ent_activate(constraint_ent, tick);
                        ASSERT(!constraint_entry);  /* Existing entry should never be present here */
                        sim_ent_lookup_set(contact_lookup, key, constraint_ent->handle);
                    }

                    /* Push collision data */
                    if (ctx->pre_solve_callback || ctx->post_solve_callback) {
                        struct phys_collision_data *data = arena_push_zero(arena, struct phys_collision_data);
                        ++res.count;
                        data->constraint = &constraint_ent->contact_constraint_data;
                        data->e0 = e0->handle;
                        data->e1 = e1->handle;
                        data->normal = collider_res.normal;
                        data->dt = elapsed_dt;

                        /* Calculate point */
                        struct v2 point = collider_res.points[0].point;
                        if (collider_res.num_points > 1) {
                            point = v2_add(point, v2_mul(v2_sub(collider_res.points[1].point, point), 0.5f));
                        }
                        data->point = point;

                        /* Calculate relative velocity */
                        struct v2 vrel;
                        {
                            struct v2 v0 = e0->linear_velocity;
                            struct v2 v1 = e1->linear_velocity;
                            f32 w0 = e0->angular_velocity;
                            f32 w1 = e1->angular_velocity;
                            struct v2 vcp0 = v2_sub(point, e0_xf.og);
                            struct v2 vcp1 = v2_sub(point, e1_xf.og);
                            struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
                            struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
                            vrel = v2_sub(vel0, vel1);
                        }
                        data->vrel = vrel;
                    }
                }
                constraint = &constraint_ent->contact_constraint_data;
                constraint->normal = collider_res.normal;
                constraint->friction = math_sqrt(e0->friction * e1->friction);

                /* Delete old contacts that are no longer present */
                for (u32 i = 0; i < constraint->num_points; ++i) {
                    struct phys_contact_point *old = &constraint->points[i];
                    u32 id = old->id;
                    b32 found = false;
                    for (u32 j = 0; j < collider_res.num_points; ++j) {
                        if (collider_res.points[j].id == id) {
                            found = true;
                            break;
                        }
                    }
                    if (!found) {
                        /* Delete contact by replacing with last in array */
                        *old = constraint->points[--constraint->num_points];
                        --i;
                    }
                }

                /* Update / insert returned contacts */
                for (u32 i = 0; i < collider_res.num_points; ++i) {
                    struct collider_collision_point *res_point = &collider_res.points[i];
                    struct v2 point = res_point->point;
                    f32 sep = res_point->separation;
                    u32 id = res_point->id;
                    struct phys_contact_point *contact = NULL;
                    /* Match */
                    for (u32 j = 0; j < constraint->num_points; ++j) {
                        struct phys_contact_point *t = &constraint->points[j];
                        if (t->id == id) {
                            contact = t;
                            break;
                        }
                    }
                    if (!contact) {
                        /* Insert */
                        contact = &constraint->points[constraint->num_points++];
                        MEMZERO_STRUCT(contact);
                        contact->id = id;
                        constraint->softness = G.contact_softness;
                        constraint->pushout_velocity = 3.0f;
                    }

                    /* Update points & separation */
                    contact->point_local_e0 = xform_invert_mul_v2(e0_xf, point);
                    contact->point_local_e1 = xform_invert_mul_v2(e1_xf, point);
                    contact->starting_separation = sep;

#if DEVELOPER
                    contact->dbg_pt = point;
#endif
                }
            } else if (constraint_ent->valid) {
                constraint_ent->contact_constraint_data.num_points = 0;
            }

            /* TODO: Remove this (debugging) */
#if COLLIDER_DEBUG && COLLIDER_DEBUG_DETAILED
            {
                struct sim_ent *dbg_ent = sim_ent_nil();
                struct sim_ent_lookup_entry *dbg_entry = sim_ent_lookup_get(debug_lookup, key);
                if (dbg_entry) {
                    dbg_ent = sim_ent_from_handle(ss, dbg_entry->entity);
                }

                if (!dbg_ent->valid) {
                    /* FIXME: Entity never released */
                    dbg_ent = sim_ent_alloc(root);
                    sim_ent_enable_prop(dbg_ent, SIM_ENT_PROP_COLLISION_DEBUG);
                    sim_ent_lookup_set(debug_lookup, key, dbg_ent->handle);
                }

                struct phys_collision_debug *dbg = &dbg_ent->collision_debug_data;

                dbg->e0 = e0->handle;
                dbg->e1 = e1->handle;
                dbg->res = collider_res;

                if (constraint) {
                    MEMCPY(dbg->points, constraint->points, sizeof(dbg->points));
                    dbg->num_points = constraint->num_points;
                } else {
                    dbg->num_points = 0;
                }

                dbg->xf0 = e0_xf;
                dbg->xf1 = e1_xf;

                /* Update closest points */
                {
                    struct collider_closest_points_result closest_points_res = collider_closest_points(&e0_collider, &e1_collider, e0_xf, e1_xf);
                    dbg->closest0 = closest_points_res.p0;
                    dbg->closest1 = closest_points_res.p1;
                }
            }
#endif
        }
        space_iter_end(&iter);
    }
    return res;
}

void phys_prepare_contacts(struct phys_ctx *ctx, u64 phys_iteration)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    struct sim_ent_lookup *contact_lookup = &ss->contact_lookup;

    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *constraint_ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(constraint_ent)) continue;
        if (!sim_ent_has_prop(constraint_ent, SIM_ENT_PROP_CONTACT_CONSTRAINT)) continue;

        struct phys_contact_constraint *constraint = &constraint_ent->contact_constraint_data;

        u32 num_points = constraint->num_points;
        struct sim_ent *e0 = sim_ent_from_handle(ss, constraint->e0);
        struct sim_ent *e1 = sim_ent_from_handle(ss, constraint->e1);
        if (constraint->last_phys_iteration >= phys_iteration && num_points > 0 && sim_ent_is_valid_and_active(e0) && sim_ent_is_valid_and_active(e1)) {
            struct v2 normal = constraint->normal;
            struct v2 tangent = v2_perp(normal);

            struct xform e0_xf = sim_ent_get_xform(e0);
            struct xform e1_xf = sim_ent_get_xform(e1);

            /* TODO: Cache this */
            /* Calculate masses */
            f32 inv_m0;
            f32 inv_m1;
            f32 inv_i0;
            f32 inv_i1;
            {
                f32 scale0 = math_fabs(xform_get_determinant(e0_xf));
                f32 scale1 = math_fabs(xform_get_determinant(e1_xf));
                inv_m0 = 1.f / (e0->mass_unscaled * scale0);
                inv_m1 = 1.f / (e1->mass_unscaled * scale1);
                inv_i0 = 1.f / (e0->inertia_unscaled * scale0);
                inv_i1 = 1.f / (e1->inertia_unscaled * scale1);
            }
            constraint->inv_m0 = inv_m0;
            constraint->inv_m1 = inv_m1;
            constraint->inv_i0 = inv_i0;
            constraint->inv_i1 = inv_i1;

            if (sim_ent_has_prop(e0, SIM_ENT_PROP_PHYSICAL_KINEMATIC)) {
                constraint->inv_m0 = 0;
                constraint->inv_i0 = 0;
            }
            if (sim_ent_has_prop(e1, SIM_ENT_PROP_PHYSICAL_KINEMATIC)) {
                constraint->inv_m1 = 0;
                constraint->inv_i1 = 0;
            }

            /* Update / insert returned contacts */
            for (u32 i = 0; i < num_points; ++i) {
                struct phys_contact_point *contact = &constraint->points[i];

                struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, contact->point_local_e0), e0_xf.og);
                struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, contact->point_local_e1), e1_xf.og);

                /* Normal mass */
                {
                    f32 vcp0_wedge = v2_wedge(vcp0, normal);
                    f32 vcp1_wedge = v2_wedge(vcp1, normal);
                    f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge);
                    contact->inv_normal_mass = k > 0.0f ? 1.0f / k : 0.0f;
                }

                /* Tangent mass */
                {
                    f32 vcp0_wedge = v2_wedge(vcp0, tangent);
                    f32 vcp1_wedge = v2_wedge(vcp1, tangent);
                    f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge);
                    contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f;
                }

#if !SIM_PHYSICS_ENABLE_WARM_STARTING
                contact->normal_impulse = 0;
                contact->tangent_impulse = 0;
#endif
            }
        } else {
            /* Mark constraint for removal */
            constraint_ent->contact_constraint_data.num_points = 0;
            sim_ent_disable_prop(constraint_ent, SIM_ENT_PROP_ACTIVE);
            sim_ent_enable_prop(constraint_ent, SIM_ENT_PROP_RELEASE_THIS_TICK);
            /* Remove from lookup */
            struct sim_ent_lookup_key key = sim_ent_lookup_key_from_two_handles(constraint->e0, constraint->e1);
            struct sim_ent_lookup_entry *entry = sim_ent_lookup_get(contact_lookup, key);
            if (entry) {
                sim_ent_lookup_remove(contact_lookup, entry);
            } else {
                ASSERT(false);  /* This should always exist */
            }
        }
    }

#if COLLIDER_DEBUG
    struct sim_ent_lookup *debug_lookup = ctx->debug_lookup;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *dbg_ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(dbg_ent)) continue;
        if (!sim_ent_has_prop(dbg_ent, SIM_ENT_PROP_COLLISION_DEBUG)) continue;

        struct phys_collision_debug *dbg = &dbg_ent->collision_debug_data;
        struct sim_ent *e0 = sim_ent_from_handle(ss, dbg->e0);
        struct sim_ent *e1 = sim_ent_from_handle(ss, dbg->e1);


        if (!sim_ent_is_valid_and_active(e0) || !sim_ent_is_valid_and_active(e1)
            || !(sim_ent_has_prop(e0, SIM_ENT_PROP_PHYSICAL_DYNAMIC) || sim_ent_has_prop(e0, SIM_ENT_PROP_PHYSICAL_KINEMATIC))
            || !(sim_ent_has_prop(e1, SIM_ENT_PROP_PHYSICAL_DYNAMIC) || sim_ent_has_prop(e1, SIM_ENT_PROP_PHYSICAL_KINEMATIC))) {
            /* Mark dbg ent for removal */
            sim_ent_disable_prop(dbg_ent, SIM_ENT_PROP_ACTIVE);
            sim_ent_enable_prop(dbg_ent, SIM_ENT_PROP_RELEASE_THIS_TICK);

            /* Remove from lookup */
            struct sim_ent_lookup_key key = sim_ent_lookup_key_from_two_handles(dbg->e0, dbg->e1);
            struct sim_ent_lookup_entry *entry = sim_ent_lookup_get(debug_lookup, key);

            if (entry) {
                sim_ent_lookup_remove(debug_lookup, entry);
            } else {
                ASSERT(false);  /* This should always exist */
            }
        }
    }
#endif
}

void phys_warm_start_contacts(struct phys_ctx *ctx)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *constraint_ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(constraint_ent)) continue;
        if (!sim_ent_has_prop(constraint_ent, SIM_ENT_PROP_CONTACT_CONSTRAINT)) continue;

        struct phys_contact_constraint *constraint = &constraint_ent->contact_constraint_data;

        u32 num_points = constraint->num_points;
        struct sim_ent *e0 = sim_ent_from_handle(ss, constraint->e0);
        struct sim_ent *e1 = sim_ent_from_handle(ss, constraint->e1);

        if (num_points > 0 && sim_ent_is_valid_and_active(e0) && sim_ent_is_valid_and_active(e1) && !constraint->skip_solve) {
            struct xform e0_xf = sim_ent_get_xform(e0);
            struct xform e1_xf = sim_ent_get_xform(e1);

            f32 inv_m0 = constraint->inv_m0;
            f32 inv_m1 = constraint->inv_m1;
            f32 inv_i0 = constraint->inv_i0;
            f32 inv_i1 = constraint->inv_i1;

            struct v2 v0 = e0->linear_velocity;
            struct v2 v1 = e1->linear_velocity;
            f32 w0 = e0->angular_velocity;
            f32 w1 = e1->angular_velocity;

            /* Warm start */
            struct v2 normal = constraint->normal;
            struct v2 tangent = v2_perp(normal);
            f32 inv_num_points = 1.f / num_points;
            for (u32 i = 0; i < num_points; ++i) {
                struct phys_contact_point *point = &constraint->points[i];
                struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, point->point_local_e0), e0_xf.og);
                struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, point->point_local_e1), e1_xf.og);

                struct v2 impulse = v2_add(v2_mul(normal, point->normal_impulse), v2_mul(tangent, point->tangent_impulse));
                impulse = v2_mul(impulse, inv_num_points);

                v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
                v1 = v2_add(v1, v2_mul(impulse, inv_m1));
                w0 -= v2_wedge(vcp0, impulse) * inv_i0;
                w1 += v2_wedge(vcp1, impulse) * inv_i1;
            }

            sim_ent_set_linear_velocity(e0, v0);
            sim_ent_set_angular_velocity(e0, w0);
            sim_ent_set_linear_velocity(e1, v1);
            sim_ent_set_angular_velocity(e1, w1);
        }
    }
}

void phys_solve_contacts(struct phys_ctx *ctx, f32 dt, b32 apply_bias)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *constraint_ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(constraint_ent)) continue;
        if (!sim_ent_has_prop(constraint_ent, SIM_ENT_PROP_CONTACT_CONSTRAINT)) continue;

        struct phys_contact_constraint *constraint = &constraint_ent->contact_constraint_data;

        struct sim_ent *e0 = sim_ent_from_handle(ss, constraint->e0);
        struct sim_ent *e1 = sim_ent_from_handle(ss, constraint->e1);

        struct v2 v0 = e0->linear_velocity;
        struct v2 v1 = e1->linear_velocity;
        f32 w0 = e0->angular_velocity;
        f32 w1 = e1->angular_velocity;

        u32 num_points = constraint->num_points;
        if (num_points > 0 && sim_ent_is_valid_and_active(e0) && sim_ent_is_valid_and_active(e1) && !constraint->skip_solve) {
            struct xform e0_xf = sim_ent_get_xform(e0);
            struct xform e1_xf = sim_ent_get_xform(e1);

            f32 inv_m0 = constraint->inv_m0;
            f32 inv_m1 = constraint->inv_m1;
            f32 inv_i0 = constraint->inv_i0;
            f32 inv_i1 = constraint->inv_i1;

            /* Normal impulse */
            struct v2 normal = constraint->normal;
            for (u32 point_index = 0; point_index < num_points; ++point_index) {
                struct phys_contact_point *point = &constraint->points[point_index];
                struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0);
                struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1);
                struct v2 vcp0 = v2_sub(p0, e0_xf.og);
                struct v2 vcp1 = v2_sub(p1, e1_xf.og);

                f32 separation = v2_dot(v2_sub(p1, p0), normal) + point->starting_separation;

                f32 velocity_bias = 0.0f;
                f32 mass_scale = 1.0f;
                f32 impulse_scale = 0.0f;

                if (separation > 0.0f) {
                    /* Speculative */
                    velocity_bias = separation / dt;
                } else if (apply_bias) {
                    /* Soft constraint */
                    struct math_spring_result softness = constraint->softness;
                    f32 pushout_velocity = constraint->pushout_velocity;
                    mass_scale = softness.mass_scale;
                    impulse_scale = softness.impulse_scale;
                    velocity_bias = max_f32(softness.bias_rate * separation, -pushout_velocity);
                }

                struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
                struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
                struct v2 vrel = v2_sub(vel0, vel1);

                f32 k = point->inv_normal_mass;

                /* (to be applied along n) */
                f32 vn = v2_dot(vrel, normal);
                f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (point->normal_impulse * impulse_scale);

                f32 old_impulse = point->normal_impulse;
                f32 new_impulse = max_f32(old_impulse + j, 0);
                f32 delta = new_impulse - old_impulse;
                point->normal_impulse = new_impulse;

                struct v2 impulse = v2_mul(normal, delta);
                v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
                v1 = v2_add(v1, v2_mul(impulse, inv_m1));
                w0 -= v2_wedge(vcp0, impulse) * inv_i0;
                w1 += v2_wedge(vcp1, impulse) * inv_i1;
            }

            /* Tangent impulse */
            struct v2 tangent = v2_perp(normal);
            for (u32 point_index = 0; point_index < num_points; ++point_index) {
                struct phys_contact_point *point = &constraint->points[point_index];
                struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, point->point_local_e0), e0_xf.og);
                struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, point->point_local_e1), e1_xf.og);

                struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
                struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
                struct v2 vrel = v2_sub(vel0, vel1);

                f32 k = point->inv_tangent_mass;

                /* (to be applied along t) */
                f32 vt = v2_dot(vrel, tangent);
                f32 j = vt * k;

                f32 max_friction = constraint->friction * point->normal_impulse;
                f32 old_impulse = point->tangent_impulse;
                f32 new_impulse = clamp_f32(old_impulse + j, -max_friction, max_friction);
                f32 delta = new_impulse - old_impulse;
                point->tangent_impulse = new_impulse;

                struct v2 impulse = v2_mul(tangent, delta);
                v0 = v2_sub(v0, v2_mul(impulse, inv_m0));
                v1 = v2_add(v1, v2_mul(impulse, inv_m1));
                w0 -= v2_wedge(vcp0, impulse) * inv_i0;
                w1 += v2_wedge(vcp1, impulse) * inv_i1;
            }

            sim_ent_set_linear_velocity(e0, v0);
            sim_ent_set_angular_velocity(e0, w0);
            sim_ent_set_linear_velocity(e1, v1);
            sim_ent_set_angular_velocity(e1, w1);
        }
    }
}

/* ========================== *
 * Motor joint
 * ========================== */

struct phys_motor_joint phys_motor_joint_from_def(struct phys_motor_joint_def def)
{
    struct phys_motor_joint res = ZI;
    res.e0 = def.e0;
    res.e1 = def.e1;
    res.correction_rate = clamp_f32(def.correction_rate, 0, 1);
    res.max_force = def.max_force;
    res.max_torque = def.max_torque;
    return res;
}

void phys_prepare_motor_joints(struct phys_ctx *ctx)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(joint_ent)) continue;
        if (!sim_ent_has_prop(joint_ent, SIM_ENT_PROP_MOTOR_JOINT)) continue;

        struct phys_motor_joint *joint = &joint_ent->motor_joint_data;

        struct sim_ent *e0 = sim_ent_from_handle(ss, joint->e0);
        struct sim_ent *e1 = sim_ent_from_handle(ss, joint->e1);

        if (sim_ent_is_valid_and_active(e0) && sim_ent_is_valid_and_active(e1)) {
            struct xform e0_xf = sim_ent_get_xform(e0);
            struct xform e1_xf = sim_ent_get_xform(e1);

            /* TODO: Cache this */
            /* Calculate masses */
            f32 inv_m0;
            f32 inv_m1;
            f32 inv_i0;
            f32 inv_i1;
            {
                f32 scale0 = math_fabs(xform_get_determinant(e0_xf));
                f32 scale1 = math_fabs(xform_get_determinant(e1_xf));
                inv_m0 = 1.f / (e0->mass_unscaled * scale0);
                inv_m1 = 1.f / (e1->mass_unscaled * scale1);
                inv_i0 = 1.f / (e0->inertia_unscaled * scale0);
                inv_i1 = 1.f / (e1->inertia_unscaled * scale1);
            }
            joint->inv_m0 = inv_m0;
            joint->inv_m1 = inv_m1;
            joint->inv_i0 = inv_i0;
            joint->inv_i1 = inv_i1;

            joint->point_local_e0 = V2(0, 0);
            joint->point_local_e1 = V2(0, 0);

            struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og);
            struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og);

            struct xform linear_mass_xf;
            linear_mass_xf.bx.x = inv_m0 + inv_m1 + vcp0.y * vcp0.y * inv_i0 + vcp1.y * vcp1.y * inv_i1;
            linear_mass_xf.bx.y = -vcp0.y * vcp0.x * inv_i0 - vcp1.y * vcp1.x * inv_i1;
            linear_mass_xf.by.x = linear_mass_xf.bx.y;
            linear_mass_xf.by.y = inv_m0 + inv_m1 + vcp0.x * vcp0.x * inv_i0 + vcp1.x * vcp1.x * inv_i1;
            joint->linear_mass_xf = xform_invert(linear_mass_xf);

            joint->angular_mass = 1.f / (inv_i0 + inv_i1);

#if !SIM_PHYSICS_ENABLE_WARM_STARTING
            joint->linear_impulse = V2(0, 0);
            joint->angular_impulse = 0;
#endif
        } else {
            /* Mark joint for removal */
            sim_ent_disable_prop(joint_ent, SIM_ENT_PROP_ACTIVE);
            sim_ent_enable_prop(joint_ent, SIM_ENT_PROP_RELEASE_THIS_TICK);
        }
    }
}

void phys_warm_start_motor_joints(struct phys_ctx *ctx)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(joint_ent)) continue;
        if (!sim_ent_has_prop(joint_ent, SIM_ENT_PROP_MOTOR_JOINT)) continue;

        struct phys_motor_joint *joint = &joint_ent->motor_joint_data;

        struct sim_ent *e0 = sim_ent_from_handle(ss, joint->e0);
        struct sim_ent *e1 = sim_ent_from_handle(ss, joint->e1);

        struct xform e0_xf = sim_ent_get_xform(e0);
        struct xform e1_xf = sim_ent_get_xform(e1);

        f32 inv_m0 = joint->inv_m0;
        f32 inv_m1 = joint->inv_m1;
        f32 inv_i0 = joint->inv_i0;
        f32 inv_i1 = joint->inv_i1;

        struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og);
        struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og);

        sim_ent_set_linear_velocity(e0, v2_sub(e0->linear_velocity, v2_mul(joint->linear_impulse, inv_m0)));
        sim_ent_set_linear_velocity(e1, v2_add(e1->linear_velocity, v2_mul(joint->linear_impulse, inv_m1)));
        e0->angular_velocity -= (v2_wedge(vcp0, joint->linear_impulse) + joint->angular_impulse) * inv_i0;
        e1->angular_velocity += (v2_wedge(vcp1, joint->linear_impulse) + joint->angular_impulse) * inv_i1;
    }
}

void phys_solve_motor_joints(struct phys_ctx *ctx, f32 dt)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(joint_ent)) continue;
        if (!sim_ent_has_prop(joint_ent, SIM_ENT_PROP_MOTOR_JOINT)) continue;

        struct phys_motor_joint *joint = &joint_ent->motor_joint_data;

        struct sim_ent *e0 = sim_ent_from_handle(ss, joint->e0);
        struct sim_ent *e1 = sim_ent_from_handle(ss, joint->e1);

        struct xform e0_xf = sim_ent_get_xform(e0);
        struct xform e1_xf = sim_ent_get_xform(e1);

        f32 inv_m0 = joint->inv_m0;
        f32 inv_m1 = joint->inv_m1;
        f32 inv_i0 = joint->inv_i0;
        f32 inv_i1 = joint->inv_i1;

        struct v2 v0 = e0->linear_velocity;
        struct v2 v1 = e1->linear_velocity;
        f32 w0 = e0->angular_velocity;
        f32 w1 = e1->angular_velocity;

        f32 correction_rate = joint->correction_rate;
        f32 inv_dt = 1.f / dt;

        /* Angular constraint */
        {
            f32 max_impulse = joint->max_torque * dt;
            f32 angular_separation = math_unwind_angle(xform_get_rotation(e1_xf) - xform_get_rotation(e0_xf));

            f32 angular_bias = angular_separation * correction_rate * inv_dt;

            f32 old_impulse = joint->angular_impulse;
            f32 new_impulse = clamp_f32(old_impulse + (-joint->angular_mass * (w1 - w0 + angular_bias)), -max_impulse, max_impulse);
            joint->angular_impulse = new_impulse;

            f32 delta = new_impulse - old_impulse;
            w0 -= delta * inv_i0;
            w1 += delta * inv_i1;
        }

        /* Linear constraint */
        {
            struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og);
            struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og);

            f32 max_impulse = joint->max_force * dt;

            struct v2 linear_separation = v2_sub(v2_add(e1_xf.og, vcp1), v2_add(e0_xf.og, vcp0));
            struct v2 linear_bias = v2_mul(linear_separation, correction_rate * inv_dt);

            struct v2 vrel = v2_sub(v2_add(v1, v2_perp_mul(vcp1, w1)), v2_add(v0, v2_perp_mul(vcp0, w0)));
            struct v2 impulse = v2_neg(xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vrel, linear_bias)));

            struct v2 old_impulse = joint->linear_impulse;
            struct v2 new_impulse = v2_clamp_len(v2_add(old_impulse, impulse), max_impulse);
            joint->linear_impulse = new_impulse;

            struct v2 delta = v2_sub(new_impulse, old_impulse);
            v0 = v2_sub(v0, v2_mul(delta, inv_m0));
            v1 = v2_add(v1, v2_mul(delta, inv_m1));
            w0 -= v2_wedge(vcp0, delta) * inv_i0;
            w1 += v2_wedge(vcp1, delta) * inv_i1;
        }

        sim_ent_set_linear_velocity(e0, v0);
        sim_ent_set_angular_velocity(e0, w0);
        sim_ent_set_linear_velocity(e1, v1);
        sim_ent_set_angular_velocity(e1, w1);
    }
}

/* ========================== *
 * Mouse joint
 * ========================== */

struct phys_mouse_joint phys_mouse_joint_from_def(struct phys_mouse_joint_def def)
{
    struct phys_mouse_joint res = ZI;
    res.target = def.target;
    res.point_local_start = def.point_local_start;
    res.point_local_end = def.point_local_end;
    res.linear_softness = G.mouse_joint_linear_softness;
    res.angular_softness = G.mouse_joint_angular_softness;
    res.max_force = def.max_force;
    return res;
}

void phys_prepare_mouse_joints(struct phys_ctx *ctx)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(joint_ent)) continue;
        if (!sim_ent_has_prop(joint_ent, SIM_ENT_PROP_MOUSE_JOINT)) continue;

        struct phys_mouse_joint *joint = &joint_ent->mouse_joint_data;
        struct sim_ent *ent = sim_ent_from_handle(ss, joint->target);
        if (sim_ent_is_valid_and_active(ent)) {
            struct xform xf = sim_ent_get_xform(ent);

            /* TODO: Cache this */
            /* Calculate masses */
            f32 inv_m;
            f32 inv_i;
            {
                f32 scale = math_fabs(xform_get_determinant(xf));
                inv_m = 1.f / (ent->mass_unscaled * scale);
                inv_i = 1.f / (ent->inertia_unscaled * scale);
            }
            joint->inv_m = inv_m;
            joint->inv_i = inv_i;

            struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_start), xf.og);

            struct xform linear_mass_xf;
            linear_mass_xf.bx.x = inv_m + inv_i * vcp.y * vcp.y;
            linear_mass_xf.bx.y = -inv_i * vcp.x * vcp.y;
            linear_mass_xf.by.x = linear_mass_xf.bx.y;
            linear_mass_xf.by.y = inv_m + inv_i * vcp.x * vcp.x;
            joint->linear_mass_xf = xform_invert(linear_mass_xf);

#if !SIM_PHYSICS_ENABLE_WARM_STARTING
            joint->linear_impulse = V2(0, 0);
            joint->angular_impulse = 0;
#endif
        } else {
            /* Mark joint for removal */
            sim_ent_disable_prop(joint_ent, SIM_ENT_PROP_ACTIVE);
            sim_ent_enable_prop(joint_ent, SIM_ENT_PROP_RELEASE_THIS_TICK);
        }
    }
}

void phys_warm_start_mouse_joints(struct phys_ctx *ctx)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(joint_ent)) continue;
        if (!sim_ent_has_prop(joint_ent, SIM_ENT_PROP_MOUSE_JOINT)) continue;

        struct phys_mouse_joint *joint = &joint_ent->mouse_joint_data;
        struct sim_ent *ent = sim_ent_from_handle(ss, joint->target);
        if (sim_ent_is_valid_and_active(ent)) {
            f32 inv_m = joint->inv_m;
            f32 inv_i = joint->inv_i;
            struct xform xf = sim_ent_get_xform(ent);
            struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_start), xf.og);
            sim_ent_set_linear_velocity(ent, v2_add(ent->linear_velocity, v2_mul(joint->linear_impulse, inv_m)));
            ent->angular_velocity += (v2_wedge(vcp, joint->linear_impulse) + joint->angular_impulse) * inv_i;
        }
    }
}

void phys_solve_mouse_joints(struct phys_ctx *ctx, f32 dt)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(joint_ent)) continue;
        if (!sim_ent_has_prop(joint_ent, SIM_ENT_PROP_MOUSE_JOINT)) continue;

        struct phys_mouse_joint *joint = &joint_ent->mouse_joint_data;
        struct sim_ent *ent = sim_ent_from_handle(ss, joint->target);
        if (sim_ent_is_valid_and_active(ent)) {
            struct v2 v = ent->linear_velocity;
            f32 w = ent->angular_velocity;

            f32 inv_m = joint->inv_m;
            f32 inv_i = joint->inv_i;

            /* Angular impulse */
            {
                struct math_spring_result softness = joint->angular_softness;
                f32 mass_scale = softness.mass_scale;
                f32 impulse_scale = softness.impulse_scale;
                f32 impulse = mass_scale * (-w / inv_i) - impulse_scale * joint->angular_impulse;
                joint->angular_impulse += impulse;
                w += impulse * inv_i;
            }

            /* Linear impulse */
            {
                f32 max_impulse = joint->max_force / dt;

                struct xform xf = sim_ent_get_xform(ent);

                struct v2 point_start = xform_mul_v2(xf, joint->point_local_start);
                struct v2 point_end = xform_mul_v2(xf, joint->point_local_end);

                struct v2 vcp = v2_sub(point_start, xf.og);
                struct v2 separation = v2_sub(point_start, point_end);

                struct math_spring_result softness = joint->linear_softness;
                struct v2 bias = v2_mul(separation, softness.bias_rate);
                f32 mass_scale = softness.mass_scale;
                f32 impulse_scale = softness.impulse_scale;

                struct v2 vel = v2_add(v, v2_perp_mul(vcp, w));
                struct v2 b = xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vel, bias));

                struct v2 impulse;
                impulse.x = -mass_scale * b.x - impulse_scale * joint->linear_impulse.x;
                impulse.y = -mass_scale * b.y - impulse_scale * joint->linear_impulse.y;

                struct v2 old_impulse = joint->linear_impulse;
                joint->linear_impulse.x += impulse.x;
                joint->linear_impulse.y += impulse.y;

                joint->linear_impulse = v2_clamp_len(joint->linear_impulse, max_impulse);

                impulse.x = joint->linear_impulse.x - old_impulse.x;
                impulse.y = joint->linear_impulse.y - old_impulse.y;

                v = v2_add(v, v2_mul(impulse, inv_m));
                w += v2_wedge(vcp, impulse) * inv_i;
            }

            sim_ent_set_linear_velocity(ent, v);
            sim_ent_set_angular_velocity(ent, w);
        }
    }
}

/* ========================== *
 * Integration
 * ========================== */

INTERNAL struct xform get_derived_xform(struct sim_ent *ent, f32 dt)
{
    struct xform xf = sim_ent_get_xform(ent);

    struct v2 step_linear_velocity = v2_mul(ent->linear_velocity, dt);
    f32 step_angular_velocity = ent->angular_velocity * dt;

    xf.og = v2_add(xf.og, step_linear_velocity);
    xf = xform_basis_rotated_world(xf, step_angular_velocity);
    return xf;
}

void phys_integrate_forces(struct phys_ctx *ctx, f32 dt)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(ent)) continue;

        b32 is_dynamic = sim_ent_has_prop(ent, SIM_ENT_PROP_PHYSICAL_DYNAMIC);
        b32 is_kinematic = sim_ent_has_prop(ent, SIM_ENT_PROP_PHYSICAL_KINEMATIC);
        if (is_dynamic || is_kinematic) {
            struct v2 linear_velocity = ent->linear_velocity;
            f32 angular_velocity = ent->angular_velocity;
            f32 linear_damping_factor = max_f32(1.0f - (ent->linear_damping * dt), 0);
            f32 angular_damping_factor = max_f32(1.0f - (ent->angular_damping * dt), 0);

            /* Integrate forces */
            if (is_dynamic) {
                struct xform xf = sim_ent_get_xform(ent);
                f32 det_abs = math_fabs(xform_get_determinant(xf));
                f32 mass = ent->mass_unscaled * det_abs;
                f32 inertia = ent->inertia_unscaled * det_abs;
                struct v2 force_accel = v2_mul(v2_div(ent->force, mass), dt);
                f32 torque_accel = (ent->torque / inertia) * dt;
                linear_velocity = v2_add(linear_velocity, force_accel);
                angular_velocity += torque_accel;
            }

            /* Apply damping */
            linear_velocity = v2_mul(linear_velocity, linear_damping_factor);
            angular_velocity *= angular_damping_factor;

            /* Update entity */
            sim_ent_set_linear_velocity(ent, linear_velocity);
            sim_ent_set_angular_velocity(ent, angular_velocity);
            ent->force = V2(0, 0);
            ent->torque = 0;
        }

    }
}

void phys_integrate_velocities(struct phys_ctx *ctx, f32 dt)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    struct space *space = ss->space;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(ent)) continue;
        if (!sim_ent_has_prop(ent, SIM_ENT_PROP_PHYSICAL_DYNAMIC) && !sim_ent_has_prop(ent, SIM_ENT_PROP_PHYSICAL_KINEMATIC)) continue;

        struct xform xf = get_derived_xform(ent, dt);
        sim_ent_set_xform(ent, xf);

        struct space_entry *space_entry = space_entry_from_handle(space, ent->space_handle);
        if (space_entry->valid) {
            space_entry_update_aabb(space_entry, collider_aabb_from_collider(&ent->local_collider, xf));
        }
    }
}

/* ========================== *
 * Earliest time of impact
 * ========================== */

f32 phys_determine_earliest_toi_for_bullets(struct phys_ctx *ctx, f32 step_dt, f32 tolerance, u32 max_iterations)
{
    __prof;
    struct sim_snapshot *ss = ctx->ss;
    struct space *space = ss->space;
    f32 smallest_t = 1;

    for (u64 e0_index = 0; e0_index < ss->num_ents_reserved; ++e0_index) {
        struct sim_ent *e0 = &ss->ents[e0_index];
        if (!sim_ent_is_valid_and_active(e0)) continue;
        if (!(sim_ent_has_prop(e0, SIM_ENT_PROP_PHYSICAL_DYNAMIC) || sim_ent_has_prop(e0, SIM_ENT_PROP_PHYSICAL_KINEMATIC))) continue;
        if (!sim_ent_has_prop(e0, SIM_ENT_PROP_BULLET)) continue;
        if (e0->local_collider.count <= 0) continue;

        struct collider_shape e0_collider = e0->local_collider;
        struct xform e0_xf_t0 = sim_ent_get_xform(e0);
        struct xform e0_xf_t1 = get_derived_xform(e0, step_dt);

        /* TODO: Use swept aabb rather than combined aabb. This should prevent spikes from bullets returning false positive TOIs with irrelevant entities. */
        struct aabb aabb_t0 = collider_aabb_from_collider(&e0_collider, e0_xf_t0);
        struct aabb aabb_t1 = collider_aabb_from_collider(&e0_collider, e0_xf_t1);
        struct aabb combined_aabb = collider_aabb_from_combined_aabb(aabb_t0, aabb_t1);

        struct space_iter iter = space_iter_begin_aabb(space, combined_aabb);
        struct space_entry *entry;
        while ((entry = space_iter_next(&iter))) {
            struct sim_ent *e1 = sim_ent_from_handle(ss, entry->ent);
            if (e1 == e0) continue;
            if (!sim_ent_is_valid_and_active(e1)) continue;
            if (!(sim_ent_has_prop(e1, SIM_ENT_PROP_PHYSICAL_DYNAMIC) || sim_ent_has_prop(e1, SIM_ENT_PROP_PHYSICAL_KINEMATIC))) continue;
            if (e1->local_collider.count <= 0) continue;

            struct collider_shape e1_collider = e1->local_collider;
            struct xform e1_xf_t0 = sim_ent_get_xform(e1);
            struct xform e1_xf_t1 = get_derived_xform(e1, step_dt);

            f32 t = collider_time_of_impact(&e0_collider, &e1_collider, e0_xf_t0, e1_xf_t0, e0_xf_t1, e1_xf_t1, tolerance, max_iterations);
            if (t != 0 && t < smallest_t) {
                smallest_t = t;
            }
        }
        space_iter_end(&iter);
    }

    return smallest_t;
}

/* ========================== *
 * Space
 * ========================== */

void phys_update_aabbs(struct phys_ctx *ctx)
{
    struct sim_snapshot *ss = ctx->ss;
    struct space *space = ss->space;
    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
        struct sim_ent *ent = &ss->ents[sim_ent_index];
        if (!sim_ent_is_valid_and_active(ent)) continue;
        if (ent->local_collider.count <= 0) continue;

        struct xform xf = sim_ent_get_xform(ent);
        struct space_entry *space_entry = space_entry_from_handle(space, ent->space_handle);
        if (!space_entry->valid) {
            space_entry = space_entry_alloc(space, ent->handle);
            ent->space_handle = space_entry->handle;
        }
        space_entry_update_aabb(space_entry, collider_aabb_from_collider(&ent->local_collider, xf));
    }
}

/* ========================== *
 * Step
 * ========================== */

/* Returns phys iteration to be fed into next step. Supplied iteration must be > 0. */
void phys_step(struct phys_ctx *ctx, f32 timestep)
{
    __prof;
    phys_integrate_forces(ctx, timestep);
    u64 phys_iteration = ctx->ss->phys_iteration;

    phys_update_aabbs(ctx);

    f32 remaining_dt = timestep;
    while (remaining_dt > 0) {
        __profscope(step_part);
        ++phys_iteration;
        struct temp_arena scratch = scratch_begin_no_conflict();

        /* TOI */
        f32 step_dt = remaining_dt;
        {
#if SIM_PHYSICS_ENABLE_TOI
            const f32 min_toi = 0.000001f;
            const f32 tolerance = 0.0001f;
            const u32 max_iterations = 16;
            f32 earliest_toi = max_f32(phys_determine_earliest_toi_for_bullets(ctx, step_dt, tolerance, max_iterations), min_toi);
            step_dt = remaining_dt * earliest_toi;
#else
            (UNUSED)phys_determine_earliest_toi_for_bullets;
#endif
        }
        remaining_dt -= step_dt;

        struct phys_collision_data_array collision_data = phys_create_and_update_contacts(scratch.arena, ctx, timestep - remaining_dt, phys_iteration);

        phys_prepare_contacts(ctx, phys_iteration);
        phys_prepare_motor_joints(ctx);
        phys_prepare_mouse_joints(ctx);

        if (ctx->pre_solve_callback) {
            __profscope(pre_solve_callback);
            ctx->pre_solve_callback(collision_data, ctx->ss);
        }

        f32 substep_dt = step_dt / SIM_PHYSICS_SUBSTEPS;
        for (u32 i = 0; i < SIM_PHYSICS_SUBSTEPS; ++i) {
            __profscope(substep);

            /* Warm start */
#if SIM_PHYSICS_ENABLE_WARM_STARTING
            phys_warm_start_contacts(ctx);
            phys_warm_start_motor_joints(ctx);
            phys_warm_start_mouse_joints(ctx);
#endif

            /* Solve */
#if SIM_PHYSICS_ENABLE_COLLISION
            phys_solve_contacts(ctx, substep_dt, true);
#endif
            phys_solve_motor_joints(ctx, substep_dt);
            phys_solve_mouse_joints(ctx, substep_dt);

            /* Integrate */
            phys_integrate_velocities(ctx, substep_dt);

            /* Relaxation solve */
#if SIM_PHYSICS_ENABLE_COLLISION && SIM_PHYSICS_ENABLE_RELAXATION
            phys_solve_contacts(ctx, substep_dt, false);
#endif
        }

        if (ctx->post_solve_callback) {
            __profscope(post_solve_callback);
            ctx->post_solve_callback(collision_data, ctx->ss);
        }

        scratch_end(scratch);
    }

    ctx->ss->phys_iteration = phys_iteration;
}