#if defined(__clang__)
#pragma once
#include "feely_pona_unity.h"
#endif

#define FP_Game_MetersToPixelsNx1(game, val)  ((val) * (game)->meters_to_pixels)
#define FP_Game_MetersToPixelsNx2(game, x, y) (Dqn_V2_InitNx2(x, y) * (game)->meters_to_pixels)
#define FP_Game_MetersToPixelsV2(game, xy)    (xy * (game)->meters_to_pixels)

#define FP_Game_PixelsToMetersNx1(game, val)  ((val) * (1.f/(game)->meters_to_pixels))
#define FP_Game_PixelsToMetersNx2(game, x, y) (Dqn_V2_InitNx2(x, y) * (1.f/(game)->meters_to_pixels))
#define FP_Game_PixelsToMetersV2(game, xy)    (xy * (1.f/(game)->meters_to_pixels))

static bool operator==(FP_GameEntityHandle const &lhs, FP_GameEntityHandle const &rhs)
{
    bool result = lhs.id == rhs.id;
    return result;
}

static bool operator!=(FP_GameEntityHandle const &lhs, FP_GameEntityHandle const &rhs)
{
    bool result = !(lhs == rhs);
    return result;
}

static Dqn_M2x3 FP_Game_CameraModelViewM2x3(FP_GameCamera camera, TELY_Platform *platform)
{
    Dqn_M2x3 result = Dqn_M2x3_Identity();
    if (platform) {
        Dqn_V2 rotate_origin = camera.world_pos - (Dqn_V2_InitV2I(platform->core.window_size) * .5f);
        result               = Dqn_M2x3_Mul(result, Dqn_M2x3_Translate(rotate_origin));
        result               = Dqn_M2x3_Mul(result, Dqn_M2x3_Rotate(camera.rotate_rads));
        result               = Dqn_M2x3_Mul(result, Dqn_M2x3_Scale(camera.scale));
        result               = Dqn_M2x3_Mul(result, Dqn_M2x3_Translate(-rotate_origin + camera.world_pos));
    }
    return result;
}

static FP_GameEntity *FP_Game_GetEntity(FP_Game *game, FP_GameEntityHandle handle)
{
    FP_GameEntity *result = nullptr;
    if (!game)
        return result;

    result                     = game->entities.data;
    uint64_t index_from_handle = handle.id & FP_GAME_ENTITY_HANDLE_INDEX_MASK;
    if (index_from_handle >= game->entities.size)
        return result;

    FP_GameEntity *candidate = game->entities.data + index_from_handle;
    if (candidate->handle == handle)
        result = candidate;
    return result;
}

static bool FP_Game_DFSPreOrderWalkEntityTree(FP_Game *game, FP_GameEntityIterator *it, FP_GameEntity *root)
{
    if (!game || !it || !root)
        return false;

    it->last_visited = it->entity;
    if (it->init) {
        it->iteration_count++;
    } else {
        it->init               = true;
        it->entity             = root;
        it->entity_parent      = it->entity->parent;
        it->entity_next        = it->entity->next;
        it->entity_first_child = it->entity->first_child;
    }

    if (it->entity_first_child) {
        it->entity             = it->entity_first_child;
        it->entity_parent      = it->entity->parent;
        it->entity_next        = it->entity->next;
        it->entity_first_child = it->entity->first_child;
    } else {
        while (it->entity->handle != root->handle) {
            if (it->entity_next) {
                it->entity             = it->entity_next;
                it->entity_parent      = it->entity->parent;
                it->entity_next        = it->entity->next;
                it->entity_first_child = it->entity->first_child;
                break;
            } else {
                if (!it->entity_parent)
                    break;
                it->entity             = it->entity_parent;
                it->entity_parent      = it->entity->parent;
                it->entity_next        = it->entity->next;
                it->entity_first_child = it->entity->first_child;
            }
        }
    }

    return it->entity->handle != root->handle;
}

static bool FP_Game_DFSPostOrderWalkEntityTree(FP_Game *game, FP_GameEntityIterator *it, FP_GameEntity *root)
{
    if (!game || !it || !root)
        return false;

    bool ascending_tree = it->entity ? (it->last_visited == it->entity->last_child) : false;
    it->last_visited    = it->entity;

    if (it->init) {
        it->iteration_count++;
    } else {
        it->init               = true;
        it->entity             = root;
        it->entity_parent      = it->entity->parent;
        it->entity_next        = it->entity->next;
        it->entity_first_child = it->entity->first_child;
    }

    // NOTE: Descend to deepest leaf node
    if (it->entity_first_child && !ascending_tree) {
        while (it->entity_first_child) {
            it->entity             = it->entity_first_child;
            it->entity_parent      = it->entity->parent;
            it->entity_next        = it->entity->next;
            it->entity_first_child = it->entity->first_child;
        }
    } else {
        // NOTE: We are at the leaf node, try going across
        if (it->entity != root && it->entity_next) {
            it->entity             = it->entity_next;
            it->entity_parent      = it->entity->parent;
            it->entity_next        = it->entity->next;
            it->entity_first_child = it->entity->first_child;
            ascending_tree         = false;
        }

        // NOTE: Try descend again
        if (it->entity_first_child && !ascending_tree) {
            while (it->entity_first_child) {
                it->entity             = it->entity_first_child;
                it->entity_parent      = it->entity->parent;
                it->entity_next        = it->entity->next;
                it->entity_first_child = it->entity->first_child;
            }
        }

        // NOTE: If we could not move further across or down then we've
        // exhausted the tree, start moving up.
        if (it->last_visited == it->entity) {
            it->entity             = it->entity_parent;
            it->entity_parent      = it->entity->parent;
            it->entity_next        = it->entity->next;
            it->entity_first_child = it->entity->first_child;
        }
    }

    return it->entity->handle != root->handle;
}


// NOTE: Parent entity
static void FP_Game_PushParentEntity(FP_Game *game, FP_GameEntityHandle handle)
{
    DQN_ASSERTF(game->parent_entity_stack.size >= 1, "Sentinel/nil entity has not been assigned as the 0th slot yet");
    if (game)
        Dqn_FArray_Add(&game->parent_entity_stack, handle);
}

static void FP_Game_PopParentEntity(FP_Game *game)
{
    // NOTE: 0th slot is reserved for the nil entity
    if (game && game->parent_entity_stack.size > 1)
        Dqn_FArray_PopBack(&game->parent_entity_stack, 1);
}

static FP_GameEntityHandle FP_Game_ActiveParentEntity(FP_Game const *game)
{
    FP_GameEntityHandle result = {};
    if (!game || !game->parent_entity_stack.size)
        return result;
    result = game->parent_entity_stack.data[game->parent_entity_stack.size - 1];
    return result;
}

static FP_GameEntity *FP_Game_ActiveParentEntityPointer(FP_Game const *game)
{
    FP_GameEntityHandle handle = FP_Game_ActiveParentEntity(game);
    FP_GameEntity *result      = FP_Game_GetEntity(DQN_CAST(FP_Game *)game, handle);
    return result;
}

static FP_GameEntity *FP_Game_MakeEntityPointerFV(FP_Game *game, DQN_FMT_STRING_ANNOTATE char const *fmt, va_list args)
{
    FP_GameEntity *result = nullptr;
    if (!game)
        return result;

    DQN_ASSERTF(game->entities.size > 0, "Sentinel/nil entity has not been initialised yet");
    DQN_ASSERTF(game->root_entity, "Sentinel/nil entity has not been assigned yet");

    result = game->root_entity; // TODO(doyle): Root entity or ... the nil entity?
    if (game->entity_free_list) {
        result                 = game->entity_free_list;
        game->entity_free_list = game->entity_free_list->next;
        result->next           = nullptr;
    } else {
        if (game->entities.size >= (FP_GAME_ENTITY_HANDLE_INDEX_MAX + 1))
            return result;

        result = Dqn_VArray_Make(&game->entities, Dqn_ZeroMem_Yes);
        if (!result)
            return result;
        result->handle.id = (game->entities.size - 1) & FP_GAME_ENTITY_HANDLE_INDEX_MASK;
    }

    result->sprite_height.meters = 1;
    result->parent               = FP_Game_ActiveParentEntityPointer(game);
    result->name                 = TELY_ChunkPool_AllocFmtFV(game->chunk_pool, fmt, args);

    // NOTE: Attach entity as a child to the parent
    FP_GameEntity *parent = result->parent;
    if (parent->first_child)
        parent->last_child->next = result;
    else
        parent->first_child = result;
    result->prev       = parent->last_child;
    parent->last_child = result;

    DQN_ASSERT(!result->next);
    DQN_ASSERT(result->handle.id);
    DQN_ASSERT(result->parent->handle == game->parent_entity_stack.data[game->parent_entity_stack.size - 1]);
    return result;
}

static FP_GameEntity *FP_Game_MakeEntityPointerF(FP_Game *game, DQN_FMT_STRING_ANNOTATE char const *fmt, ...)
{
    va_list args;
    va_start(args, fmt);
    FP_GameEntity *result = FP_Game_MakeEntityPointerFV(game, fmt, args);
    va_end(args);
    return result;
}

static FP_GameEntityHandle FP_Game_MakeEntityF(FP_Game *game, DQN_FMT_STRING_ANNOTATE char const *fmt, ...)
{
    va_list args;
    va_start(args, fmt);
    FP_GameEntity *entity = FP_Game_MakeEntityPointerF(game, fmt, args);
    va_end(args);

    FP_GameEntityHandle result = {};
    if (entity)
        result = entity->handle;
    return result;
}

static bool FP_Game_IsNilEntity(FP_GameEntity *entity)
{
    bool result = entity ? ((entity->handle.id & FP_GAME_ENTITY_HANDLE_INDEX_MASK) == 0) : true;
    return result;
}

static bool FP_Game_IsNilEntityHandle(FP_Game *game, FP_GameEntityHandle handle)
{
    bool result = true;
    if (handle.id != 0) {
        FP_GameEntity *entity = FP_Game_GetEntity(game, handle);
        result                = FP_Game_IsNilEntity(entity);
    }
    return result;
}

static void FP_Game_DetachEntityIntoFreeList(FP_Game *game, FP_GameEntityHandle handle)
{
    FP_GameEntity *entity = FP_Game_GetEntity(game, handle);
    if (FP_Game_IsNilEntity(entity))
        return;

    // NOTE: Entities in the entity tree always have a parent (except for the 
    // nil/root entity). If an entity is passed in to this function and there's
    // no parent, it's most likely you passed in an entity already in the free
    // list (in which case only the next pointer will be set). This is most
    // likely a mistake so we guard against it here.
    if (!DQN_CHECK(entity->parent))
        return;

    uint64_t const entity_index_from_handle = entity->handle.id & FP_GAME_ENTITY_HANDLE_INDEX_MASK;
    DQN_ASSERT(entity_index_from_handle < game->entities.size);

    uint64_t const entity_generation_raw    = entity->handle.id & FP_GAME_ENTITY_HANDLE_GENERATION_MASK;
    uint64_t const entity_generation        = entity_generation_raw >> FP_GAME_ENTITY_HANDLE_GENERATION_RSHIFT;
    uint64_t const new_entity_generation    = entity_generation + 1;

    // NOTE: De-attach entity from adjacent children
    if (entity->prev)
        entity->prev->next = entity->next;

    if (entity->next)
        entity->next->prev = entity->prev;

    // NOTE: De-attach from parent
    FP_GameEntity *parent = entity->parent;
    if (parent->first_child == entity)
        parent->first_child = entity->next;

    if (parent->last_child == entity)
        parent->last_child = entity->prev;

    if (entity->name.size)
        TELY_ChunkPool_Dealloc(game->chunk_pool, entity->name.data);

    FP_SentinelList_Deinit(&entity->spawn_list, game->chunk_pool);
    FP_SentinelList_Deinit(&entity->waypoints, game->chunk_pool);

    if (new_entity_generation > entity_generation) {
        // NOTE: Update the incremented handle disassociating all prior handles
        // to this entity which would reference older generation values
        *entity           = {};
        entity->handle.id = entity_index_from_handle | (new_entity_generation << FP_GAME_ENTITY_HANDLE_GENERATION_RSHIFT);

        // NOTE: Attach entity to the free list
        entity->next           = game->entity_free_list;
        entity->prev           = nullptr;
        game->entity_free_list = entity;
    } else {
        // NOTE: We've cycled through all possible generations for this handle
        // We will not increment it and freeze it so it is no longer allocated
        // out. This prevents code that is still holding onto *really* old
        // handles
    }
}

static void FP_Game_DeleteEntity(FP_Game *game, FP_GameEntityHandle handle)
{
    uint64_t index_from_handle = handle.id & FP_GAME_ENTITY_HANDLE_INDEX_MASK;
    if (!game || !DQN_CHECK(index_from_handle < game->entities.size))
        return;

    FP_GameEntity *root = game->entities.data + index_from_handle;
    if (root->handle != handle)
        return;

    // NOTE: The iterator snaps a copy of all the internal n-ary tree pointers
    // so as we delete we do not accidentally invalidate any of the pointers.
    for (FP_GameEntityIterator it = {}; FP_Game_DFSPostOrderWalkEntityTree(game, &it, root); ) {
        DQN_ASSERT(it.entity != root);
        FP_GameEntity *entity = it.entity;
        FP_Game_DetachEntityIntoFreeList(game, entity->handle);
    }

    FP_Game_DetachEntityIntoFreeList(game, root->handle);
}

static Dqn_V2 FP_Game_CalcEntityWorldPos(FP_Game const *game, FP_GameEntityHandle handle)
{
    Dqn_V2 result = {};
    if (!game)
        return result;

    FP_GameEntity const *first = FP_Game_GetEntity(DQN_CAST(FP_Game *) game, handle);
    for (FP_GameEntity const *entity = first; entity != game->root_entity; entity = entity->parent)
        result += entity->local_pos;
    return result;
}

static Dqn_Rect FP_Game_CalcEntityLocalHitBox(FP_Game const *game, FP_GameEntityHandle handle)
{
    FP_GameEntity *entity    = FP_Game_GetEntity(DQN_CAST(FP_Game *)game, handle);
    Dqn_V2 half_hit_box_size = entity->local_hit_box_size * .5f;
    Dqn_Rect result          = Dqn_Rect_InitV2x2(entity->local_hit_box_offset - half_hit_box_size, entity->local_hit_box_size);
    return result;
}

static Dqn_Rect FP_Game_CalcEntityWorldHitBox(FP_Game const *game, FP_GameEntityHandle handle)
{
    FP_GameEntity *entity          = FP_Game_GetEntity(DQN_CAST(FP_Game *) game, handle);
    Dqn_V2           world_pos     = FP_Game_CalcEntityWorldPos(game, handle);
    Dqn_Rect         local_hit_box = FP_Game_CalcEntityLocalHitBox(game, entity->handle);
    Dqn_Rect         result        = Dqn_Rect_InitV2x2(world_pos + local_hit_box.pos, local_hit_box.size);
    return result;
}

static Dqn_Rect FP_Game_CalcEntityAttackWorldHitBox(FP_Game const *game, FP_GameEntityHandle handle)
{
    FP_GameEntity *entity    = FP_Game_GetEntity(DQN_CAST(FP_Game *) game, handle);
    Dqn_V2           world_pos = FP_Game_CalcEntityWorldPos(game, handle);
    Dqn_V2 half_hit_box_size   = entity->attack_box_size * .5f;
    Dqn_Rect result            = Dqn_Rect_InitV2x2(world_pos + entity->attack_box_offset - half_hit_box_size, entity->attack_box_size);
    return result;
}

static Dqn_Rect FP_Game_CalcEntityArrayWorldBoundingBox(FP_Game const *game, FP_GameEntityHandle const *handles, Dqn_usize count)
{
    Dqn_Rect result = {};
    if (!game || !handles)
        return result;

    DQN_FOR_UINDEX(index, count) {
        FP_GameEntityHandle  handle = handles[index];
        FP_GameEntity const *entity = FP_Game_GetEntity(DQN_CAST(FP_Game *) game, handle);
        Dqn_Rect               bbox   = FP_Game_CalcEntityLocalHitBox(game, entity->handle);
        for (FP_GameShape const &shape_ : entity->shapes) {
            FP_GameShape const *shape = &shape_;
            switch (shape->type) {
                case FP_GameShapeType_None: {
                } break;

                case FP_GameShapeType_Circle: {
                    Dqn_Rect rect =
                        Dqn_Rect_InitV2x2(shape->p1 - shape->circle_radius, Dqn_V2_InitNx1(shape->circle_radius * 2.f));
                    bbox = Dqn_Rect_Union(bbox, rect);
                } break;

                case FP_GameShapeType_Rect: /*FALLTHRU*/
                case FP_GameShapeType_Line: {
                    Dqn_V2   min   = Dqn_V2_Min(shape->p1, shape->p2);
                    Dqn_V2   max   = Dqn_V2_Max(shape->p1, shape->p2);
                    Dqn_Rect rect  = Dqn_Rect_InitV2x2(min, max - min);

                    if (shape->type == FP_GameShapeType_Rect)
                        rect.pos -= rect.size * .5f;

                    bbox = Dqn_Rect_Union(bbox, rect);
                } break;
            }
        }
        bbox.pos += FP_Game_CalcEntityWorldPos(game, entity->handle);

        if (index)
            result = Dqn_Rect_Union(result, bbox);
        else
            result = bbox;
    }
    return result;
}

static Dqn_Rect FP_Game_CalcEntityWorldBoundingBox(FP_Game *game, FP_GameEntityHandle handle)
{
    Dqn_Rect result = FP_Game_CalcEntityArrayWorldBoundingBox(game, &handle, 1);
    return result;
}

// Reset the timers and animation for the current action and set the duration
// for the new action.
static void FP_Game_EntityActionReset(FP_Game *game, FP_GameEntityHandle entity_handle, uint64_t duration_ms, TELY_AssetAnimatedSprite sprite)
{
    FP_GameEntity *entity = FP_Game_GetEntity(game, entity_handle);
    if (!entity)
        return;
    entity->action.sprite              = sprite;
    entity->action.started_at_clock_ms = game->clock_ms;
    entity->action.end_at_clock_ms     = DQN_MAX(duration_ms, game->clock_ms + duration_ms);
}

static Dqn_V2I FP_Game_WorldPosToTilePos(FP_Game *game, Dqn_V2 world_pos)
{
    Dqn_V2I result = Dqn_V2I_InitNx2(world_pos.x / game->tile_size, world_pos.y / game->tile_size);
    return result;
}

static Dqn_V2 FP_Game_TilePosToWorldPos(FP_Game *game, Dqn_V2I tile_pos)
{
    Dqn_V2 result = Dqn_V2_InitNx2(tile_pos.x * game->tile_size, tile_pos.y * game->tile_size);
    return result;
}

static Dqn_Slice<Dqn_V2I> FP_Game_AStarPathFind(FP_Game            *game,
                                                Dqn_Arena          *arena,
                                                TELY_Platform      *platform,
                                                FP_GameEntityHandle entity,
                                                Dqn_V2I             dest_tile)
{
    Dqn_Profiler_ZoneScopeWithIndex("FP_Update: A*", FP_ProfileZone_FPUpdate_AStar);
    Dqn_usize tile_count_x = DQN_CAST(Dqn_usize)(platform->core.window_size.w / game->tile_size);
    Dqn_usize tile_count_y = DQN_CAST(Dqn_usize)(platform->core.window_size.h / game->tile_size);

    Dqn_Slice<Dqn_V2I> result = {};
    if (dest_tile.x < 0 || dest_tile.x > tile_count_x ||
        dest_tile.y < 0 || dest_tile.y > tile_count_y)
        return result;

    Dqn_DSMap<FP_GameAStarNode> astar_info = Dqn_DSMap_Init<FP_GameAStarNode>(128);
    DQN_DEFER { Dqn_DSMap_Deinit(&astar_info); };

    // NOTE: Enumerate the entities that are collidable ============================================
    bool dest_tile_is_non_traversable = false;
    auto zone_enum_collidables        = Dqn_Profiler_BeginZoneWithIndex(DQN_STRING8("FP_Update: A* enumerate collidables"), FP_ProfileZone_FPUpdate_AStarEnumerateCollidables);
    for (FP_GameEntityIterator it = {}; FP_Game_DFSPreOrderWalkEntityTree(game, &it, game->root_entity); ) {
        FP_GameEntity const *walk_entity = it.entity;
        if (entity == walk_entity->handle)
            continue;

        if ((walk_entity->flags & FP_GameEntityFlag_NonTraversable) == 0)
            continue;

        // NOTE: Mark tiles that the entity is on as non-traversable
        Dqn_Rect bounding_box  = FP_Game_CalcEntityWorldBoundingBox(game, walk_entity->handle);
        Dqn_RectMinMax min_max = Dqn_Rect_MinMax(bounding_box);
        Dqn_V2I min_tile       = FP_Game_WorldPosToTilePos(game, min_max.min);
        Dqn_V2I max_tile       = FP_Game_WorldPosToTilePos(game, min_max.max);

        for (int32_t y = min_tile.y; y < max_tile.y; y++) {
            for (int32_t x = min_tile.x; x < max_tile.x; x++) {
                uint64_t tile_u64 = (DQN_CAST(uint64_t)y << 32) | (DQN_CAST(uint64_t)x << 0);
                FP_GameAStarNode *node = Dqn_DSMap_MakeKeyU64(&astar_info, tile_u64).value;
                node->non_traversable  = true;
                node->tile             = Dqn_V2I_InitNx2(x, y);

                if (node->tile == dest_tile)
                    dest_tile_is_non_traversable = true;
            }
        }
    }
    Dqn_Profiler_EndZone(zone_enum_collidables);

    // NOTE: Setup A* state ========================================================================
    Dqn_V2 entity_world_pos = FP_Game_CalcEntityWorldPos(game, entity);
    Dqn_V2I src_tile        = FP_Game_WorldPosToTilePos(game, entity_world_pos);

    Dqn_FArray<Dqn_V2I, 128> frontier = {};
    Dqn_FArray_Add(&frontier, src_tile);

    // NOTE: Initialise the starting cost
    uint64_t src_tile_u64 = (DQN_CAST(uint64_t)src_tile.y << 32) | (DQN_CAST(uint64_t)src_tile.x << 0);
    Dqn_DSMap_MakeKeyU64(&astar_info, src_tile_u64).value->tile = src_tile;

    // NOTE: Do the A* process =====================================================================
    Dqn_usize last_successful_manhattan_dist = UINT64_MAX;
    Dqn_V2I last_successful_tile             = src_tile;

    auto zone_astar_expand = Dqn_Profiler_BeginZoneWithIndex(DQN_STRING8("FP_Update: A* expand"), FP_ProfileZone_FPUpdate_AStarExpand);
    while (frontier.size) {
        Dqn_Profiler_ZoneScopeWithIndex("FP_Update: A* neighbours", FP_ProfileZone_FPUpdate_AStarExploreNeighbours);
        Dqn_V2I curr_tile = Dqn_FArray_PopFront(&frontier, 1);
        if (curr_tile == dest_tile)
            break;

        Dqn_FArray<Dqn_V2I, 4> neighbours = {};
        {
            Dqn_V2I left   = Dqn_V2I_InitNx2(curr_tile.x - 1, curr_tile.y);
            Dqn_V2I right  = Dqn_V2I_InitNx2(curr_tile.x + 1, curr_tile.y);
            Dqn_V2I top    = Dqn_V2I_InitNx2(curr_tile.x,     curr_tile.y - 1);
            Dqn_V2I bottom = Dqn_V2I_InitNx2(curr_tile.x,     curr_tile.y + 1);

            if (left.x >= 0)
                Dqn_FArray_Add(&neighbours, left);
            if (right.x <= tile_count_x)
                Dqn_FArray_Add(&neighbours, right);
            if (top.y >= 0)
                Dqn_FArray_Add(&neighbours, top);
            if (bottom.y <= tile_count_y)
                Dqn_FArray_Add(&neighbours, bottom);
        }

        uint64_t const curr_tile_u64 = (DQN_CAST(uint64_t)curr_tile.y << 32) | (DQN_CAST(uint64_t)curr_tile.x << 0);
        Dqn_usize const curr_cost    = Dqn_DSMap_FindKeyU64(&astar_info, curr_tile_u64).value->cost;
        for (Dqn_V2I next_tile : neighbours) {

            // NOTE: Calculate cost to move to this neighbouring tile.
            Dqn_usize new_cost                                 = curr_cost + 1;
            uint64_t next_tile_u64                             = (DQN_CAST(uint64_t)next_tile.y << 32) | (DQN_CAST(uint64_t)next_tile.x << 0);
            Dqn_DSMapResult<FP_GameAStarNode> next_cost_result = Dqn_DSMap_MakeKeyU64(&astar_info, next_tile_u64);
            next_cost_result.value->tile                       = next_tile;

            // NOTE: We got as close as possible, we know it's impossible to
            // reach, we will stop here.
            // TODO(doyle): Doesn't work in the general case, what if there's a
            // 2 consecutive blocks that are not traversable, we will never
            // realise that
            if (dest_tile == next_tile && dest_tile_is_non_traversable) {
                Dqn_FArray_Clear(&frontier);
                break;
            }

            if (next_cost_result.value->non_traversable)
                continue;

            // NOTE: If we have already visited this node before, we only keep the cost if it's cheaper
            if (next_cost_result.found && new_cost >= next_cost_result.value->cost)
                continue;

            // NOTE: Update the node cost value and the heuristic (estimated cost to the end)
            Dqn_usize manhattan_dist          = DQN_ABS(dest_tile.x - next_tile.x) + DQN_ABS(dest_tile.y - next_tile.y);
            next_cost_result.value->cost      = new_cost;
            next_cost_result.value->came_from = curr_tile;
            next_cost_result.value->heuristic = new_cost + manhattan_dist;

            // NOTE: Store the last node we visited that had the best cost.
            // We may end up with a partial path find that could only get
            // part-way to the solution which this variable will track for us.
            if (manhattan_dist < last_successful_manhattan_dist) {
                last_successful_manhattan_dist = manhattan_dist;
                last_successful_tile           = next_tile;
            }

            // TODO(doyle): Find the insert location into the frontier
            bool inserted = false;
            DQN_FOR_UINDEX(index, frontier.size) {
                Dqn_V2I frontier_tile        = frontier.data[index];
                uint64_t frontier_tile_u64   = DQN_CAST(uint64_t)frontier_tile.y << 32 | DQN_CAST(uint64_t)frontier_tile.x << 0;
                Dqn_usize frontier_heuristic = Dqn_DSMap_FindKeyU64(&astar_info, frontier_tile_u64).value->heuristic;
                if (next_cost_result.value->heuristic >= frontier_heuristic)
                    continue;

                Dqn_FArray_Insert(&frontier, index, next_tile);
                inserted = true;
                break;
            }

            if (inserted)
                continue;

            Dqn_FArray_Add(&frontier, next_tile);
        }
    }
    Dqn_Profiler_EndZone(zone_astar_expand);

    #if 0
    TELY_Renderer *renderer = &platform->renderer;
    for (uint32_t old_index = 1 /*Sentinel*/; old_index < astar_info.occupied; old_index++) {
        Dqn_DSMapSlot<FP_GameAStarNode> const *slot = astar_info.slots + old_index;
        FP_GameAStarNode const                *node = &slot->value;
        Dqn_V2                                 pos  = FP_Game_TilePosToWorldPos(game, node->tile) + (game->tile_size * .5f);
        TELY_Render_CircleColourV4(renderer, pos, 4.f, TELY_RenderShapeMode_Fill, TELY_COLOUR_BLUE_CADET_V4);
    }
    #endif

    Dqn_usize slice_size = 0;
    for (Dqn_V2I it = last_successful_tile; it != src_tile; slice_size++) {
        uint64_t key_u64 = (DQN_CAST(uint64_t)it.y << 32) | (DQN_CAST(uint64_t)it.x << 0);
        it               = Dqn_DSMap_FindKeyU64(&astar_info, key_u64).value->came_from;
    }

    result     = Dqn_Slice_Alloc<Dqn_V2I>(arena, slice_size, Dqn_ZeroMem_No);
    slice_size = 0;
    for (Dqn_V2I it = last_successful_tile; it != src_tile; ) {
        result.data[slice_size++] = it;
        uint64_t key_u64          = (DQN_CAST(uint64_t)it.y << 32) | (DQN_CAST(uint64_t)it.x << 0);
        it                        = Dqn_DSMap_FindKeyU64(&astar_info, key_u64).value->came_from;
    }

    DQN_ASSERT(result.size == slice_size);
    return result;
}

static Dqn_V2 FP_Game_CalcWaypointWorldPos(FP_Game *game, FP_GameWaypoint const *waypoint)
{
    Dqn_V2 result = {};
    if (!game || !waypoint)
        return result;

    FP_GameEntity *waypoint_entity = FP_Game_GetEntity(game, waypoint->entity);
    if (FP_Game_IsNilEntity(waypoint_entity))
        return result;

    // NOTE: We found a waypoint that is valid to move towards
    switch (waypoint->type) {
        case FP_GameWaypointType_At: {
            result = FP_Game_CalcEntityWorldPos(game, waypoint_entity->handle);
        } break;

        case FP_GameWaypointType_Side: {
            Dqn_Rect entity_rect = FP_Game_CalcEntityWorldHitBox(game, waypoint_entity->handle);
            switch (waypoint->type_direction) {
                case FP_GameDirection_Up: {
                    result = Dqn_V2_InitNx2(entity_rect.pos.x + entity_rect.size.w * .5f, entity_rect.pos.y - entity_rect.size.h * .1f);
                } break;

                case FP_GameDirection_Down: {
                    result = Dqn_V2_InitNx2(entity_rect.pos.x + entity_rect.size.w * .5f, entity_rect.pos.y + entity_rect.size.h + entity_rect.size.h * .1f);
                } break;

                case FP_GameDirection_Left: {
                    result = Dqn_V2_InitNx2(entity_rect.pos.x - entity_rect.size.w * .1f, entity_rect.pos.y + entity_rect.size.h * .5f);
                } break;

                case FP_GameDirection_Right: {
                    result = Dqn_V2_InitNx2(entity_rect.pos.x + entity_rect.size.w + entity_rect.size.w * .1f, entity_rect.pos.y + entity_rect.size.h * .5f);
                } break;

                case FP_GameDirection_Count: DQN_INVALID_CODE_PATH; break;
            }
        } break;

        case FP_GameWaypointType_Offset: {
            result = FP_Game_CalcEntityWorldPos(game, waypoint_entity->handle) + waypoint->offset;
        } break;
    }

    return result;
}

FP_GameFindClosestEntityResult FP_Game_FindClosestEntityWithType(FP_Game *game, FP_GameEntityHandle src_entity_handle, FP_EntityType type)
{
    FP_GameFindClosestEntityResult result = {};
    FP_GameEntity *src_entity             = FP_Game_GetEntity(game, src_entity_handle);
    if (FP_Game_IsNilEntity(src_entity))
        return result;

    Dqn_V2 src_pos      = FP_Game_CalcEntityWorldPos(game, src_entity_handle);
    result.dist_squared = DQN_F32_MAX;
    result.pos          = Dqn_V2_InitNx1(DQN_F32_MAX);

    for (FP_GameEntityIterator it = {}; FP_Game_DFSPostOrderWalkEntityTree(game, &it, game->root_entity); ) {
        FP_GameEntity *it_entity = it.entity;
        if (it_entity->type != type)
            continue;

        Dqn_V2 pos   = FP_Game_CalcEntityWorldPos(game, it_entity->handle);
        Dqn_f32 dist = Dqn_V2_LengthSq_V2x2(pos, src_pos);
        if (dist < result.dist_squared) {
            result.pos          = pos;
            result.dist_squared = dist;
            result.entity       = it_entity->handle;
        }
    }

    return result;
}

static bool FP_Game_CanEntityAttack(FP_GameEntity *entity, uint64_t current_time_ms)
{
    bool result = (current_time_ms - entity->last_attack_timestamp) >= entity->attack_cooldown_ms;

    return result;
}

static void FP_Game_EntityTransitionState(FP_Game *game, FP_GameEntity *entity, uint32_t desired_state)
{
    switch (entity->type) {
        case FP_EntityType_Terry: {
            if (desired_state == FP_EntityTerryState_Attack) {
                if (!FP_Game_CanEntityAttack(entity, game->clock_ms)) {
                    // NOTE: Cooldown not met do not transition
                    return;
                }
                entity->last_attack_timestamp = game->clock_ms;
            }
        } break;

        case FP_EntityType_Smoochie: {
            if (desired_state == FP_EntitySmoochieState_Attack) {
                if (!FP_Game_CanEntityAttack(entity, game->clock_ms)) {
                    // NOTE: Cooldown not met do not transition
                    return;
                }
                entity->last_attack_timestamp = game->clock_ms;
            }
        } break;

        case FP_EntityType_Clinger: {
            if (desired_state == FP_EntityClingerState_Attack) {
                if (!FP_Game_CanEntityAttack(entity, game->clock_ms)) {
                    // NOTE: Cooldown not met do not transition
                    return;
                }
                entity->last_attack_timestamp = game->clock_ms;
            }
        } break;
    }
    // NOTE: If no returns are hit above we proceed with the state change
    entity->action.next_state = desired_state;
}