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

#if COLLIDER_DEBUG
u32 collider_debug_steps = U32_MAX;
//u32 collider_debug_steps = 1000000;
//u32 collider_debug_steps = 50;
#endif

INTERNAL void _dbgbreakable(void)
{
    DEBUGBREAKABLE;
}

#define DBGSTEP \
    dbg_step++; \
    if (dbg_step >= collider_debug_steps) { \
        goto abort; \
    } else if (dbg_step >= collider_debug_steps - 1) { \
        _dbgbreakable(); \
    }

struct v2 collider_support_point(struct collider_shape *a, struct xform xf, struct v2 dir)
{
    struct v2 *points = a->points;
    u32 count = a->count;
    f32 radius = a->radius;

    /* TODO: Could probably binary search for largest dot since shape is convex */
    struct v2 furthest = ZI;
    f32 furthest_dot = -F32_INFINITY;
    for (u32 i = 0; i < count; ++i) {
        struct v2 p = xform_mul_v2(xf, points[i]);
        f32 dot = v2_dot(dir, p);
        if (dot > furthest_dot) {
            furthest = p;
            furthest_dot = dot;
        }
    }

    if (radius > 0.0) {
        dir = v2_mul_v2(v2_with_len(dir, radius), xform_get_scale(xf));
        furthest = v2_add(furthest, dir);
    }

    return furthest;
}

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

INTERNAL struct v2 menkowski_point(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct v2 dir)
{
    return v2_sub(collider_support_point(shape0, xf0, dir), collider_support_point(shape1, xf1, v2_neg(dir)));
}

struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
{
    struct temp_arena scratch = scratch_begin_no_conflict();  /* TODO: Only begin scratch for EPA */
    struct collider_collision_points_result res = ZI;

    struct v2 *points0 = shape0->points;
    struct v2 *points1 = shape1->points;
    u32 count0 = shape0->count;
    u32 count1 = shape1->count;
    f32 radius0 = shape0->radius;
    f32 radius1 = shape1->radius;
    (UNUSED)radius0;
    (UNUSED)radius1;

    /* TODO: Parameterize */
    const f32 tolerance = 0.005f;  /* How close can shapes be before collision is considered */
    //const f32 tolerance = 0.05f;  /* How close can shapes be before collision is considered */
    const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f;
    const u32 max_epa_iterations = 64;  /* To prevent extremely large prototypes when origin is in exact center of rounded feature */

    b32 colliding = false;
    b32 simplex_is_closest_edge = false;

    struct collider_simplex s = ZI;
    struct v2 *proto = NULL;
    u32 proto_count = 0;

    struct v2 normal = ZI;
    struct collider_collision_point points[2] = ZI;
    u32 num_points = 0;

    struct v2 dir = ZI;
    struct v2 m = ZI;

#if COLLIDER_DEBUG
    u32 dbg_step = 0;
#endif

    /* ========================== *
     * GJK
     *
     * Determine encapsulating simplex if colliding, or closest edge / point to
     * origin on simplex (for check if shape distances are within tolerance)
     * ========================== */
    {
        /* First point is support point in shape's general directions to eachother */
        dir = v2_sub(xf1.og, xf0.og);
        if (v2_is_zero(dir)) dir = V2(1, 0);
        s.a = menkowski_point(shape0, shape1, xf0, xf1, dir);
        s.len = 1;

        struct v2 removed_a = ZI;
        struct v2 removed_b = ZI;
        u32 num_removed = 0;
        while (true) {
            if (s.len == 1) {
                /* Second point is support point towards origin */
                dir = v2_neg(s.a);

                DBGSTEP;
                m = menkowski_point(shape0, shape1, xf0, xf1, dir);
                /* Check that new point is far enough away from existing point */
                if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) {
                    simplex_is_closest_edge = true;
                    break;
                }
                s.b = s.a;
                s.a = m;
                s.len = 2;

                /* 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));
            }

            {
                DBGSTEP;
                m = menkowski_point(shape0, shape1, xf0, xf1, dir);
                /* Check that new point is far enough away from existing points */
                if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq ||
                    v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq ||
                    (
                        (num_removed >= 1) && (
                        (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) ||
                        (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq))
                    ) ||
                    math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq)
                {
                    simplex_is_closest_edge = true;
                    break;
                }
                s.c = s.b;
                s.b = s.a;
                s.a = m;
                s.len = 3;

                if (math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq ||
                    math_fabs(v2_wedge(v2_sub(s.c, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq ||
                    math_fabs(v2_wedge(v2_sub(s.c, s.b), v2_neg(s.b))) <= min_unique_pt_dist_sq) {
                    /* Simplex lies on origin */
                    colliding = true;
                    break;
                }
            }

            /* Determine region of the simplex in which the origin lies */
            DBGSTEP;
            struct v2 vab = v2_sub(s.b, s.a);
            struct v2 vac = v2_sub(s.c, s.a);
            struct v2 vbc = v2_sub(s.c, s.b);

            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);

            f32 rab_dot = v2_dot(rab_dir, v2_neg(s.a));
            f32 rac_dot = v2_dot(rac_dir, v2_neg(s.a));
            f32 rbc_dot = v2_dot(rbc_dir, v2_neg(s.b));

            f32 vab_dot = v2_dot(vab, v2_neg(s.a)) / v2_len_sq(vab);
            f32 vac_dot = v2_dot(vac, v2_neg(s.a)) / v2_len_sq(vac);
            f32 vbc_dot = v2_dot(vbc, v2_neg(s.b)) / v2_len_sq(vbc);

            if (rab_dot >= 0 && vab_dot >= 0 && vab_dot <= 1) {
                /* Region ab, remove c */
                num_removed = 1;
                removed_a = s.c;
                s.len = 2;
                dir = rab_dir;  /* Next third point is in direction of region ab */
            } else if (rac_dot >= 0 && vac_dot >= 0 && vac_dot <= 1) {
                /* Region ac, remove b */
                num_removed = 1;
                removed_a = s.b;
                s.len = 2;
                s.b = s.c;
                dir = rac_dir;  /* Next third point is in direction of region ac */
            } else if (rbc_dot >= 0 && vbc_dot >= 0 && vbc_dot <= 1) {
                /* Region bc, remove a */
                num_removed = 1;
                removed_a = s.a;
                s.len = 2;
                s.a = s.b;
                s.b = s.c;
                dir = rbc_dir;  /* Next third point is in direction of region bc */
            } else if (vab_dot <= 0 && vac_dot <= 0) {
                /* Region a, remove bc */
                num_removed = 2;
                removed_a = s.b;
                removed_b = s.c;
                s.len = 1;
            } else if (vab_dot >= 1 && vbc_dot <= 0) {
                /* Region b, remove ac */
                num_removed = 2;
                removed_a = s.a;
                removed_b = s.c;
                s.len = 1;
                s.a = s.b;
            } else if (vac_dot >= 1 && vbc_dot >= 1) {
                /* Region c, remove ab */
                num_removed = 2;
                removed_a = s.a;
                removed_b = s.b;
                s.len = 1;
                s.a = s.c;
            } else {
                /* No region, must be in simplex */
                colliding = true;
                break;
            }
        }
    }

    if (colliding) {
        /* ========================== *
         * Epa (to find collision normal from inside shape)
         * ========================== */

        const f32 epa_epsilon_sq = 0.001f * 0.001f;
        //const f32 epa_epsilon_sq = F32_INFINITY;

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

        u32 epa_iterations = 0;
        while (colliding) {
            ++epa_iterations;
            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;
            struct v2 pen = ZI;
            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];
                struct v2 pe = proto[pe_index];

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

                struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_len_sq(vse)));
                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;
                    pen = pd;
                }
            }

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

            /* Find new point in dir */
            DBGSTEP;
            {
                /* TODO: If winding order is guaranteed then this can become v2_perp_left/right? */
                struct v2 a = proto[pen_ps_index];
                struct v2 b = proto[pen_pe_index];
                struct v2 vab = v2_sub(b, a);
                if (pen_len_sq < epa_epsilon_sq) {
                    /* Next point is in direction of line normal pointing outwards from simplex */
                    struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0];  /* Next point along prototype after edge */
                    dir = v2_perp_towards_dir(vab, v2_sub(a, n));
                } else {
                    dir = v2_perp_towards_dir(vab, a);
                }

            }
            m = menkowski_point(shape0, shape1, xf0, xf1, dir);

            /* Check unique */
            /* TODO: Better */
            {
                b32 unique = true;
                for (u32 i = 0; i < proto_count; ++i) {
                    struct v2 edge_start = proto[i];
                    struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0];
                    struct v2 vsm = v2_sub(m, edge_start);
                    if (v2_len_sq(vsm) < min_unique_pt_dist_sq ||
                        math_fabs(v2_wedge(v2_sub(edge_end, edge_start), vsm)) < min_unique_pt_dist_sq) {
                        unique = false;
                        break;
                    }
                }
                if (!unique || epa_iterations >= max_epa_iterations) {
                    res.path = 1;
                    if (pen_len_sq < epa_epsilon_sq) {
                        normal = v2_norm(dir);
                    } else {
                        normal = v2_norm(pen);
                    }
                    break;
                }
            }

            /* Insert point into prototype */
            /* FIXME: Preserve winding order */
            arena_push(scratch.arena, struct collider_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 if (simplex_is_closest_edge) {
        if (s.len == 1) {
            struct v2 p = v2_neg(s.a);
            if (v2_len_sq(p) <= (tolerance * tolerance)) {
                res.path = 2;
                normal = v2_norm(dir);
                colliding = true;
            }
        } else {
            /* Shapes are not overlapping (origin is outside of simplex). Project
             * origin to determine if distance is within tolerance. */
            ASSERT(s.len == 2);
            struct v2 vab = v2_sub(s.b, s.a);
            struct v2 vao = v2_neg(s.a);
            f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
            struct v2 p = v2_add(s.a, v2_mul(vab, ratio));
            if (v2_len_sq(p) <= (tolerance * tolerance)) {
                res.path = 2;
                normal = v2_norm(dir);
                colliding = true;
            }
        }
    }

    if (colliding) {
        /* ========================== *
         * Clip to determine final points
         * ========================== */

         /* Max vertices must be < 16 to fit in 4 bit ids */
        CT_ASSERT(ARRAY_COUNT(shape0->points) <= 16);

        DBGSTEP;
        {
            const f32 wedge_epsilon = 0.001f;
            //const f32 wedge_epsilon = 0.1f;

            /* shape0 a -> b winding = clockwise */
            u32 id_a0;
            u32 id_b0;
            struct v2 a0;
            struct v2 b0;

            /* shape1 a -> b winding = counterclockwise */
            u32 id_a1;
            u32 id_b1;
            struct v2 a1;
            struct v2 b1;
            {
                u32 p_i = collider_support_point_index(shape0, xf0, normal);
                u32 a_i = (p_i > 0) ? (p_i - 1) : (count0 - 1);
                u32 b_i = ((p_i + 1) < count0) ? (p_i + 1) : 0;

                struct v2 p = xform_mul_v2(xf0, points0[p_i]);
                struct v2 a = xform_mul_v2(xf0, points0[a_i]);
                struct v2 b = xform_mul_v2(xf0, points0[b_i]);

                struct v2 vap = v2_sub(p, a);
                struct v2 vpb = v2_sub(b, p);

#if 1
                /* Swap a & b depending on winding order */
                if (v2_wedge(vap, vpb) < 0) {
                    u32 tmp_u32 = a_i;
                    a_i = b_i;
                    b_i = tmp_u32;
                    struct v2 tmp_v2 = a;
                    a = b;
                    b = tmp_v2;
                    tmp_v2 = vap;
                    vap = v2_neg(vpb);
                    vpb = v2_neg(tmp_v2);
                }
#endif

                f32 vap_wedge = v2_wedge(vap, normal);
                f32 vpb_wedge = v2_wedge(vpb, normal);
                if (vap_wedge < (vpb_wedge + wedge_epsilon)) {
                    id_a0 = a_i;
                    id_b0 = p_i;
                    a0 = a;
                    b0 = p;
                } else {
                    id_a0 = p_i;
                    id_b0 = b_i;
                    a0 = p;
                    b0 = b;
                }
            }
            {
                struct v2 neg_normal = v2_neg(normal);

                u32 p_i = collider_support_point_index(shape1, xf1, neg_normal);
                u32 a_i = ((p_i + 1) < count1) ? (p_i + 1) : 0;
                u32 b_i = (p_i > 0) ? (p_i - 1) : (count1 - 1);

                struct v2 p = xform_mul_v2(xf1, points1[p_i]);
                struct v2 a = xform_mul_v2(xf1, points1[a_i]);
                struct v2 b = xform_mul_v2(xf1, points1[b_i]);

                struct v2 vap = v2_sub(p, a);
                struct v2 vpb = v2_sub(b, p);

#if 1
                /* Swap a & b depending on winding order */
                if (v2_wedge(vap, vpb) > 0) {
                    u32 tmp_u32 = a_i;
                    a_i = b_i;
                    b_i = tmp_u32;
                    struct v2 tmp_v2 = a;
                    a = b;
                    b = tmp_v2;
                    tmp_v2 = vap;
                    vap = v2_neg(vpb);
                    vpb = v2_neg(tmp_v2);
                }
#endif

                f32 vap_wedge = v2_wedge(vap, normal);
                f32 vpb_wedge = v2_wedge(vpb, normal);
                if (vap_wedge < (vpb_wedge + wedge_epsilon)) {
                    id_a1 = a_i;
                    id_b1 = p_i;
                    a1 = a;
                    b1 = p;
                } else {
                    id_a1 = p_i;
                    id_b1 = b_i;
                    a1 = p;
                    b1 = b;
                }
            }

#if 1
            if (radius0 > 0.0) {
                struct v2 scale = xform_get_scale(xf0);
                struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius0), scale);
                a0 = v2_add(a0, normal_radius);
                b0 = v2_add(b0, normal_radius);
            }

            if (radius1 > 0.0) {
                struct v2 scale = xform_get_scale(xf1);
                struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius1), scale);
                a1 = v2_sub(a1, normal_radius);
                b1 = v2_sub(b1, normal_radius);
            }
#endif

            f32 a0t = 0;
            f32 a1t = 0;
            f32 b0t = 0;
            f32 b1t = 0;

            struct v2 vab0 = v2_sub(b0, a0);
            struct v2 vab1 = v2_sub(b1, a1);

            {
                struct v2 va0a1 = v2_sub(a1, a0);
                struct v2 vb0b1 = v2_sub(b1, b0);

                f32 vab0_wedge_normal = v2_wedge(vab0, normal);
                f32 vab1_wedge_normal = v2_wedge(vab1, normal);
                f32 va0a1_wedge_normal = v2_wedge(va0a1, normal);
                f32 vb0b1_wedge_normal = v2_wedge(vb0b1, normal);

                if (math_fabs(vab0_wedge_normal) > 0.01f) {
                    f32 w = 1 / vab0_wedge_normal;
                    a0t = clamp_f32(va0a1_wedge_normal * w, 0, 1);
                    b0t = clamp_f32(vb0b1_wedge_normal * -w, 0, 1);
                }

                if (math_fabs(vab1_wedge_normal) > 0.01f) {
                    f32 w = 1 / vab1_wedge_normal;
                    a1t = clamp_f32(-va0a1_wedge_normal * w, 0, 1);
                    b1t = clamp_f32(-vb0b1_wedge_normal * -w, 0, 1);
                }
            }

            struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t));
            struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t));
            struct v2 b0_clipped = v2_add(b0, v2_mul(vab0, -b0t));
            struct v2 b1_clipped = v2_add(b1, v2_mul(vab1, -b1t));

            struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped);
            struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped);

            f32 a_sep = v2_dot(va0a1_clipped, normal);
            f32 b_sep = v2_dot(vb0b1_clipped, normal);

            struct v2 contact_a = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f));
            struct v2 contact_b = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f));

            if (a_sep < tolerance) {
                struct collider_collision_point *point = &points[num_points++];
                point->id = id_a0 | (id_a1 << 4);
                point->separation = a_sep;
                point->point = contact_a;
            }

            if (b_sep < tolerance) {
                struct collider_collision_point *point = &points[num_points++];
                point->id = id_b0 | (id_b1 << 4);
                point->separation = b_sep;
                point->point = contact_b;
            }

            res.a0 = a0_clipped;
            res.a1 = a1_clipped;
            res.b0 = b0_clipped;
            res.b1 = b1_clipped;
        }
    }

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

/* ========================== *
 * Debug functions
 * TODO: Remove these
 * ========================== */

 /* TODO: Remove this (debugging) */
struct v2_array menkowski(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, u32 detail)
{
    struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
    for (u64 i = 0; i < detail; ++i) {
        f32 angle = ((f32)i / detail) * (2 * PI);
        struct v2 dir = v2_from_angle(angle);
        struct v2 p = menkowski_point(shape0, shape1, xf0, xf1, dir);
        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 collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
{
    /* FIXME: Account for radius */
    struct v2_array res = { .points = arena_dry_push(arena, struct v2) };
    struct v2 *points0 = shape0->points;
    struct v2 *points1 = shape1->points;
    u32 count0 = shape0->count;
    u32 count1 = shape1->count;
    for (u64 i = 0; i < count0; ++i) {
        struct v2 p0 = xform_mul_v2(xf0, points0[i]);
        for (u64 j = 0; j < count1; ++j) {
            struct v2 p1 = xform_mul_v2(xf1, points1[j]);
            *arena_push(arena, struct v2) = v2_sub(p0, p1);
            ++res.count;
        }
    }
    return res;
}














/* ========================== *
 * Boolean GJK (unused)
 * ========================== */

#if 0
b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_shape *shape1)
{
    struct { struct v2 a, b, c; } s = ZI;

    /* FIXME: Infinite loop when shapes exactly overlap same space? */
    struct v2 dir, p;

    /* First point is support point in shape's general directions to eachother */
    dir = v2_sub(starting_point(shape1), starting_point(shape0));
    if (v2_is_zero(dir)) dir = V2(1, 0);
    s.a = menkowski_point(shape0, shape1, dir);

    /* Second point is support point towards origin */
    dir = v2_neg(s.a);
    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;

            struct v2 vab = v2_sub(s.b, s.a);
            struct v2 vac = v2_sub(s.c, s.a);
            struct v2 a_to_origin = v2_neg(s.a);

            dir = v2_perp_towards_dir(vab, v2_neg(vac));  /* Normal of ab pointing away from c */
            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(vac, v2_neg(vab));  /* 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;
}
#endif







#if 0
struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
{
    struct temp_arena scratch = scratch_begin_no_conflict();  /* TODO: Only begin scratch for EPA */
    struct collider_collision_points_result res = ZI;

    struct v2 *points0 = shape0->points;
    struct v2 *points1 = shape1->points;
    u32 count0 = shape0->count;
    u32 count1 = shape1->count;
    f32 radius0 = shape0->radius;
    f32 radius1 = shape1->radius;
    (UNUSED)radius0;
    (UNUSED)radius1;

    /* TODO: Parameterize */
    const f32 tolerance = 0.005f;  /* How close can shapes be before collision is considered */
    const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f;
    const u32 max_epa_iterations = 64;  /* To prevent extremely large prototypes when origin is in exact center of rounded feature */

    b32 colliding = false;
    b32 simplex_is_closest_edge = false;

    struct collider_simplex s = ZI;
    struct v2 *proto = NULL;
    u32 proto_count = 0;

    struct v2 normal = ZI;
    struct collider_collision_point points[2] = ZI;
    u32 num_points = 0;

    struct v2 dir = ZI;
    struct v2 m = ZI;

#if COLLIDER_DEBUG
    u32 dbg_step = 0;
#endif

    /* ========================== *
     * GJK
     *
     * Determine encapsulating simplex if colliding, or closest edge / point to
     * origin on simplex (for check if shape distances are within tolerance)
     * ========================== */
    {
        /* First point is support point in shape's general directions to eachother */
        dir = v2_sub(xf1.og, xf0.og);
        if (v2_is_zero(dir)) dir = V2(1, 0);
        s.a = menkowski_point(shape0, shape1, xf0, xf1, dir);
        s.len = 1;

        struct v2 removed_a = ZI;
        struct v2 removed_b = ZI;
        u32 num_removed = 0;
        while (true) {
            if (s.len == 1) {
                /* Second point is support point towards origin */
                dir = v2_neg(s.a);

                DBGSTEP;
                m = menkowski_point(shape0, shape1, xf0, xf1, dir);
                /* Check that new point is far enough away from existing point */
                if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) {
                    simplex_is_closest_edge = true;
                    break;
                }
                s.b = s.a;
                s.a = m;
                s.len = 2;

                /* 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));
            }

            {
                DBGSTEP;
                m = menkowski_point(shape0, shape1, xf0, xf1, dir);
                /* Check that new point is far enough away from existing points */
                if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq ||
                    v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq ||
                    (
                        (num_removed >= 1) && (
                            (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) ||
                            (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq))
                        ) ||
                    math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq) {
                    simplex_is_closest_edge = true;
                    break;
                }
                s.c = s.b;
                s.b = s.a;
                s.a = m;
                s.len = 3;

                if (math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq ||
                    math_fabs(v2_wedge(v2_sub(s.c, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq ||
                    math_fabs(v2_wedge(v2_sub(s.c, s.b), v2_neg(s.b))) <= min_unique_pt_dist_sq) {
                    /* Simplex lies on origin */
                    colliding = true;
                    break;
                }
            }

            /* Determine region of the simplex in which the origin lies */
            DBGSTEP;
            struct v2 vab = v2_sub(s.b, s.a);
            struct v2 vac = v2_sub(s.c, s.a);
            struct v2 vbc = v2_sub(s.c, s.b);

            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);

            f32 rab_dot = v2_dot(rab_dir, v2_neg(s.a));
            f32 rac_dot = v2_dot(rac_dir, v2_neg(s.a));
            f32 rbc_dot = v2_dot(rbc_dir, v2_neg(s.b));

            f32 vab_dot = v2_dot(vab, v2_neg(s.a)) / v2_len_sq(vab);
            f32 vac_dot = v2_dot(vac, v2_neg(s.a)) / v2_len_sq(vac);
            f32 vbc_dot = v2_dot(vbc, v2_neg(s.b)) / v2_len_sq(vbc);

            if (rab_dot >= 0 && vab_dot >= 0 && vab_dot <= 1) {
                /* Region ab, remove c */
                num_removed = 1;
                removed_a = s.c;
                s.len = 2;
                dir = rab_dir;  /* Next third point is in direction of region ab */
            } else if (rac_dot >= 0 && vac_dot >= 0 && vac_dot <= 1) {
                /* Region ac, remove b */
                num_removed = 1;
                removed_a = s.b;
                s.len = 2;
                s.b = s.c;
                dir = rac_dir;  /* Next third point is in direction of region ac */
            } else if (rbc_dot >= 0 && vbc_dot >= 0 && vbc_dot <= 1) {
                /* Region bc, remove a */
                num_removed = 1;
                removed_a = s.a;
                s.len = 2;
                s.a = s.b;
                s.b = s.c;
                dir = rbc_dir;  /* Next third point is in direction of region bc */
            } else if (vab_dot <= 0 && vac_dot <= 0) {
                /* Region a, remove bc */
                num_removed = 2;
                removed_a = s.b;
                removed_b = s.c;
                s.len = 1;
            } else if (vab_dot >= 1 && vbc_dot <= 0) {
                /* Region b, remove ac */
                num_removed = 2;
                removed_a = s.a;
                removed_b = s.c;
                s.len = 1;
                s.a = s.b;
            } else if (vac_dot >= 1 && vbc_dot >= 1) {
                /* Region c, remove ab */
                num_removed = 2;
                removed_a = s.a;
                removed_b = s.b;
                s.len = 1;
                s.a = s.c;
            } else {
                /* No region, must be in simplex */
                colliding = true;
                break;
            }
        }
    }

    if (colliding) {
        /* ========================== *
         * Epa (to find collision normal from inside shape)
         * ========================== */

        const f32 epa_epsilon_sq = 0.001f * 0.001f;

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

        u32 epa_iterations = 0;
        while (colliding) {
            ++epa_iterations;
            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;
            struct v2 pen = ZI;
            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];
                struct v2 pe = proto[pe_index];

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

                struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_len_sq(vse)));
                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;
                    pen = pd;
                }
            }

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

            /* Find new point in dir */
            DBGSTEP;
            {
                /* TODO: If winding order is guaranteed then this can become v2_perp_left/right? */
                struct v2 a = proto[pen_ps_index];
                struct v2 b = proto[pen_pe_index];
                struct v2 vab = v2_sub(b, a);
                if (pen_len_sq < epa_epsilon_sq) {
                    /* Next point is in direction of line normal pointing outwards from simplex */
                    struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0];  /* Next point along prototype after edge */
                    dir = v2_perp_towards_dir(vab, v2_sub(a, n));
                } else {
                    dir = v2_perp_towards_dir(vab, a);
                }

            }
            m = menkowski_point(shape0, shape1, xf0, xf1, dir);

            /* Check unique */
            /* TODO: Better */
            {
                b32 unique = true;
                for (u32 i = 0; i < proto_count; ++i) {
                    struct v2 edge_start = proto[i];
                    struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0];
                    struct v2 vsm = v2_sub(m, edge_start);
                    if (v2_len_sq(vsm) < min_unique_pt_dist_sq ||
                        math_fabs(v2_wedge(v2_sub(edge_end, edge_start), vsm)) < min_unique_pt_dist_sq) {
                        unique = false;
                        break;
                    }
                }
                if (!unique || epa_iterations >= max_epa_iterations) {
                    res.path = 1;
                    if (pen_len_sq < epa_epsilon_sq) {
                        normal = v2_norm(dir);
                    } else {
                        normal = v2_norm(pen);
                    }
                    break;
                }
            }

            /* Insert point into prototype */
            /* FIXME: Preserve winding order */
            arena_push(scratch.arena, struct collider_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 if (simplex_is_closest_edge) {
        if (s.len == 1) {
            struct v2 p = v2_neg(s.a);
            if (v2_len_sq(p) <= (tolerance * tolerance)) {
                res.path = 2;
                normal = v2_norm(dir);
                colliding = true;
            }
        } else {
            /* Shapes are not overlapping (origin is outside of simplex). Project
             * origin to determine if distance is within tolerance. */
            ASSERT(s.len == 2);
            struct v2 vab = v2_sub(s.b, s.a);
            struct v2 vao = v2_neg(s.a);
            f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
            struct v2 p = v2_add(s.a, v2_mul(vab, ratio));
            if (v2_len_sq(p) <= (tolerance * tolerance)) {
                res.path = 2;
                normal = v2_norm(dir);
                colliding = true;
            }
        }
    }

    if (colliding) {
        /* ========================== *
         * Clip to determine final points
         * ========================== */

         /* Max vertices must be < 16 to fit in 4 bit ids */
        CT_ASSERT(ARRAY_COUNT(shape0->points) <= 16);

        DBGSTEP;
        {
            //const f32 wedge_epsilon = 0.001f;
            const f32 wedge_epsilon = 0.1f;

            /* shape0 a -> b winding = clockwise */
            u32 id_a0;
            u32 id_b0;
            struct v2 a0;
            struct v2 b0;

            /* shape1 a -> b winding = counterclockwise */
            u32 id_a1;
            u32 id_b1;
            struct v2 a1;
            struct v2 b1;
            {
                u32 p_i = collider_support_point_index(shape0, xf0, normal);
                u32 a_i = (p_i > 0) ? (p_i - 1) : (count0 - 1);
                u32 b_i = ((p_i + 1) < count0) ? (p_i + 1) : 0;

                struct v2 p = xform_mul_v2(xf0, points0[p_i]);
                struct v2 a = xform_mul_v2(xf0, points0[a_i]);
                struct v2 b = xform_mul_v2(xf0, points0[b_i]);

                struct v2 vap = v2_sub(p, a);
                struct v2 vpb = v2_sub(b, p);

                /* Swap a & b depending on winding order */
                if (v2_wedge(vap, vpb) < 0) {
                    u32 tmp_u32 = a_i;
                    a_i = b_i;
                    b_i = tmp_u32;
                    struct v2 tmp_v2 = a;
                    a = b;
                    b = tmp_v2;
                    tmp_v2 = vap;
                    vap = v2_neg(vpb);
                    vpb = v2_neg(tmp_v2);
                }

                f32 vap_wedge = v2_wedge(vap, normal);
                f32 vpb_wedge = v2_wedge(vpb, normal);
                if (vap_wedge < (vpb_wedge + wedge_epsilon)) {
                    id_a0 = a_i;
                    id_b0 = p_i;
                    a0 = a;
                    b0 = p;
                } else {
                    id_a0 = p_i;
                    id_b0 = b_i;
                    a0 = p;
                    b0 = b;
                }
            }
            {
                struct v2 neg_normal = v2_neg(normal);

                u32 p_i = collider_support_point_index(shape1, xf1, neg_normal);
                u32 a_i = ((p_i + 1) < count1) ? (p_i + 1) : 0;
                u32 b_i = (p_i > 0) ? (p_i - 1) : (count1 - 1);

                struct v2 p = xform_mul_v2(xf1, points1[p_i]);
                struct v2 a = xform_mul_v2(xf1, points1[a_i]);
                struct v2 b = xform_mul_v2(xf1, points1[b_i]);

                struct v2 vap = v2_sub(p, a);
                struct v2 vpb = v2_sub(b, p);

                /* Swap a & b depending on winding order */
                if (v2_wedge(vap, vpb) > 0) {
                    u32 tmp_u32 = a_i;
                    a_i = b_i;
                    b_i = tmp_u32;
                    struct v2 tmp_v2 = a;
                    a = b;
                    b = tmp_v2;
                    tmp_v2 = vap;
                    vap = v2_neg(vpb);
                    vpb = v2_neg(tmp_v2);
                }

                f32 vap_wedge = v2_wedge(vap, normal);
                f32 vpb_wedge = v2_wedge(vpb, normal);
                if (vap_wedge < (vpb_wedge + wedge_epsilon)) {
                    id_a1 = a_i;
                    id_b1 = p_i;
                    a1 = a;
                    b1 = p;
                } else {
                    id_a1 = p_i;
                    id_b1 = b_i;
                    a1 = p;
                    b1 = b;
                }
            }
#if 0
#if 1
            if (radius0 > 0.0) {
                struct v2 scale = xform_get_scale(xf0);
                struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius0), scale);
                a0 = v2_add(a0, normal_radius);
                b0 = v2_add(b0, normal_radius);
            }

            if (radius1 > 0.0) {
                struct v2 scale = xform_get_scale(xf1);
                struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius1), scale);
                a1 = v2_sub(a1, normal_radius);
                b1 = v2_sub(b1, normal_radius);
            }
#else
            if (radius0 > 0.0) {
                struct v2 scale = xform_get_scale(xf0);
                struct v2 perp_radius = v2_mul_v2(v2_with_len(v2_neg(v2_perp(v2_sub(b0, a0))), radius0), scale);
                a0 = v2_add(a0, perp_radius);
                b0 = v2_add(b0, perp_radius);
            }

            if (radius1 > 0.0) {
                struct v2 scale = xform_get_scale(xf1);
                struct v2 perp_radius = v2_mul_v2(v2_with_len(v2_neg(v2_perp(v2_sub(b1, a1))), radius1), scale);
                a1 = v2_sub(a1, perp_radius);
                b1 = v2_sub(b1, perp_radius);
            }
#endif
#endif

            f32 a0t = 0;
            f32 a1t = 0;
            f32 b0t = 0;
            f32 b1t = 0;

            struct v2 vab0 = v2_sub(b0, a0);
            struct v2 vab1 = v2_sub(b1, a1);

            {
                struct v2 va0a1 = v2_sub(a1, a0);
                struct v2 vb0b1 = v2_sub(b1, b0);

                f32 vab0_wedge_normal = v2_wedge(vab0, normal);
                f32 vab1_wedge_normal = v2_wedge(vab1, normal);
                f32 va0a1_wedge_normal = v2_wedge(va0a1, normal);
                f32 vb0b1_wedge_normal = v2_wedge(vb0b1, normal);

                if (math_fabs(vab0_wedge_normal) > 0.01f) {
                    f32 w = 1 / vab0_wedge_normal;
                    a0t = clamp_f32(va0a1_wedge_normal * w, 0, 1);
                    b0t = clamp_f32(vb0b1_wedge_normal * -w, 0, 1);
                }

                if (math_fabs(vab1_wedge_normal) > 0.01f) {
                    f32 w = 1 / vab1_wedge_normal;
                    a1t = clamp_f32(-va0a1_wedge_normal * w, 0, 1);
                    b1t = clamp_f32(-vb0b1_wedge_normal * -w, 0, 1);
                }
            }

            struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t));
            struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t));
            struct v2 b0_clipped = v2_add(b0, v2_mul(vab0, -b0t));
            struct v2 b1_clipped = v2_add(b1, v2_mul(vab1, -b1t));

            struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped);
            struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped);

            f32 a_sep = v2_dot(va0a1_clipped, normal);
            f32 b_sep = v2_dot(vb0b1_clipped, normal);

            struct v2 contact_a = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f));
            struct v2 contact_b = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f));

            //b32 merge_contacts = v2_len_sq(v2_sub(contact_b, contact_a)) < 0.01f;
            b32 merge_contacts = false;

            b32 force = false;

#if 0
            if (a_sep > tolerance && b_sep > tolerance) {
                res.path = 999999999;
                DEBUGBREAKABLE;
            }
#endif

            if (force || a_sep < tolerance) {
                struct collider_collision_point *point = &points[num_points++];
                point->id = id_a0 | (id_a1 << 4);
                point->separation = a_sep;
                point->point = contact_a;
            }

            if (force || (b_sep < tolerance && !merge_contacts)) {
                struct collider_collision_point *point = &points[num_points++];
                point->id = id_b0 | (id_b1 << 4);
                point->separation = b_sep;
                point->point = contact_b;
            }

            res.a0 = a0_clipped;
            res.a1 = a1_clipped;
            res.b0 = b0_clipped;
            res.b1 = b1_clipped;
        }
    }

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