#include "gjk.h"
#include "math.h"
#include "arena.h"
#include "scratch.h"

#if GJK_DEBUG
u32 gjk_debug_steps = U32_MAX;
#endif

#define DBGSTEP if (dbg_step++ >= gjk_debug_steps) goto abort

INTERNAL struct v2 poly_support(struct v2_array a, struct v2 dir)
{
    /* TODO: Could probably binary search for largest dot since shape is convex */
    struct v2 furthest = a.points[0];
    f32 furthest_dot = v2_dot(dir, furthest);
    for (u32 i = 1; i < a.count; ++i) {
        struct v2 p = a.points[i];
        f32 dot = v2_dot(dir, p);
        if (dot > furthest_dot) {
            furthest = p;
            furthest_dot = dot;
        }
    }
    return furthest;
}

INTERNAL struct v2 menkowski_point(struct v2_array poly0, struct v2_array shape1, struct v2 dir)
{
    return v2_sub(poly_support(poly0, dir), poly_support(shape1, v2_neg(dir)));
}

b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1)
{
    struct { struct v2 a, b, c; } s = { 0 };

    /* First point is support point in shape's general directions to eachother */
    s.a = menkowski_point(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]));

    /* Second point is support point towards origin */
    struct v2 dir = v2_neg(s.a);
    struct v2 p = menkowski_point(shape0, shape1, dir);
    if (v2_dot(dir, p) >= 0) {
        s.b = s.a;
        s.a = p;
        while (true) {
            /* Third point is support point in direction of line normal towards origin */
            dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a));
            p = menkowski_point(shape0, shape1, dir);
            if (v2_dot(dir, p) < 0) {
                /* New point did not cross origin, collision impossible */
                break;
            }

            s.c = s.b;
            s.b = s.a;
            s.a = p;

            dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(v2_sub(s.c, s.a)));  /* Normal of ab pointing away from c */
            struct v2 a_to_origin = v2_neg(s.a);
            if (v2_dot(dir, a_to_origin) >= 0) {
                /* Point is in region ab, remove c from simplex (will happen automatically next iteration) */
            } else {
                /* Point is not in region ab */
                dir = v2_perp_towards_dir(v2_sub(s.c, s.a), v2_neg(v2_sub(s.b, s.a)));  /* Normal of ac pointing away from b */
                if (v2_dot(dir, a_to_origin) >= 0) {
                    /* Point is in region ac, remove b from simplex */
                    s.b = s.c;
                } else {
                    /* Point is in simplex */
                    return true;
                }
            }
        }
    }

    return false;
}

INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array poly0, struct v2_array poly1, struct v2 dir)
{
    struct gjk_menkowski_point res;
    res.p0 = poly_support(poly0, dir);
    res.p1 = poly_support(poly1, v2_neg(dir));
    res.p = v2_sub(res.p0, res.p1);
    return res;
}

struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1)
{
    struct temp_arena scratch = scratch_begin_no_conflict();
    struct gjk_extended_result res = { 0 };

    /* TODO: Verify epsilon */
    f32 epsilon = 0.0000100;
    struct gjk_simplex s = { 0 };
    b32 colliding = false;
    struct v2 shape0_p = { 0 };
    struct v2 shape1_p = { 0 };
    struct gjk_menkowski_point *proto = NULL;
    u32 proto_count = 0;

#if GJK_DEBUG
    u32 dbg_step = 0;
#endif

    /* ========================== *
     * GJK collision check
     * Construct encapsulating simplex OR closest feature if not colliding
     * ========================== */

    struct v2 dir = { 0 };
    struct gjk_menkowski_point m = { 0 };
    /* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */
    {
        /* First point is support point in shape's general directions to eachother */
        s.a = menkowski_point_extended(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]));
        s.len = 1;

        while (!colliding) {
            if (s.len == 1) {
                /* Second point is support point towards origin */
                dir = v2_neg(s.a.p);

                DBGSTEP;
                m = menkowski_point_extended(shape0, shape1, dir);
                if (v2_eq(m.p, s.a.p)) {
                    /* Point is the same */
                    break;
                }
                s.b = s.a;
                s.a = m;
                s.len = 2;
                if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
                    /* New ab lies on origin */
                    break;
                }

                /* Third point is support point in direction of line normal towards origin */
                dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p));
            }

            {
                DBGSTEP;
                m = menkowski_point_extended(shape0, shape1, dir);
                if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) {
                    /* New point is on existing line ab */
                    break;
                }
                s.c = s.b;
                s.b = s.a;
                s.a = m;
                s.len = 3;
                if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
                    /* New ab lies on origin */
                    s.len = 2;
                    break;
                } else if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
                    /* New ac lies on origin */
                    s.b = s.c;
                    s.len = 2;
                    break;
                }
            }

            /* Determine voronoi region of the simplex in which the origin lies */
            i32 voronoi_mask = 0;
            struct v2 vab = v2_sub(s.b.p, s.a.p);
            struct v2 vac = v2_sub(s.c.p, s.a.p);
            struct v2 vbc = v2_sub(s.c.p, s.b.p);
            struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac));
            struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab));
            struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab);
            voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0;  /* Regions ab, a, and b*/
            voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1;  /* Regions ac, a, and c */
            voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2;  /* Regions bc, b, and c */
            /* Remove point or edge and determine next direction based on voronoi region */
            switch (voronoi_mask) {
                case 0: {  /* No region, must be in simplex */
                    colliding = true;
                } break;
                case 1: {  /* Region ab, remove c */
                    dir = rab_dir;  /* Next third point is in direction of region ab */
                    s.len = 2;
                } break;
                case 2: {  /* Region ac, remove b */
                    dir = rac_dir;  /* Next third point is in direction of region ac */
                    s.b = s.c;
                    s.len = 2;
                } break;
                case 4: {  /* Region bc, remove a */
                    dir = rbc_dir;  /* Next third point is in direction of region bc */
                    s.a = s.b;
                    s.b = s.c;
                    s.len = 2;
                } break;
                case 3: {  /* Region a, remove bc */
                    s.len = 1;
                } break;
                case 5: {  /* Region b, remove ac */
                    s.a = s.b;
                    s.len = 1;
                } break;
                case 6: {  /* Region c, remove ab */
                    s.a = s.c;
                    s.len = 1;
                } break;
            }
        }
    }

    if (colliding) {
        /* ========================== *
         * Epa
         * ========================== */
        proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
        proto_count = 0;
        {
            ASSERT(s.len == 3);
            struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
            tmp[0] = s.a;
            tmp[1] = s.b;
            tmp[2] = s.c;
            proto_count = 3;
        }

        /* ========================== *
            * Expand simplex towards closest edge to origin inside menkowski
            * ========================== */

        while (true) {
            f32 pen_len_sq = F32_INFINITY;

            /* Find dir from origin to closest edge */
            /* FIXME: Winding order of ps & pe index */
            u32 pen_ps_index = 0;
            u32 pen_pe_index = 0;
            for (u32 i = 0; i < proto_count; ++i) {
                u32 ps_index = i;
                u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
                struct v2 ps = proto[ps_index].p;
                struct v2 pe = proto[pe_index].p;

                struct v2 vse = v2_sub(pe, ps);
                struct v2 vso = v2_neg(ps);

                f32 d1 = v2_dot(vso, vse);
                f32 d2 = v2_dot(vse, vse);
                struct v2 vsd = v2_mul(vse, (d1 / d2));
                struct v2 pd = v2_add(ps, vsd);

                f32 pd_len_sq = v2_len_sq(pd);
                if (pd_len_sq < pen_len_sq) {
                    pen_ps_index = ps_index;
                    pen_pe_index = pe_index;
                    pen_len_sq = pd_len_sq;
                    dir = pd;
                }
            }

            /* TODO: Move to break (debugging) */
            s.a = proto[pen_ps_index];
            s.b = proto[pen_pe_index];
            s.len = 2;

            /* Find new point in dir */
            m = menkowski_point_extended(shape0, shape1, dir);

            /* Check unique */
            /* TODO: Better */
            DBGSTEP;
            {
                b32 unique = true;
                for (u32 i = 0; i < proto_count; ++i) {
                    struct v2 edge_start = proto[i].p;
                    struct v2 edge_end = i < proto_count - 1 ? proto[i + 1].p : proto[0].p;
                    if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m.p, edge_start))) < epsilon) {
                        unique = false;
                        break;
                    }
                }
                if (!unique) {
                    break;
                }
            }

            /* Insert point into prototype */
            /* FIXME: Preserve winding order */
            arena_push(scratch.arena, struct gjk_menkowski_point);
            ++proto_count;
            for (u32 i = proto_count - 1; i > pen_pe_index; --i) {
                u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
                u32 shift_to = i;
                proto[shift_to] = proto[shift_from];
            }
            proto[pen_pe_index] = m;
        }
    }

    /* Resolve points */
    if (s.len == 1) {
        shape0_p = s.a.p0;
        shape1_p = s.a.p1;
    } else if (s.len == 2) {
        /* FIXME: Winding order dependent? */
        ASSERT(s.len == 2);
        f32 ratio;
        {
            /* Determine ratio between edge a & b that projected origin lies */
            struct v2 vab = v2_sub(s.b.p, s.a.p);
            struct v2 vao = v2_neg(s.a.p);
            ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
        }
        /* Shape 0 */
        shape0_p = v2_sub(s.b.p0, s.a.p0);
        shape0_p = v2_mul(shape0_p, ratio);
        shape0_p = v2_add(shape0_p, s.a.p0);
        /* Shape 1 */
        shape1_p = v2_sub(s.b.p1, s.a.p1);
        shape1_p = v2_mul(shape1_p, ratio);
        shape1_p = v2_add(shape1_p, s.a.p1);
    }

    res.solved = true;
abort:
    if (proto_count > 0) {
        for (u32 i = 0; i < min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); ++i) {
            res.prototype.points[i] = proto[i].p;
        }
        res.prototype.len = proto_count;
    } else {
        if (s.len >= 1) {
            res.prototype.points[0] = s.a.p;
            if (s.len >= 2) {
                res.prototype.points[1] = s.b.p;
                if (s.len >= 3) {
                    res.prototype.points[2] = s.c.p;
                }
            }
        }
        res.prototype.len = s.len;
    }
    res.colliding = colliding;
    res.p0 = shape0_p;
    res.p1 = shape1_p;
    res.simplex = s;
    scratch_end(scratch);
    return res;
}

/* TODO: Remove this (debugging) */
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1)
{
    struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
    u64 rays = 500;
    for (u64 i = 0; i < rays; ++i) {
        f32 angle = ((f32)i / rays) * (2 * PI);
        struct v2 dir = v2_from_angle(angle);
        struct v2 p = menkowski_point_extended(poly0, poly1, dir).p;
        if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
            *arena_push(arena, struct v2) = p;
            ++res.count;
        }
    }
    return res;
}

/* TODO: Remove this (debugging) */
struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1)
{
    struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
    for (u64 i = 0; i < poly0.count; ++i) {
        struct v2 p0 = poly0.points[i];
        for (u64 j = 0; j < poly1.count; ++j) {
            struct v2 p1 = poly1.points[j];
            *arena_push(arena, struct v2) = v2_sub(p0, p1);
            ++res.count;
        }
    }

    return res;
}









/* ========================== *
 * Swept GJK (unused)
 * ========================== */

#if 0

struct gjk_swept_result {
    b32 colliding;
    struct v2 p0, p1;  /* Closest points on each shape */

    /* For debugging */
    struct gjk_simplex simplex;
    b32 velocity_intersects;
    b32 solved;
    struct gjk_prototype prototype;
};

struct poly_support_swept_result {
    struct v2 p;
    struct v2 original;
};

INTERNAL struct poly_support_swept_result poly_support_swept(struct v2_array a, struct v2 dir, struct v2 linear_velocity)
{
    /* TODO: Could probably binary search for largest dot since shape is convex */
    struct v2 furthest = V2(0, 0);
    struct v2 furthest_original = V2(0, 0);
    f32 furthest_dot = -F32_INFINITY;

    for (u32 i = 0; i < a.count; ++i) {
        struct v2 p = a.points[i];
        f32 dot = v2_dot(dir, p);
        if (dot > furthest_dot) {
            furthest = p;
            furthest_original = p;
            furthest_dot = dot;
        }
    }

    for (u32 i = 0; i < a.count; ++i) {
        struct v2 p = a.points[i];
        struct v2 modified;
#if 0
        if (v2_dot(linear_velocity, dir) > 0) {
            modified = v2_add(p, linear_velocity);
        } else {
            modified = p;
    }
#else
        modified = v2_add(p, linear_velocity);
#endif
        f32 dot = v2_dot(dir, modified);
        if (dot > furthest_dot) {
            furthest = modified;
            furthest_original = p;
            furthest_dot = dot;
        }
    }

    struct poly_support_swept_result res = { 0 };
    res.p = furthest;
    res.original = furthest_original;
    return res;
}

INTERNAL struct gjk_menkowski_point menkowski_point_extended_swept(struct v2_array poly0, struct v2_array poly1, struct v2 dir, struct v2 linear_velocity)
{
    struct gjk_menkowski_point res;
    struct poly_support_swept_result res0 = poly_support_swept(poly0, dir, linear_velocity);
    struct poly_support_swept_result res1 = poly_support_swept(poly1, v2_neg(dir), V2(0, 0));
    res.p0 = res0.original;
    res.p1 = res1.original;
    res.p = v2_sub(res0.p, res1.p);
    return res;
}

struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 linear_velocity)
{
    struct temp_arena scratch = scratch_begin_no_conflict();
    struct gjk_extended_result res = { 0 };

    /* FIXME: Divs by 0 */

    /* TODO: Verify epsilon */
    f32 epsilon = 0.00001;
    struct gjk_simplex s = {
        .len = 3,
        .a = V2(F32_NAN, F32_NAN),
        .b = V2(F32_NAN, F32_NAN),
        .c = V2(F32_NAN, F32_NAN)
    };
    b32 colliding = false;
    struct v2 shape0_p = { 0 };
    struct v2 shape1_p = { 0 };
    b32 velocity_intersects = false;
    f32 velocity_intersection = 0;

    /* TODO: Move this back down */
    struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
    u32 proto_count = 0;

#if GJK_DEBUG
    u32 dbg_step = 0;
#endif

    /* ========================== *
     * GJK collision check
     * Construct encapsulating simplex OR closest feature if not colliding
     * ========================== */

    struct v2 dir = { 0 };
    struct gjk_menkowski_point m = { 0 };
    /* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */
    {
        /* First point is support point in shape's general directions to eachother */
        s.a = menkowski_point_extended_swept(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]), linear_velocity);
        s.len = 1;

        while (!colliding) {
            if (s.len == 1) {
                /* Second point is support point towards origin */
                dir = v2_neg(s.a.p);

                DBGSTEP;
                m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity);
                if (v2_eq(m.p, s.a.p)) {
                    /* Point is the same */
                    break;
                }
                s.b = s.a;
                s.a = m;
                s.len = 2;
                if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
                    /* New ab lies on origin */
                    break;
                }
                /* Third point is support point in direction of line normal towards origin */
                dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p));
            }

            {
                DBGSTEP;
                m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity);
                if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) {
                    /* New point is on existing line ab */
                    break;
                }
                if (v2_eq(m.p, s.a.p) || v2_eq(m.p, s.b.p) || v2_eq(m.p, s.c.p)) {
                    /* New point is existing c */
                    s.b = s.a;
                    s.a = m;
                    s.len = 2;
                    break;
                }
                s.c = s.b;
                s.b = s.a;
                s.a = m;
                s.len = 3;
                if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
                    /* New ab lies on origin */
                    s.len = 2;
                    break;
                }
                if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) {
                    /* New ac lies on origin */
                    s.b = s.c;
                    s.len = 2;
                    break;
                }
            }

            DBGSTEP;
            /* Determine voronoi region of the simplex in which the origin lies */
            i32 voronoi_mask = 0;
            struct v2 vab = v2_sub(s.b.p, s.a.p);
            struct v2 vac = v2_sub(s.c.p, s.a.p);
            struct v2 vbc = v2_sub(s.c.p, s.b.p);
            struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac));
            struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab));
            struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab);
            voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0;  /* Regions ab, a, and b*/
            voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1;  /* Regions ac, a, and c */
            voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2;  /* Regions bc, b, and c */
            /* Remove point or edge and determine next direction based on voronoi region */
            switch (voronoi_mask) {
                default: {  /* No region, must be in simplex */
                    colliding = true;
                } break;
                case 1: {  /* Region ab, remove c */
                    dir = rab_dir;  /* Next third point is in direction of region ab */
                    s.len = 2;
                } break;
                case 2: {  /* Region ac, remove b */
                    dir = rac_dir;  /* Next third point is in direction of region ac */
                    s.b = s.c;
                    s.len = 2;
                } break;
                case 4: {  /* Region bc, remove a */
                    dir = rbc_dir;  /* Next third point is in direction of region bc */
                    s.a = s.b;
                    s.b = s.c;
                    s.len = 2;
                } break;
                case 3: {  /* Region a, remove bc */
                    s.len = 1;
                } break;
                case 5: {  /* Region b, remove ac */
                    s.a = s.b;
                    s.len = 1;
                } break;
                case 6: {  /* Region c, remove ab */
                    s.a = s.c;
                    s.len = 1;
                } break;
            }
        }
    }

    if (colliding) {
        /* ========================== *
         * Epa
         * ========================== */
        proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point);
        proto_count = 0;
        {
            ASSERT(s.len == 3);
            struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3);
            tmp[0] = s.a;
            tmp[1] = s.b;
            tmp[2] = s.c;
            proto_count = 3;
        }

        if (v2_eq(linear_velocity, V2(0, 0))) {
            /* ========================== *
             * Expand simplex towards closest edge to origin inside menkowski
             * ========================== */

            while (true) {
                DBGSTEP;
                f32 pen_len_sq = F32_INFINITY;

                /* Find dir from origin to closest edge */
                /* FIXME: Winding order of ps & pe index */
                u32 pen_ps_index = 0;
                u32 pen_pe_index = 0;
                for (u32 i = 0; i < proto_count; ++i) {
                    u32 ps_index = i;
                    u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
                    struct v2 ps = proto[ps_index].p;
                    struct v2 pe = proto[pe_index].p;

                    struct v2 vse = v2_sub(pe, ps);
                    struct v2 vso = v2_neg(ps);

                    f32 d1 = v2_dot(vso, vse);
                    f32 d2 = v2_dot(vse, vse);
                    struct v2 vsd = v2_mul(vse, (d1 / d2));
                    struct v2 pd = v2_add(ps, vsd);

                    f32 pd_len_sq = v2_len_sq(pd);
                    if (pd_len_sq < pen_len_sq) {
                        pen_ps_index = ps_index;
                        pen_pe_index = pe_index;
                        pen_len_sq = pd_len_sq;
                        dir = pd;
                    }
                }

                /* TODO: Move to break (debugging) */
                s.a = proto[pen_ps_index];
                s.b = proto[pen_pe_index];
                s.len = 2;

                /* Find new point in dir */
                m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity);

                /* Check unique */
                /* TODO: Better */
                {
                    b32 unique = true;
                    for (u32 i = 0; i < proto_count; ++i) {
                        struct v2 edge_start = proto[i].p;
                        struct v2 edge_end = i < proto_count - 1 ? proto[i + 1].p : proto[0].p;
                        if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m.p, edge_start))) < epsilon) {
                            unique = false;
                            break;
                        }
                    }
                    if (!unique) {
                        break;
                    }
                }

                /* Insert point into prototype */
                /* FIXME: Preserve winding order */
                arena_push(scratch.arena, struct gjk_menkowski_point);
                ++proto_count;
                for (u32 i = proto_count - 1; i > pen_pe_index; --i) {
                    u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
                    u32 shift_to = i;
                    proto[shift_to] = proto[shift_from];
                }
                proto[pen_pe_index] = m;
            }
        } else {
            /* ========================== *
             * Expand simplex towards furthest edge along velocity
             * ========================== */

            while (true) {
                DBGSTEP;

                /* FIXME: Winding order of ps & pe index */
                f32 highest_score = -F32_INFINITY;
                u32 picked_start_index = 0;
                u32 picked_end_index = 0;
                b32 picked_intersects = false;
                f32 picked_intersection = 0;
                for (u32 i = 0; i < proto_count; ++i) {
                    u32 ps_index = i;
                    u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0;
                    struct v2 ps = proto[ps_index].p;
                    struct v2 pe = proto[pe_index].p;

                    struct v2 vse = v2_sub(pe, ps);
                    struct v2 vso = v2_neg(ps);

                    f32 w = v2_wedge(vse, linear_velocity);
                    f32 t1 = 0;
                    f32 t2 = 0;
                    b32 intersects = false;
                    if (w != 0) {
                        w = 1 / w;
                        t1 = v2_wedge(vso, linear_velocity) * w;
                        t2 = v2_wedge(vso, vse) * w;
                        intersects = -epsilon < t1 && t1 < 1 + epsilon && -epsilon < t2 && t2 < 1 + epsilon;
                    }

                    struct v2 next_dir = dir;
                    f32 score = highest_score;
                    if (intersects) {
                        /* Score is intersection (+1 to ensure intersections get priority) */
                        score = 1 + t1;
                        next_dir = v2_perp_towards_dir(vse, linear_velocity);
                    } else {
                        /* Score is inverse projection len */
                        struct v2 vsv = v2_sub(linear_velocity, ps);
                        struct v2 vsd = v2_mul(vse, v2_dot(vse, vsv) / v2_dot(vse, vse));
                        f32 len = v2_len(v2_sub(vsd, vsv));
                        score = 1.0 - len;
                        next_dir = v2_perp_towards_dir(vse, ps);
                    }

                    if (score > highest_score) {
                        picked_start_index = ps_index;
                        picked_end_index = pe_index;
                        highest_score = score;
                        picked_intersects = intersects;
                        picked_intersection = t1;
                        dir = next_dir;
                    }
                }

                /* TODO: Move to break (debugging) */
                velocity_intersects = picked_intersects;
                velocity_intersection = picked_intersection;
                s.a = proto[picked_start_index];
                s.b = proto[picked_end_index];
                s.len = 2;

                /* Find new point in dir */
                m = menkowski_point_extended_swept(shape0, shape1, dir, linear_velocity);

                /* Check unique */
                /* TODO: Better */
                /* FIXME: Winding order */
                u32 insert_start_index = 0;
                u32 insert_end_index = 1;
                {
                    b32 unique = true;
                    for (u32 i = 0; i < proto_count; ++i) {
                        u32 start_index = i;
                        u32 end_index = i < proto_count - 1 ? i + 1 : 0;
                        struct v2 edge_start = proto[start_index].p;
                        struct v2 edge_end = proto[end_index].p;

                        struct v2 vso = v2_neg(edge_start);
                        struct v2 vse = v2_sub(edge_end, edge_start);
                        struct v2 vsm = v2_sub(m.p, edge_start);

                        f32 w = v2_wedge(vse, vsm);
                        if (math_fabs(w) > epsilon) {
                            w = v2_wedge(vse, m.p);
                            if (w != 0) {
                                w = 1 / w;
                                f32 t1 = v2_wedge(vso, m.p) * w;
                                f32 t2 = v2_wedge(vso, vse) * w;
                                b32 intersects = 0 <= t1 && t1 <= 1 && 0 <= t2 && t2 <= 1;
                                if (intersects) {
                                    insert_start_index = start_index;
                                    insert_end_index = end_index;
                                }
                            }
                        } else {
                            unique = false;
                            break;
                        }
                    }
                    if (!unique) {
                        break;
                    }
                }

                (UNUSED)insert_start_index;
                DBGSTEP;
                /* Insert point into prototype */
                /* FIXME: Preserve winding order */
                arena_push(scratch.arena, struct v2);
                ++proto_count;
                for (u32 i = proto_count - 1; i > insert_end_index; --i) {
                    u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
                    u32 shift_to = i;
                    proto[shift_to] = proto[shift_from];
                }
                proto[insert_end_index] = m;
            }
        }
    }

    /* Resolve points */
    if (s.len == 1) {
        ASSERT(!colliding);  /* Is this possible? Would make calculating collision normal tricky. */
        shape0_p = s.a.p0;
        shape1_p = s.a.p1;
    } else {
        ASSERT(s.len == 2);
        /* FIXME: Winding order dependent? */
        f32 ratio;
        if (colliding && !v2_eq(linear_velocity, V2(0, 0))) {
            if (velocity_intersects) {
                /* Ratio between edge a & b that velocity intersection lies */
                ratio = velocity_intersection;
            } else {
                /* Ratio between edge a & b that projected velocity lies */
                struct v2 vab = v2_sub(s.b.p, s.a.p);
                struct v2 vap = v2_sub(linear_velocity, s.a.p);
                ratio = clamp_f32(v2_dot(vab, vap) / v2_dot(vab, vab), 0, 1);
            }
        } else {
            /* Determine ratio between edge a & b that projected origin lies */
            struct v2 vab = v2_sub(s.b.p, s.a.p);
            struct v2 vao = v2_neg(s.a.p);
            ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
        }
        /* Shape 0 */
        shape0_p = v2_sub(s.b.p0, s.a.p0);
        shape0_p = v2_mul(shape0_p, ratio);
        shape0_p = v2_add(shape0_p, s.a.p0);
        /* Shape 1 */
        shape1_p = v2_sub(s.b.p1, s.a.p1);
        shape1_p = v2_mul(shape1_p, ratio);
        shape1_p = v2_add(shape1_p, s.a.p1);
    }


    res.solved = true;
abort:
    if (proto_count > 0) {
        for (u32 i = 0; i < min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); ++i) {
            res.prototype.points[i] = proto[i].p;
        }
        res.prototype.len = proto_count;
    } else {
        if (s.len >= 1) {
            res.prototype.points[0] = s.a.p;
            if (s.len >= 2) {
                res.prototype.points[1] = s.b.p;
                if (s.len >= 3) {
                    res.prototype.points[2] = s.c.p;
                }
            }
        }
        res.prototype.len = s.len;
    }
    res.colliding = colliding;
    res.p0 = shape0_p;
    res.p1 = shape1_p;
    res.simplex = s;
    res.velocity_intersects = velocity_intersects;
    scratch_end(scratch);
    return res;
}


/* TODO: Remove this (debugging) */
struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity)
{
    struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
    u64 rays = 500;
    for (u64 i = 0; i < rays; ++i) {
        f32 angle = ((f32)i / rays) * (2 * PI);
        struct v2 dir = v2_from_angle(angle);
        struct v2 p = menkowski_point_extended_swept(poly0, poly1, dir, linear_velocity).p;
        if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) {
            *arena_push(arena, struct v2) = p;
            ++res.count;
        }
    }
    return res;
}

/* TODO: Remove this (debugging) */
struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct v2 linear_velocity)
{
    struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
    for (u64 i = 0; i < poly0.count; ++i) {
        struct v2 p0 = poly0.points[i];
        for (u64 j = 0; j < poly1.count; ++j) {
            struct v2 p1 = poly1.points[j];
            struct v2 diff = v2_sub(p0, p1);
            {
                *arena_push(arena, struct v2) = diff;
                ++res.count;
            }
            {
                *arena_push(arena, struct v2) = v2_add(diff, linear_velocity);
                ++res.count;
            }
        }
    }

    return res;
}

#endif