#include "game.h"
#include "sys.h"
#include "util.h"
#include "world.h"
#include "sprite.h"
#include "sound.h"
#include "mixer.h"
#include "math.h"
#include "scratch.h"
#include "atomic.h"
#include "app.h"
#include "log.h"
#include "gjk.h"

GLOBAL struct {
    struct atomic_i32 game_thread_shutdown;
    struct sys_thread game_thread;

    b32 paused;

    /* Game thread input */
    struct sys_mutex game_cmds_mutex;
    struct arena game_cmds_arena;

    /* Ticks */
    struct sys_mutex prev_tick_mutex;
    struct atomic_u64 prev_tick_id;
    struct world prev_tick;
    struct world tick;
} G = { 0 }, DEBUG_ALIAS(G, G_game);

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

INTERNAL APP_EXIT_CALLBACK_FUNC_DEF(game_shutdown);
INTERNAL SYS_THREAD_ENTRY_POINT_FUNC_DEF(game_thread_entry_point, arg);

struct game_startup_receipt game_startup(struct mixer_startup_receipt *mixer_sr,
                                         struct sprite_startup_receipt *sheet_sr,
                                         struct sound_startup_receipt *sound_sr)
{
    (UNUSED)mixer_sr;
    (UNUSED)sheet_sr;
    (UNUSED)sound_sr;

    /* Initialize game cmd storage */
    G.game_cmds_mutex = sys_mutex_alloc();
    G.game_cmds_arena = arena_alloc(GIGABYTE(64));

    /* Initialize ticks */
    world_alloc(&G.tick);
    world_alloc(&G.prev_tick);
    /* FIXME: Make the world struct itself readonly as well */
    arena_set_readonly(&G.prev_tick.entity_store->arena);
    G.prev_tick_mutex = sys_mutex_alloc();

    G.tick.timescale = GAME_TIMESCALE;
    G.game_thread = sys_thread_alloc(&game_thread_entry_point, NULL, STR("[P2] Game thread"));
    app_register_exit_callback(&game_shutdown);

    return (struct game_startup_receipt) { 0 };
}

INTERNAL APP_EXIT_CALLBACK_FUNC_DEF(game_shutdown)
{
    __prof;
    atomic_i32_eval_exchange(&G.game_thread_shutdown, true);
    sys_thread_wait_release(&G.game_thread);
}

/* ========================== *
 * Game cmd
 * ========================== */

INTERNAL void push_cmds(struct game_cmd_array cmd_array)
{
    struct sys_lock lock = sys_mutex_lock_e(&G.game_cmds_mutex);
    struct game_cmd *cmds = arena_push_array(&G.game_cmds_arena, struct game_cmd, cmd_array.count);
    MEMCPY(cmds, cmd_array.cmds, cmd_array.count * sizeof(*cmds));
    sys_mutex_unlock(&lock);
}

INTERNAL struct game_cmd_array pop_cmds(struct arena *arena)
{
    struct game_cmd_array array = { 0 };
    if (G.game_cmds_arena.pos > 0) {
        struct sys_lock lock = sys_mutex_lock_e(&G.game_cmds_mutex);
        struct buffer game_cmds_buff = arena_to_buffer(&G.game_cmds_arena);
        arena_align(arena, alignof(struct game_cmd));
        array.cmds = (struct game_cmd *)arena_push_array(arena, u8, game_cmds_buff.size);
        array.count = game_cmds_buff.size / sizeof(struct game_cmd);
        MEMCPY(array.cmds, game_cmds_buff.data, game_cmds_buff.size);
        arena_reset(&G.game_cmds_arena);
        sys_mutex_unlock(&lock);
    }
    return array;
}

/* ========================== *
 * Activate
 * ========================== */

INTERNAL void activate_now(struct entity *ent)
{
    entity_enable_prop(ent, ENTITY_PROP_ACTIVE);
    ent->activation_tick = G.tick.tick_id;
    ++ent->continuity_gen;
}

/* ========================== *
 * Test
 * ========================== */

/* TODO: Remove this */

INTERNAL void spawn_test_entities(void)
{
    struct entity *root = entity_from_handle(G.tick.entity_store, G.tick.entity_store->root);

    /* Player */
    struct entity *player_ent;
    {
        struct v2 pos = V2(0.25, -3);
        //struct v2 pos = V2(1.1230469346046448864129274625156, -1);  /* Touching right side of box */
        //struct v2 pos = V2(1.1230469346046448864129274625156 - 0.0001, -1);  /* Touching right side of box */
        //struct v2 pos = V2(0.374142020941, -0.246118023992);  /* Touching glitch spot */
        struct v2 size = V2(1, 1);
        //f32 r = PI / 4;
        //f32 r = PI / 3;
        //f32 r = PI / 2;
        f32 r = 0;
        f32 skew = 0;

        struct entity *e = entity_alloc(root);

        struct xform xf = XFORM_TRS(.t = pos, .r = r, .s = size);
        xf = xform_skewed_to(xf, skew);
        entity_set_xform(e, xf);

        e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase"));
        //e->sprite_span_name = STR("idle.unarmed");
        //e->sprite_span_name = STR("idle.one_handed");
        e->sprite_span_name = STR("idle.two_handed");

        entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED);
        e->control_force = 4500;
        //e->control_force = 1200;
        e->control_torque = 10;
        e->control.focus = V2(0, -1);

        entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
        e->mass_unscaled = 100;
        e->inertia_unscaled = 25;
        e->linear_ground_friction = 1000;
        e->angular_ground_friction = 100;

        //entity_enable_prop(e, ENTITY_PROP_TEST);

        player_ent = e;
    }

    /* Weapon */
    {
#if 0
        struct v2 pos = V2(1, 0);
        struct v2 size = V2(1, 1);
        f32 r = PI / 4;
        struct entity *e = entity_alloc(root);
#else
        struct entity *e = entity_alloc(player_ent);
#endif

        e->sprite = sprite_tag_from_path(STR("res/graphics/gun.ase"));

        entity_enable_prop(e, ENTITY_PROP_ATTACHED);
        e->attach_slice = STR("attach.wep");

        entity_enable_prop(e, ENTITY_PROP_WEAPON);
        e->trigger_delay = 1.0 / 10.0;

        player_ent->equipped = e->handle;
    }

    /* Box */
    {
        //struct v2 pos = V2(0.5, -0.5);
        struct v2 pos = V2(0.5, -1);
        struct v2 size = V2(1, 1);
        f32 rot = 0;
        struct entity *e = entity_alloc(root);

        e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase"));

        entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
#if 0
        e->mass_unscaled = 1000;
        e->inertia_unscaled = 1000;
#else
        e->mass_unscaled = F32_INFINITY;
        e->inertia_unscaled = F32_INFINITY;
#endif
        e->linear_ground_friction = 10000;
        e->angular_ground_friction = 10000;

        entity_set_xform(e, XFORM_TRS(.t = pos, .s = size, .r = rot));
    }

    /* Camera */
    {
        struct entity *e = entity_alloc(root);
        entity_set_xform(e, XFORM_IDENT);

        entity_enable_prop(e, ENTITY_PROP_CAMERA);
        entity_enable_prop(e, ENTITY_PROP_CAMERA_ACTIVE);
        e->camera_follow = player_ent->handle;

        f32 width = (f32)DEFAULT_CAMERA_WIDTH;
        f32 height = (f32)DEFAULT_CAMERA_HEIGHT;
        e->camera_quad_xform = XFORM_TRS(.s = V2(width, height));
    }
}

/* ========================== *
 * Update
 * ========================== */

INTERNAL void publish_game_tick(void)
{
    __prof;
    struct sys_lock lock = sys_mutex_lock_e(&G.prev_tick_mutex);
    arena_set_readwrite(&G.prev_tick.entity_store->arena);
    {
        world_copy_replace(&G.prev_tick, &G.tick);
    }
    arena_set_readonly(&G.prev_tick.entity_store->arena);
    atomic_u64_eval_exchange(&G.prev_tick_id, G.prev_tick.tick_id);
    sys_mutex_unlock(&lock);
}

INTERNAL void game_update(struct game_cmd_array game_cmds)
{
    __prof;

    struct temp_arena scratch = scratch_begin_no_conflict();

    /* ========================== *
     * Begin frame
     * ========================== */

    ++G.tick.tick_id;
    G.tick.tick_ts = sys_timestamp();
    G.tick.dt = max_f64(0.0, (1.0 / GAME_FPS) * G.tick.timescale);
    G.tick.time += G.tick.dt;

    f64 dt = G.tick.dt;
    f64 time = G.tick.time;
    struct entity_store *store = G.tick.entity_store;
    struct entity *root = entity_from_handle(store, store->root);
    struct sprite_scope *sprite_frame_scope = sprite_scope_begin();

    (UNUSED)dt;
    (UNUSED)time;

    /* ========================== *
     * Spawn test entities
     * ========================== */

    /* TODO: remove this (testing) */
    /* Initialize entities */
    static b32 run = 0;
    if (!run) {
        run = 1;
        spawn_test_entities();
    }

    /* ========================== *
     * Process global game cmds
     * ========================== */

    for (u64 cmd_index = 0; cmd_index < game_cmds.count; ++cmd_index) {
        struct game_cmd cmd = game_cmds.cmds[cmd_index];

        switch (cmd.kind) {
            /* Clear level */
            case GAME_CMD_KIND_CLEAR_ALL: {
                logf_info("Clearing level");
                entity_store_reset(store);
            } break;

            /* Spawn test */
            case GAME_CMD_KIND_SPAWN_TEST: {
                logf_info("Spawning (test)");
                spawn_test_entities();
            } break;

            default: break;
        };
    }

    /* ========================== *
     * Activate entities
     * ========================== */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!ent->valid) continue;

        if (!entity_has_prop(ent, ENTITY_PROP_ACTIVE)) {
            u64 atick = ent->activation_tick;
            if (atick != 0 || G.tick.tick_id >= atick) {
                activate_now(ent);
            }
        }
    }

    /* ========================== *
     * Reset triggered entities
     * ========================== */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;

        if (entity_has_prop(ent, ENTITY_PROP_TRIGGER_NEXT_TICK)) {
            entity_disable_prop(ent, ENTITY_PROP_TRIGGER_NEXT_TICK);
            entity_enable_prop(ent, ENTITY_PROP_TRIGGERED_THIS_TICK);
        } else if (entity_has_prop(ent, ENTITY_PROP_TRIGGERED_THIS_TICK)) {
            entity_disable_prop(ent, ENTITY_PROP_TRIGGERED_THIS_TICK);
        }
    }

    /* ========================== *
     * Update animations from sprite
     * ========================== */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
        if (sprite_tag_is_nil(ent->sprite)) continue;

        /* Update animation */

        {
            struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite);
            struct sprite_sheet_span span = sprite_sheet_get_span(sheet, ent->sprite_span_name);

            f64 time_in_frame = ent->animation_time_in_frame + G.tick.dt;
            u64 frame_index = ent->animation_frame;
            if (frame_index < span.start || frame_index > span.end) {
                frame_index = span.start;
            }

            struct sprite_sheet_frame frame = sprite_sheet_get_frame(sheet, frame_index);
            while (time_in_frame > frame.duration) {
                time_in_frame -= frame.duration;
                ++frame_index;
                if (frame_index > span.end) {
                    /* Loop animation */
                    frame_index = span.start;
                }
                frame = sprite_sheet_get_frame(sheet, frame_index);
            }

            ent->animation_time_in_frame = time_in_frame;
            ent->animation_frame = frame_index;
        }

        /* Update sprite local xform */

        struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite);
        struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("pivot"), ent->animation_frame);
        struct v2 sprite_size = v2_div(sheet->frame_size, (f32)PIXELS_PER_UNIT);

        struct v2 dir = v2_mul_v2(sprite_size, slice.dir);
        f32 rot = v2_angle(dir) + PI / 2;

        struct xform xf = XFORM_IDENT;
        xf = xform_rotated(xf, -rot);
        xf = xform_scaled(xf, sprite_size);
        xf = xform_translated(xf, v2_neg(slice.center));
        ent->sprite_local_xform = xf;
    }

    /* ========================== *
     * Update attachments
     * ========================== */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
        if (!entity_has_prop(ent, ENTITY_PROP_ATTACHED)) continue;

        struct entity *parent = entity_from_handle(store, ent->parent);
        struct sprite_tag parent_sprite = parent->sprite;
        struct sprite_sheet *parent_sheet = sprite_sheet_from_tag_await(sprite_frame_scope, parent_sprite);

        struct xform parent_sprite_xf = parent->sprite_local_xform;

        struct sprite_sheet_slice attach_slice = sprite_sheet_get_slice(parent_sheet, ent->attach_slice, parent->animation_frame);
        struct v2 attach_pos = xform_mul_v2(parent_sprite_xf, attach_slice.center);
        struct v2 attach_dir = xform_basis_mul_v2(parent_sprite_xf, attach_slice.dir);

        struct xform xf = entity_get_local_xform(ent);
        xf.og = attach_pos;
        xf = xform_rotated_to(xf, v2_angle(attach_dir) + PI / 2);
        entity_set_local_xform(ent, xf);
    }

    /* ========================== *
     * Update control from player cmds
     * ========================== */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;

        if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
            /* Process cmds */
            struct v2 move = ent->control.move;
            struct v2 focus = ent->control.focus;
            b32 firing = entity_has_prop(ent, ENTITY_PROP_TRIGGERING_EQUIPPED);

            for (u64 i = 0; i < game_cmds.count; ++i) {
                struct game_cmd cmd = game_cmds.cmds[i];
                b32 start = cmd.state == GAME_CMD_STATE_START;
                b32 stop = cmd.state == GAME_CMD_STATE_STOP;
                (UNUSED)start;
                (UNUSED)stop;

                /* TODO: Combine movement from multiple inputs? E.G. a sudden
                 * start and immediate stop cmd should still move the player a
                 * tad. */
                switch (cmd.kind) {
                    case GAME_CMD_KIND_PLAYER_MOVE:
                    {
                        move = cmd.move_dir;
                        focus = v2_sub(cmd.aim_pos, entity_get_xform(ent).og);
                    } break;

                    case GAME_CMD_KIND_PLAYER_FIRE:
                    {
                        if (start) {
                            firing = true;
                        } else if (stop) {
                            firing = false;
                        }
                    } break;

                    default: break;
                }
            }

            /* Movement */
            if (v2_len_sq(move) > 1) {
                /* Cap movement vector magnitude at 1 */
                move = v2_norm(move);
            }
            ent->control.move = move;
            ent->control.focus = focus;

            /* Firing */
            if (firing) {
                entity_enable_prop(ent, ENTITY_PROP_TRIGGERING_EQUIPPED);
            } else {
                entity_disable_prop(ent, ENTITY_PROP_TRIGGERING_EQUIPPED);
            }
        }
    }

    /* ========================== *
     * Test
     * ========================== */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
        if (!entity_has_prop(ent, ENTITY_PROP_TEST)) continue;

#if 0
        if (!ent->test_initialized) {
            ent->test_initialized = true;
            ent->test_start_local_xform = entity_get_local_xform(ent);
            ent->test_start_sprite_xform = ent->sprite_local_xform;
        }

        f32 t = (f32)time;
        struct v2 og = v2_mul(V2(math_cos(t), math_sin(t)), 3);
        f32 r = t * 3;
        struct v2 s = V2(1 + (math_fabs(math_sin(t * 5)) * 3), 1);
        f32 skew = t * PI / 10;
        (UNUSED)og;
        (UNUSED)r;
        (UNUSED)s;

        og = v2_add(og, ent->test_start_local_xform.og);
        r += xform_get_rotation(ent->test_start_local_xform);
        s = v2_add(s, xform_get_scale(ent->test_start_local_xform));
        skew += xform_get_skew(ent->test_start_local_xform);

        struct xform xf = entity_get_local_xform(ent);
        xf.og = og;
        xf = xform_rotated_to(xf, r);
        xf = xform_scaled_to(xf, s);
        xf = xform_skewed_to(xf, skew);
        entity_set_local_xform(ent, xf);
#else
        f32 t = (f32)time;
        struct v2 og = v2_mul(V2(math_cos(t), math_sin(t)), 3);
        f32 rot = t * PI / 3;
        struct v2 scale = V2(1 + (math_fabs(math_sin(t * 5)) * 3), 1);
        f32 skew = t * PI / 10;
        (UNUSED)og;
        (UNUSED)rot;
        (UNUSED)scale;
        (UNUSED)skew;

        struct xform xf = entity_get_local_xform(ent);
        //xf = xform_rotated_to(xf, rot);
        //xf = xform_scaled_to(xf, scale);
        xf = xform_skewed_to(xf, skew);
        entity_set_local_xform(ent, xf);
#endif
    }

    /* ========================== *
     * Trigger equipped
     * ========================== */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;

        if (entity_has_prop(ent, ENTITY_PROP_TRIGGERING_EQUIPPED)) {
            struct entity *eq = entity_from_handle(store, ent->equipped);
            if (eq->valid) {
                entity_enable_prop(eq, ENTITY_PROP_TRIGGERED_THIS_TICK);
            }
        }
    }

    /* ========================== *
     * Process triggered entities
     * ========================== */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
        if (!entity_has_prop(ent, ENTITY_PROP_TRIGGERED_THIS_TICK)) continue;
        if ((time - ent->last_triggered < ent->trigger_delay) && ent->last_triggered != 0) continue;

        ent->last_triggered = time;

        /* Fire weapon */
        if (entity_has_prop(ent, ENTITY_PROP_WEAPON)) {
            struct sprite_tag sprite = ent->sprite;
            u32 animation_frame = ent->animation_frame;
            struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, sprite);

            struct xform sprite_local_xform = ent->sprite_local_xform;

            struct sprite_sheet_slice out_slice = sprite_sheet_get_slice(sheet, STR("out"), animation_frame);
            struct v2 rel_pos = xform_mul_v2(sprite_local_xform, out_slice.center);
            struct v2 rel_dir = xform_basis_mul_v2(sprite_local_xform, out_slice.dir);

            {
                /* Spawn bullet */
                struct entity *bullet = entity_alloc(root);
                bullet->sprite = sprite_tag_from_path(STR("res/graphics/bullet.ase"));

                bullet->bullet_src = ent->handle;
                bullet->bullet_src_pos = rel_pos;
                bullet->bullet_src_dir = rel_dir;
                bullet->bullet_impulse = 0.25;
                //bullet->bullet_impulse = 1;
                bullet->mass_unscaled = 0.04;

                entity_enable_prop(bullet, ENTITY_PROP_BULLET);
            }
        }
    }

    /* ========================== *
     * Create forces from control move
     * ========================== */

#if 0
    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;

        struct v2 move = ent->control.move;
        if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
            struct entity *f = entity_alloc(ent);
            entity_enable_prop(f, ENTITY_PROP_FORCE);
            f->force = v2_mul(move, ent->control_force);
            activate_now(f);
        }
    }
#else
    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;

        if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
            struct v2 move = ent->control.move;
            struct v2 force = v2_mul(move, ent->control_force);
            entity_apply_force_to_center(ent, force);
        }
    }
#endif

    /* ========================== *
     * Create forces from control focus (aim)
     * ========================== */

#if 0
    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;

        if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
            struct xform xf = entity_get_xform(ent);

            /* Solve for final angle using law of sines */
            f32 final_xf_angle;
            {
                struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite);
                struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("attach.wep"), ent->animation_frame);

                f32 forward_hold_angle_offset;
                {
                    struct xform xf_unrotated = xform_rotated_to(xf, 0);
                    struct v2 hold_pos_unrotated = xform_mul_v2(xf_unrotated, xform_mul_v2(ent->sprite_local_xform, slice.center));
                    forward_hold_angle_offset = v2_angle_from_dirs(V2(0, -1), v2_sub(hold_pos_unrotated, xf_unrotated.og));
                }

                struct v2 ent_pos = xf.og;
                struct v2 focus_pos = v2_add(ent_pos, ent->control.focus);

                struct v2 hold_pos;
                {
                    struct xform hold_pos_xf = xform_translated(ent->sprite_local_xform, slice.center);
                    hold_pos_xf = xform_mul(xf, hold_pos_xf);

                    struct v2 hold_pos_xf_dir = xform_basis_mul_v2(hold_pos_xf, slice.dir);
                    hold_pos_xf = xform_rotated_to(hold_pos_xf, v2_angle(hold_pos_xf_dir) + PI / 2);

                    if (v2_eq(hold_pos_xf.og, ent_pos)) {
                        /* If hold pos is same as origin (E.G if pivot is being used as hold pos), then move hold pos forward a tad to avoid issue */
                        hold_pos_xf = xform_translated(hold_pos_xf, V2(0, -1));
                    }
                    hold_pos = hold_pos_xf.og;
                }

                struct v2 hold_dir = xform_basis_mul_v2(xf, xform_basis_mul_v2(ent->sprite_local_xform, slice.dir));
                struct v2 hold_ent_dir = v2_sub(ent_pos, hold_pos);
                struct v2 focus_ent_dir = v2_sub(ent_pos, focus_pos);

                f32 hold_ent_len = v2_len(hold_ent_dir);
                f32 focus_ent_len = v2_len(focus_ent_dir);

                f32 final_hold_angle_btw_ent_and_focus = v2_angle_from_dirs(hold_ent_dir, hold_dir);
                f32 final_focus_angle_btw_ent_and_hold = math_asin((math_sin(final_hold_angle_btw_ent_and_focus) * hold_ent_len) / focus_ent_len);
                f32 final_ent_angle_btw_focus_and_hold = PI - (final_focus_angle_btw_ent_and_hold + final_hold_angle_btw_ent_and_focus);

                final_xf_angle = v2_angle_from_dirs(V2(0, -1), v2_sub(focus_pos, ent_pos)) + final_ent_angle_btw_focus_and_hold - forward_hold_angle_offset;
            }

            if (!F32_IS_NAN(final_xf_angle)) {
                const f32 angle_error_allowed = 0.001;
                if (math_fabs(final_xf_angle - v2_angle(xf.bx)) > angle_error_allowed) {
                    xf = xform_rotated_to(xf, final_xf_angle);
                }
            }

            entity_set_xform(ent, xf);
        }
    }
#else
    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;

        if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
            struct xform xf = entity_get_xform(ent);

            /* Solve for final angle using law of sines */
            f32 final_xf_angle;
            {
                struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite);
                struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("attach.wep"), ent->animation_frame);

                f32 forward_hold_angle_offset;
                {
                    struct xform xf_unrotated = xform_rotated_to(xf, 0);
                    struct v2 hold_pos_unrotated = xform_mul_v2(xf_unrotated, xform_mul_v2(ent->sprite_local_xform, slice.center));
                    forward_hold_angle_offset = v2_angle_from_dirs(V2(0, -1), v2_sub(hold_pos_unrotated, xf_unrotated.og));
                }

                struct v2 ent_pos = xf.og;
                struct v2 focus_pos = v2_add(ent_pos, ent->control.focus);

                struct v2 hold_pos;
                {
                    struct xform hold_pos_xf = xform_translated(ent->sprite_local_xform, slice.center);
                    hold_pos_xf = xform_mul(xf, hold_pos_xf);

                    struct v2 hold_pos_xf_dir = xform_basis_mul_v2(hold_pos_xf, slice.dir);
                    hold_pos_xf = xform_rotated_to(hold_pos_xf, v2_angle(hold_pos_xf_dir) + PI / 2);

                    if (v2_eq(hold_pos_xf.og, ent_pos)) {
                        /* If hold pos is same as origin (E.G if pivot is being used as hold pos), then move hold pos forward a tad to avoid issue */
                        hold_pos_xf = xform_translated(hold_pos_xf, V2(0, -1));
                    }
                    hold_pos = hold_pos_xf.og;
                }

                struct v2 hold_dir = xform_basis_mul_v2(xf, xform_basis_mul_v2(ent->sprite_local_xform, slice.dir));
                struct v2 hold_ent_dir = v2_sub(ent_pos, hold_pos);
                struct v2 focus_ent_dir = v2_sub(ent_pos, focus_pos);

                f32 hold_ent_len = v2_len(hold_ent_dir);
                f32 focus_ent_len = v2_len(focus_ent_dir);

                f32 final_hold_angle_btw_ent_and_focus = v2_angle_from_dirs(hold_ent_dir, hold_dir);
                f32 final_focus_angle_btw_ent_and_hold = math_asin((math_sin(final_hold_angle_btw_ent_and_focus) * hold_ent_len) / focus_ent_len);
                f32 final_ent_angle_btw_focus_and_hold = PI - (final_focus_angle_btw_ent_and_hold + final_hold_angle_btw_ent_and_focus);

                final_xf_angle = v2_angle_from_dirs(V2(0, -1), v2_sub(focus_pos, ent_pos)) + final_ent_angle_btw_focus_and_hold - forward_hold_angle_offset;
            }

            if (!F32_IS_NAN(final_xf_angle)) {
                const f32 angle_error_allowed = 0.001;
                f32 cur_angle = v2_angle(xf.bx);
                f32 diff = final_xf_angle - cur_angle;
                if (math_fabs(diff) > angle_error_allowed) {

#if 0
#if 0
                    /* Create force */
                    struct entity *f = entity_alloc(ent);
                    entity_enable_prop(f, ENTITY_PROP_TORQUE);

                    i32 dir = math_fsign(diff);
                    f32 damp = math_fabs(math_fmod(diff, 2 * PI)) / (2 * PI);
                    f32 torque = ent->control_torque * dir * damp;

                    f->torque = torque;
                    activate_now(f);
#else
                    /* TODO: Remove this (testing) */
                    /* Create force */
                    if (!ent->test_torque_applied) {
                        ent->test_torque_applied = true;
                        entity_apply_angular_impulse(ent, 10);
                    }
#endif
#endif
                }
            }
        }
    }
#endif

    /* ========================== *
     * Create ground friction force (gravity)
     * ========================== */

#if 1
    /* TODO: Do this globally rather than creating entities for constant forces */
    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
        if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue;
#if 1
        if (ent->linear_ground_friction != 0) {
            /* Linear velocity */
            {
                struct v2 linear_velocity = ent->linear_velocity;
                if (!v2_is_zero(linear_velocity)) {
                    /* FIXME: Incorrect behavior at low FPS & low entity density */
                    const f32 clamp_epsilon = 0.01;
                    f32 linear_velocity_len = v2_len(linear_velocity);
                    if (linear_velocity_len >= clamp_epsilon) {
                        f32 force_len = -linear_velocity_len * ent->linear_ground_friction;
                        struct v2 force = v2_mul(v2_norm(linear_velocity), force_len);
                        entity_apply_force_to_center(ent, force);
                    } else {
                        /* If linear_velocity is below clamp_epsilon, stop entity movement. */
                        struct xform xf = entity_get_xform(ent);
                        f32 mass = ent->mass_unscaled * math_fabs(xform_get_determinant(xf));
                        struct v2 impulse = v2_mul(v2_neg(linear_velocity), mass);
                        entity_apply_linear_impulse_to_center(ent, impulse);
                    }
                }
            }
        }
        if (ent->angular_ground_friction != 0) {
            /* Angular velocity */
            {
                f32 angular_velocity = ent->angular_velocity;
                if (angular_velocity != 0) {
                    /* FIXME: Incorrect (just testing) */
                    const f32 clamp_epsilon = 0.001;
                    if (math_fabs(angular_velocity) >= clamp_epsilon) {
                        f32 torque = -angular_velocity * ent->angular_ground_friction;
                        entity_apply_torque(ent, torque);
                    } else {
                        /* If angular_velocity is below clamp_epsilon, stop entity movement. */
                        struct xform xf = entity_get_xform(ent);
                        f32 inertia = ent->inertia_unscaled * math_fabs(xform_get_determinant(xf));
                        f32 impulse = -angular_velocity * inertia;
                        entity_apply_angular_impulse(ent, impulse);
                    }
            }
        }
        }
#else
            /* TODO: Remove this (gravity test) */
            struct xform xf = entity_get_xform(ent);
            f32 mass = ent->mass_unscaled * math_fabs(xform_get_determinant(xf));
            entity_apply_force_to_center(ent, V2(0, 9.81 * mass));
#endif
    }
#endif

    /* ========================== *
     * Integrate forces
     * ========================== */

#if 0
    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
        if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue;

        struct xform xf = entity_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;

        /* Determine force & torque acceleration */
        struct v2 tick_force_accel = v2_mul(v2_div(ent->force, mass), dt);
        f32 tick_torque_accel = (ent->torque / inertia) * dt;

        /* Integrate */
        /* TODO: Don't need to multiply each term by dt separately */
        struct v2 tick_linear_velocity = v2_add(v2_mul(ent->linear_velocity, dt), v2_mul(tick_force_accel, dt));
        f32 tick_angular_velocity = (ent->angular_velocity * dt) + (tick_torque_accel * dt);

        xf.og = v2_add(xf.og, tick_linear_velocity);
        xf = xform_rotated(xf, tick_angular_velocity);

        ent->linear_velocity = v2_div(tick_linear_velocity, dt);
        ent->angular_velocity = tick_angular_velocity / dt;
        ent->force = V2(0, 0);
        ent->torque = 0;

        entity_set_xform(ent, xf);
    }
#else
    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
        if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue;

        struct xform xf = entity_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;

        /* Determine force & torque acceleration */
        struct v2 force_accel = v2_mul(v2_div(ent->force, mass), dt);
        f32 torque_accel = (ent->torque / inertia) * dt;

        /* Integrate */
        ent->linear_velocity = v2_add(ent->linear_velocity, force_accel);
        ent->angular_velocity += torque_accel;

        /* Reset forces */
        ent->force = V2(0, 0);
        ent->torque = 0;
    }
#endif

#if 1
    /* ========================== *
     * Collision
     * ========================== */

    for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
        struct entity *e0 = &store->entities[e0_index];
        if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue;
        if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue;
        if (!entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED) && !entity_has_prop(e0, ENTITY_PROP_BULLET)) continue;

        struct xform e0_xf = entity_get_xform(e0);
        struct quad e0_quad;
        struct v2_array e0_poly;
        {
            struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite);
            struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame);
            e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect));
            e0_quad = xform_mul_quad(e0_xf, e0_quad);
            e0_poly = (struct v2_array) {
                .count = ARRAY_COUNT(e0_quad.e),
                .points = e0_quad.e
            };
        }

        b32 colliding = false;
        struct v2 p0 = V2(0, 0);
        struct v2 p1 = V2(0, 0);
        struct entity *colliding_with = entity_nil();
        struct gjk_simplex simplex = { 0 };
        struct gjk_prototype prototype = { 0 };
        struct v2 velocity = V2(0, 0);
        b32 solved = false;
        for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
            struct entity *e1 = &store->entities[e1_index];
            if (e1 == e0) continue;
            if (!(e1->valid && entity_has_prop(e1, ENTITY_PROP_ACTIVE))) continue;
            if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue;
            if (entity_has_prop(e1, ENTITY_PROP_PLAYER_CONTROLLED)) continue;

            struct xform e1_xf = entity_get_xform(e1);
            struct quad e1_quad;
            struct v2_array e1_poly;
            {
                struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite);
                struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame);
                e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect));
                e1_quad = xform_mul_quad(e1_xf, e1_quad);
                e1_poly = (struct v2_array) {
                    .count = ARRAY_COUNT(e1_quad.e),
                    .points = e1_quad.e
                };
            }

            struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly);

            colliding = res.colliding;
            p0 = res.p0;
            p1 = res.p1;
            colliding_with = e1;
            simplex = res.simplex;
            prototype = res.prototype;
            solved = res.solved;
        }

        e0->colliding = colliding;
        e0->colliding_with = colliding_with->handle;
        e0->point = p0;
        e0->simplex = simplex;
        e0->prototype = prototype;
        e0->pendir = velocity;
        e0->solved = solved;

        if (colliding_with->valid) {
            colliding_with->colliding = colliding;
            colliding_with->colliding_with = e0->handle;
            colliding_with->point = p1;
        }

        {
#if 1
            if (colliding) {
                struct entity *e1 = colliding_with;
                struct xform e1_xf = entity_get_xform(e1);

                f32 scale0 = math_fabs(xform_get_determinant(e0_xf));
                f32 scale1 = math_fabs(xform_get_determinant(e1_xf));
                f32 m0 = e0->mass_unscaled * scale0;
                f32 m1 = e1->mass_unscaled * scale1;
                f32 i0 = e0->inertia_unscaled * scale0;
                f32 i1 = e1->inertia_unscaled * scale1;
                f32 inv_m0 = 1.f / m0;
                f32 inv_m1 = 1.f / m1;
                f32 inv_i0 = 1.f / i0;
                f32 inv_i1 = 1.f / i1;
                (UNUSED)inv_m0;
                (UNUSED)inv_m1;
                (UNUSED)inv_i0;
                (UNUSED)inv_i1;

                struct v2 pen = v2_sub(p1, p0);
                struct v2 pen_norm = v2_norm(pen);

#if 1
                f32 bias_factor = 0.2;
                f32 bias_slop = 0.005;

                f32 bias = (bias_factor / dt) * max((v2_len(pen) - bias_slop), 0);
#else
                f32 bias = 0;
#endif


                struct v2 vcp0 = v2_sub(p0, e0_xf.og);
                struct v2 vcp1 = v2_sub(p1, e1_xf.og);

                struct v2 p0_vel = v2_add(e0->linear_velocity, v2_perp_mul(vcp0, e0->angular_velocity));
                struct v2 p1_vel = v2_add(e1->linear_velocity, v2_perp_mul(vcp1, e1->angular_velocity));

                struct v2 vrel = v2_sub(p1_vel, p0_vel);

                f32 vn = v2_dot(vrel, pen_norm);
                vn = max_f32(vn, 0);


                struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, pen_norm) * inv_i0);
                struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, pen_norm) * inv_i1);

                f32 k = inv_m0 + inv_m1 + v2_dot(pen_norm, v2_add(idk0, idk1));

                /* (to be applied along n) */
                f32 j = (vn + bias) / k;
                //j = max_f32(j, 0);



                if (j > 0) {
                    DEBUGBREAKABLE;
                }

                if (!e0->test_collided) {
                    e0->test_collided = true;
                }



                struct v2 imp = v2_mul(pen_norm, j);
                (UNUSED)imp;

                //imp = v2_mul(imp, dt);


#if 1
                entity_apply_linear_impulse(e0, imp, p0);
                entity_apply_linear_impulse(e1, v2_neg(imp), p1);
#endif

            }
#endif
        }
    }
#endif

    /* ========================== *
     * Update positions
     * ========================== */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
        if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue;

        struct xform xf = entity_get_xform(ent);
        struct v2 tick_linear_velocity = v2_mul(ent->linear_velocity, dt);
        f32 tick_angular_velocity = ent->angular_velocity * dt;
        xf.og = v2_add(xf.og, tick_linear_velocity);
        xf = xform_rotated(xf, tick_angular_velocity);
        entity_set_xform(ent, xf);
    }

    /* ========================== *
     * Initialize bullet kinematics from sources
     * ========================== */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;

        /* FIXME: Apply src entity velocity to bullet velocity */

        if (entity_has_prop(ent, ENTITY_PROP_BULLET) && ent->activation_tick == G.tick.tick_id) {
            struct entity *src = entity_from_handle(store, ent->bullet_src);
            struct xform src_xf = entity_get_xform(src);

            struct v2 pos = xform_mul_v2(src_xf, ent->bullet_src_pos);
            struct v2 impulse = xform_basis_mul_v2(src_xf, ent->bullet_src_dir);
            impulse = v2_mul(v2_norm(impulse), ent->bullet_impulse);

            struct xform xf = XFORM_TRS(.t = pos, .r = v2_angle(impulse) + PI / 2);
            entity_set_xform(ent, xf);
            entity_enable_prop(ent, ENTITY_PROP_PHYSICAL);

            entity_apply_linear_impulse_to_center(ent, impulse);
        }
    }

    /* ========================== *
     * Update camera position
     * ========================== */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;

        /* Camera follow */
        if (entity_has_prop(ent, ENTITY_PROP_CAMERA)) {
            struct entity *follow = entity_from_handle(store, ent->camera_follow);

            struct xform xf = entity_get_xform(ent);

            f32 aspect_ratio = 1.0;
            {
                struct xform quad_xf = xform_mul(entity_get_xform(ent), ent->camera_quad_xform);
                struct v2 camera_size = xform_get_scale(quad_xf);
                if (!v2_is_zero(camera_size)) {
                    aspect_ratio = camera_size.x / camera_size.y;
                }
            }
            f32 ratio_y = 0.33f;
            f32 ratio_x = ratio_y / aspect_ratio;
            struct v2 camera_focus_dir = v2_mul_v2(follow->control.focus, V2(ratio_x, ratio_y));
            struct v2 camera_focus_pos = v2_add(entity_get_xform(follow).og, camera_focus_dir);
            ent->camera_xform_target = xf;
            ent->camera_xform_target.og = camera_focus_pos;

            /* Lerp camera */
            if (ent->camera_applied_lerp_continuity_gen_plus_one == ent->camera_lerp_continuity_gen + 1) {
                f32 t = 1 - math_pow(2.f, -20.f * (f32)G.tick.dt);
                xf = xform_lerp(xf, ent->camera_xform_target, t);
            } else {
                /* Skip lerp */
                xf = ent->camera_xform_target;
            }
            ent->camera_applied_lerp_continuity_gen_plus_one = ent->camera_lerp_continuity_gen + 1;
            entity_set_xform(ent, xf);
        }
    }

    /* ========================== *
     * Update sound emitters
     * ========================== */

    /* TODO: Sound entities should be created by game thread, but played by the
     * user thread. This is so sounds play at the correct time on the user
     * thread regardless of interp delay. */

    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
        struct entity *ent = &store->entities[entity_index];
        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;

        if (entity_has_prop(ent, ENTITY_PROP_TEST_SOUND_EMITTER)) {
            struct mixer_desc desc = ent->sound_desc;
            desc.speed = G.tick.timescale;

            desc.pos = entity_get_xform(ent).og;
            struct sound *sound = sound_load_async(ent->sound_name, 0);
            b32 played = ent->sound_handle.gen != 0;
            if (sound) {
                if (!played) {
                    ent->sound_handle = mixer_play_ex(sound, desc);
                } else {
                    mixer_track_set(ent->sound_handle, desc);
                }
            }
        }
    }

    /* ========================== *
     * Release entities
     * ========================== */

     /* TODO: Breadth first iteration to only release parent entities (since
      * child entities will be released along with parent anyway) */

    {
        struct temp_arena temp = arena_temp_begin(scratch.arena);

        struct entity **ents_to_release = arena_dry_push(temp.arena, struct entity *);
        u64 ents_to_release_count = 0;
        for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
            struct entity *ent = &store->entities[entity_index];
            if (!ent->valid) continue;

            if (entity_has_prop(ent, ENTITY_PROP_RELEASE)) {
                *arena_push(temp.arena, struct entity *) = ent;
                ++ents_to_release_count;
            }
        }

        for (u64 i = 0; i < ents_to_release_count; ++i) {
            struct entity *ent = ents_to_release[i];
            if (ent->valid) {
                entity_release(store, ent);
            }
        }

        arena_temp_end(temp);
    }

    /* ========================== *
     * Publish tick
     * ========================== */

    /* Publish tick */
    publish_game_tick();
    __profframe("Game");

    /* ========================== *
     * End frame cache scopes
     * ========================== */

    sprite_scope_end(sprite_frame_scope);

    scratch_end(scratch);
}

/* ========================== *
 * Game thread
 * ========================== */

INTERNAL SYS_THREAD_ENTRY_POINT_FUNC_DEF(game_thread_entry_point, arg)
{
    struct temp_arena scratch = scratch_begin_no_conflict();

    (UNUSED)arg;
    sys_timestamp_t last_frame_ts = 0;
    f64 target_dt = GAME_FPS > (0) ? (1.0 / GAME_FPS) : 0;
    while (!atomic_i32_eval(&G.game_thread_shutdown)) {
        __profscope(game_update_w_sleep);
        struct temp_arena temp = arena_temp_begin(scratch.arena);
        sleep_frame(last_frame_ts, target_dt);
        last_frame_ts = sys_timestamp();
        {
            struct game_cmd_array game_cmds = pop_cmds(temp.arena);
            if (!G.paused) {
                game_update(game_cmds);
            }
            /* Check for pause / next frame cmds */
            for (u64 i = 0; i < game_cmds.count; ++i) {
                struct game_cmd cmd = game_cmds.cmds[i];
                switch (cmd.kind) {
                    case GAME_CMD_KIND_PAUSE: {
                        G.paused = !G.paused;
                    } break;

                    case GAME_CMD_KIND_STEP: {
                        if (G.paused) {
                            G.paused = false;
                            game_update(game_cmds);
                            G.paused = true;
                        }
                    } break;

                    default: break;
                }
            }
        }
        arena_temp_end(temp);
    }

    scratch_end(scratch);
}

/* ========================== *
 * Interface
 * ========================== */

void game_get_latest_tick(struct world *dest)
{
    __prof;
    struct sys_lock lock = sys_mutex_lock_s(&G.prev_tick_mutex);
    world_copy_replace(dest, &G.prev_tick);
    sys_mutex_unlock(&lock);
}

u64 game_get_latest_tick_id(void)
{
    return atomic_u64_eval(&G.prev_tick_id);
}

void game_push_cmds(struct game_cmd_array cmd_array)
{
    push_cmds(cmd_array);
}