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

/* How close can non-overlapping shapes be before collision is considered */
#define COLLISION_TOLERANCE     0.005f

/* NOTE: Should always be less than tolerance, since colliding = true if origin is within this distance. */
#define MIN_UNIQUE_PT_DIST_SQ   (0.0001f * 0.0001f)

/* To prevent extremely large prototypes when origin is in exact center of rounded feature */
#define MAX_EPA_ITERATIONS      64

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

INTERNAL void _dbgbreakable(void)
{
#if RTC
    DEBUGBREAKABLE;
#endif
}

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

INTERNAL struct collider_support_point collider_get_support_point_internal(struct collider_shape *shape, struct xform xf, struct v2 dir, i32 ignore)
{
    struct v2 *points = shape->points;
    u32 count = shape->count;
    f32 radius = shape->radius;

    dir = v2_rotated(dir, -xform_get_rotation(xf));
    dir = v2_mul_v2(dir, xform_get_scale(xf));

    if (count == 1) {
        /* Skip 'ignore' on single point colliders */
        ignore = -1;
    }

    struct v2 furthest = ZI;
    u32 furthest_index = 0;
    f32 furthest_dot = -F32_INFINITY;
    for (u32 i = 0; i < count; ++i) {
        if ((i32)i == ignore) {
            continue;
        }
        struct v2 p = points[i];
        f32 dot = v2_dot(dir, p);
        if (dot > furthest_dot) {
            furthest = p;
            furthest_dot = dot;
            furthest_index = i;
        }
    }

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

    furthest = xform_mul_v2(xf, furthest);

    struct collider_support_point res;
    res.p = furthest;
    res.i = furthest_index;
    return res;
}

struct collider_support_point collider_get_support_point(struct collider_shape *shape, struct xform xf, struct v2 dir)
{
    return collider_get_support_point_internal(shape, xf, dir, -1);
}

INTERNAL struct collider_menkowski_point get_menkowski_point(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct v2 dir)
{
    struct collider_menkowski_point res;
    res.s0 = collider_get_support_point(shape0, xf0, dir);
    res.s1 = collider_get_support_point(shape1, xf1, v2_neg(dir));
    res.p = v2_sub(res.s0.p, res.s1.p);
    return res;
}

/* ========================== *
 * AABB
 * ========================== */

struct aabb collider_aabb_from_collider(struct collider_shape *shape, struct xform xf)
{
    struct aabb res;
    res.p0.x = collider_get_support_point(shape, xf, V2(-1, 0)).p.x - COLLISION_TOLERANCE;
    res.p0.y = collider_get_support_point(shape, xf, V2(0, -1)).p.y - COLLISION_TOLERANCE;
    res.p1.x = collider_get_support_point(shape, xf, V2(1, 0)).p.x + COLLISION_TOLERANCE;
    res.p1.y = collider_get_support_point(shape, xf, V2(0, 1)).p.y + COLLISION_TOLERANCE;
    return res;
}

struct aabb collider_aabb_from_combined_aabb(struct aabb b0, struct aabb b1)
{
    struct aabb res;
    res.p0.x = min_f32(min_f32(b0.p0.x, b0.p1.x), min_f32(b1.p0.x, b1.p1.x));
    res.p0.y = min_f32(min_f32(b0.p0.y, b0.p1.y), min_f32(b1.p0.y, b1.p1.y));
    res.p1.x = max_f32(max_f32(b0.p0.x, b0.p1.x), max_f32(b1.p0.x, b1.p1.x));
    res.p1.y = max_f32(max_f32(b0.p0.y, b0.p1.y), max_f32(b1.p0.y, b1.p1.y));
    return res;
}

b32 collider_test_aabb(struct aabb box0, struct aabb box1)
{
    f32 b0_x0 = box0.p0.x;
    f32 b0_x1 = box0.p1.x;
    f32 b1_x0 = box1.p0.x;
    f32 b1_x1 = box1.p1.x;
    f32 b0_y0 = box0.p0.y;
    f32 b0_y1 = box0.p1.y;
    f32 b1_y0 = box1.p0.y;
    f32 b1_y1 = box1.p1.y;
    return ((b0_x0 >= b1_x0 && b0_x0 <= b1_x1) || (b0_x1 >= b1_x0 && b0_x1 <= b1_x1) || (b1_x0 >= b0_x0 && b1_x0 <= b0_x1) || (b1_x1 >= b0_x0 && b1_x1 <= b0_x1)) &&
           ((b0_y0 >= b1_y0 && b0_y0 <= b1_y1) || (b0_y1 >= b1_y0 && b0_y1 <= b1_y1) || (b1_y0 >= b0_y0 && b1_y0 <= b0_y1) || (b1_y1 >= b0_y0 && b1_y1 <= b0_y1));
}

/* ========================== *
 * GJK
 *
 * Determine simplex in menkowksi difference that encapsulates origin if shapes
 * overlap, or closest edge / point to origin on menkowski difference if they
 * do not.
 * ========================== */

struct gjk_result {
    struct collider_menkowski_simplex simplex;
    struct v2 final_dir;

    /* If true, simplex represents triangle inside of menkowski difference
     * encapsulating the origin. If false, simplex represents the closest
     * feature on menkowski difference to the origin. */
    b32 overlapping;

#if COLLIDER_DEBUG
    u32 dbg_step;
#endif
};

#if COLLIDER_DEBUG
INTERNAL struct gjk_result gjk_get_simplex(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, f32 min_unique_pt_dist_sq, u32 dbg_step)
#else
INTERNAL struct gjk_result gjk_get_simplex(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, f32 min_unique_pt_dist_sq)
#endif
{
    b32 overlapping = false;
    struct collider_menkowski_simplex s = ZI;
    struct v2 dir = ZI;
    struct collider_menkowski_point m = ZI;

    /* 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 = get_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.p);

            DBGSTEP;
            m = get_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.p, s.a.p)) < min_unique_pt_dist_sq) {
                overlapping = false;
                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.p, s.a.p), v2_neg(s.a.p));
        }

        {
            DBGSTEP;
            m = get_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.p, s.a.p)) < min_unique_pt_dist_sq ||
                v2_len_sq(v2_sub(m.p, s.b.p)) < min_unique_pt_dist_sq ||
                (
                    (num_removed >= 1) && (
                        (v2_len_sq(v2_sub(m.p, removed_a)) < min_unique_pt_dist_sq) ||
                        (num_removed >= 2 && v2_len_sq(v2_sub(m.p, removed_b)) < min_unique_pt_dist_sq))
                    ) ||
                math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < min_unique_pt_dist_sq) {
                overlapping = false;
                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))) <= min_unique_pt_dist_sq) ||
                (math_fabs(v2_wedge(v2_sub(s.c.p, s.b.p), v2_neg(s.b.p))) <= min_unique_pt_dist_sq) ||
                (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) <= min_unique_pt_dist_sq)) {
                /* Simplex lies on origin */
                overlapping = true;
                break;
            }
        }

        /* Determine region of the simplex in which the origin lies */
        DBGSTEP;
        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);

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

        f32 vab_dot = v2_dot(vab, v2_neg(s.a.p)) / v2_len_sq(vab);
        f32 vac_dot = v2_dot(vac, v2_neg(s.a.p)) / v2_len_sq(vac);
        f32 vbc_dot = v2_dot(vbc, v2_neg(s.b.p)) / 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.p;
            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.p;
            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.p;
            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.p;
            removed_b = s.c.p;
            s.len = 1;
        } else if (vab_dot >= 1 && vbc_dot <= 0) {
            /* Region b, remove ac */
            num_removed = 2;
            removed_a = s.a.p;
            removed_b = s.c.p;
            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.p;
            removed_b = s.b.p;
            s.len = 1;
            s.a = s.c;
        } else {
            /* No region, must be in simplex */
            overlapping = true;
            break;
        }
    }

#if COLLIDER_DEBUG
    abort:
#endif

    struct gjk_result res = {
        .simplex = s,
        .overlapping = overlapping,
        .final_dir = dir,
#if COLLIDER_DEBUG
        .dbg_step = dbg_step
#endif
    };

    return res;
}

/* ========================== *
 * EPA
 *
 * Expands upon result of GJK calculation to determine collision normal & closest edge when shapes are overlapping
 * ========================== */

struct epa_result {
    struct v2 normal;
    struct collider_menkowski_feature closest_feature;  /* Represents closest feature (edge or point) to origin on menkowski difference */

#if COLLIDER_DEBUG
    struct collider_prototype prototype;
    u32 dbg_step;
#endif
};

#if COLLIDER_DEBUG
INTERNAL struct epa_result epa_get_normal_from_gjk(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct gjk_result gjk_res, f32 min_unique_pt_dist_sq, u32 max_iterations, u32 dbg_step)
#else
INTERNAL struct epa_result epa_get_normal_from_gjk(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, struct gjk_result gjk_res, f32 min_unique_pt_dist_sq, u32 max_iterations)
#endif
{
    struct temp_arena scratch = scratch_begin_no_conflict();

    struct collider_menkowski_feature closest_feature = ZI;
    struct v2 normal = ZI;

    struct collider_menkowski_point *proto = NULL;
    u32 proto_count = 0;
    if (gjk_res.overlapping) {
        struct collider_menkowski_simplex s = gjk_res.simplex;
        proto = arena_dry_push(scratch.arena, struct collider_menkowski_point);
        {
            ASSERT(s.len == 3);
            struct collider_menkowski_point *tmp = arena_push_array_no_zero(scratch.arena, struct collider_menkowski_point, 3);
            tmp[0] = s.a;
            tmp[1] = s.b;
            tmp[2] = s.c;
            proto_count = 3;
        }

        i32 winding = v2_winding(v2_sub(s.c.p, s.a.p), v2_sub(s.b.p, s.a.p));

        u32 epa_iterations = 0;
        while (true) {
            ++epa_iterations;

            /* Find dir from origin to closest edge */
            /* FIXME: Winding order of ps & pe index */
            f32 closest_len_sq = F32_INFINITY;
            struct collider_menkowski_point closest_a = ZI;
            struct collider_menkowski_point closest_b = ZI;
            u32 closest_b_index = 0;
            for (u32 i = 0; i < proto_count; ++i) {
                u32 a_index = i;
                u32 b_index = (i < proto_count - 1) ? (i + 1) : 0;
                struct collider_menkowski_point a = proto[a_index];
                struct collider_menkowski_point b = proto[b_index];

                struct v2 vab = v2_sub(b.p, a.p);
                struct v2 vao = v2_neg(a.p);

                f32 proj_ratio = clamp_f32(v2_dot(vao, vab) / v2_len_sq(vab), 0, 1);
                struct v2 proj = v2_add(a.p, v2_mul(vab, proj_ratio));

                f32 proj_len_sq = v2_len_sq(proj);
                if (proj_len_sq < closest_len_sq - min_unique_pt_dist_sq) {
                    closest_a = a;
                    closest_b = b;
                    closest_b_index = b_index;
                    closest_len_sq = proj_len_sq;
                }
            }
            struct v2 vab = v2_sub(closest_b.p, closest_a.p);

            /* Find new point in dir */
            struct v2 dir = v2_mul(v2_perp(vab), winding);
            struct collider_menkowski_point m = get_menkowski_point(shape0, shape1, xf0, xf1, dir);

#if COLLIDER_DEBUG
            {
                normal = v2_norm(dir);
                closest_feature.a = closest_a;
                closest_feature.b = closest_b;
                closest_feature.len = 2;
            }
#endif

            /* Check validity of new point */
            DBGSTEP;
            {
                b32 valid = true;

                {
                    /* NOTE: Changing this value affects how stable normals are for circular colliders */
                    //const f32 validity_epsilon = min_unique_pt_dist_sq;  /* Arbitrary */
                    const f32 validity_epsilon = 0.00000000001f;  /* Arbitrary */

                    struct v2 vam = v2_sub(m.p, closest_a.p);
                    struct v2 vbm = v2_sub(closest_b.p, closest_a.p);

                    f32 dot = v2_dot(vab, vam) / v2_len_sq(vab);

                    if (dot >= -validity_epsilon && dot <= 1 - validity_epsilon && (v2_wedge(vab, vam) * -winding) >= -validity_epsilon) {
                        /* New point is not between edge */
                        valid = false;
                    } else if (v2_len_sq(vam) < min_unique_pt_dist_sq || v2_len_sq(vbm) < min_unique_pt_dist_sq) {
                        /* New point is too close to existing */
                        valid = false;
                    }
                }

                if (!valid || epa_iterations >= max_iterations) {
                    normal = v2_norm(dir);
                    closest_feature.a = closest_a;
                    closest_feature.b = closest_b;
                    closest_feature.len = 2;
                    break;
                }
            }

            /* Insert point into prototype */
            arena_push_no_zero(scratch.arena, struct collider_menkowski_point);
            ++proto_count;
            for (u32 i = proto_count - 1; i > closest_b_index; --i) {
                u32 shift_from = (i > 0) ? i - 1 : proto_count - 1;
                u32 shift_to = i;
                proto[shift_to] = proto[shift_from];
            }
            proto[closest_b_index] = m;
        }
    } else {
        normal = v2_norm(gjk_res.final_dir);
        closest_feature.len = gjk_res.simplex.len;
        closest_feature.a = gjk_res.simplex.a;
        closest_feature.b = gjk_res.simplex.b;
    }

#if COLLIDER_DEBUG
    abort:
#endif

    struct epa_result res = {
        .normal = normal,
        .closest_feature = closest_feature
    };

#if COLLIDER_DEBUG
    res.dbg_step = dbg_step;
    u32 len = min_u32(proto_count, ARRAY_COUNT(res.prototype.points));
    for (u32 i = 0; i < len; ++i) {
        res.prototype.points[i] = proto[i].p;
    }
    res.prototype.len = len;
#endif

    scratch_end(scratch);
    return res;
}

/* ========================== *
 * Clipping
 * ========================== */

struct clip_line_to_line_result {
    struct v2 a0_clipped, b0_clipped;
    struct v2 a1_clipped, b1_clipped;
};

INTERNAL struct clip_line_to_line_result clip_line_to_line(struct v2 a0, struct v2 b0, struct v2 a1, struct v2 b1, struct v2 normal)
{
    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_w = v2_wedge(vab0, normal);
    f32 vab1_w = v2_wedge(vab1, normal);
    f32 va0a1_w = v2_wedge(va0a1, normal);
    f32 vb0b1_w = v2_wedge(vb0b1, normal);

    /* FIXME: Handle 0 denominator */
    f32 a0t;
    f32 b0t;
    {
        f32 w = 1 / vab0_w;
        a0t = clamp_f32(va0a1_w * w, 0, 1);
        b0t = clamp_f32(vb0b1_w * -w, 0, 1);
    }
    f32 a1t;
    f32 b1t;
    {
        f32 w = 1 / vab1_w;
        a1t = clamp_f32(-va0a1_w * w, 0, 1);
        b1t = clamp_f32(-vb0b1_w * -w, 0, 1);
    }

    struct clip_line_to_line_result res;
    res.a0_clipped = v2_add(a0, v2_mul(vab0, a0t));
    res.a1_clipped = v2_add(a1, v2_mul(vab1, a1t));
    res.b0_clipped = v2_add(b0, v2_mul(vab0, -b0t));
    res.b1_clipped = v2_add(b1, v2_mul(vab1, -b1t));
    return res;
}

INTERNAL struct v2 clip_point_to_line(struct v2 a, struct v2 b, struct v2 p, struct v2 normal)
{
    struct v2 vab = v2_sub(b, a);
    struct v2 vap = v2_sub(p, a);

    f32 vab_w = v2_wedge(vab, normal);
    f32 vap_w = v2_wedge(vap, normal);

    f32 t;
    {
        f32 w = 1 / vab_w;
        t = clamp_f32(vap_w * w, 0, 1);
    }

    struct v2 res = v2_add(a, v2_mul(vab, t));
    return res;
}

/* ========================== *
 * Collision points
 * ========================== */

struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
{
    struct collider_collision_points_result res = ZI;

    const f32 tolerance = COLLISION_TOLERANCE;
    const f32 min_unique_pt_dist_sq = MIN_UNIQUE_PT_DIST_SQ;
    const u32 max_epa_iterations = MAX_EPA_ITERATIONS;

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

#if COLLIDER_DEBUG
    u32 dbg_step = 0;
#endif

    struct gjk_result gjk_res = ZI;
    struct epa_result epa_res = ZI;

    /* Run GJK */
#if COLLIDER_DEBUG
    gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq, dbg_step);
    dbg_step = gjk_res.dbg_step;
#else
    gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq);
#endif
    DBGSTEP;

    /* Run EPA */
#if COLLIDER_DEBUG
    epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations, dbg_step);
    dbg_step = epa_res.dbg_step;
#else
    epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations);
#endif
    normal = epa_res.normal;
    DBGSTEP;

    /* Determine collision */
    if (gjk_res.overlapping) {
        colliding = true;
    } else {
        struct collider_menkowski_feature f = epa_res.closest_feature;
        /* Shapes not overlapping, determine if distance between shapes within tolerance */
        if (f.len == 1) {
            struct v2 p = v2_neg(f.a.p);
            if (v2_len_sq(p) <= (tolerance * tolerance)) {
                colliding = true;
            }
        } else {
            /* Project origin to determine if distance is within tolerance. */
            ASSERT(f.len == 2);
            struct v2 vab = v2_sub(f.b.p, f.a.p);
            struct v2 vao = v2_neg(f.a.p);
            f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
            struct v2 p = v2_add(f.a.p, v2_mul(vab, ratio));
            if (v2_len_sq(p) <= (tolerance * tolerance)) {
                colliding = true;
            }
        }
    }

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

        struct collider_menkowski_feature f = epa_res.closest_feature;

        {
            b32 collapse0 = false;
            b32 collapse1 = false;

            struct collider_support_point a0 = f.a.s0;
            struct collider_support_point a1 = f.a.s1;
            struct collider_support_point b0 = f.b.s0;
            struct collider_support_point b1 = f.b.s1;
            /* FIXME: Manually account for shapes w/ 1 & 2 points */
            if (f.len == 2) {
                if (a0.i == b0.i) {
                    if (shape0->count > 1) {
                        b0 = collider_get_support_point_internal(shape0, xf0, normal, b0.i);
                    } else {
                        collapse0 = true;
                        b0 = a0;
                    }
                }
                if (a1.i == b1.i) {
                    if (shape1->count > 1) {
                        b1 = collider_get_support_point_internal(shape1, xf1, v2_neg(normal), b1.i);
                    } else {
                        collapse1 = true;
                        b1 = a1;
                    }
                }
            } else {
                collapse0 = true;
                collapse1 = true;
                b0 = a0;
                b1 = a1;
            }

            struct v2 vab0 = v2_sub(b0.p, a0.p);
            struct v2 vab1 = v2_sub(b1.p, a1.p);
            struct v2 vab0_norm = v2_norm(vab0);
            struct v2 vab1_norm = v2_norm(vab1);

            /* Swap points based on normal direction for consistent clipping */
            if (v2_wedge(normal, vab0) < 0) {
                struct collider_support_point tmp = a0;
                a0 = b0;
                b0 = tmp;
                vab0 = v2_neg(vab0);
            }
            if (v2_wedge(normal, vab1) < 0) {
                struct collider_support_point tmp = a1;
                a1 = b1;
                b1 = tmp;
                vab1 = v2_neg(vab1);
            }

            /* Collapse lines that are too far in the direction of the normal to be accurately clipped */
            f32 collapse_epsilon = 0.05f;
            collapse0 = collapse0 || math_fabs(v2_wedge(normal, vab0_norm)) < collapse_epsilon;
            collapse1 = collapse1 || math_fabs(v2_wedge(normal, vab1_norm)) < collapse_epsilon;

            /* Collapse lines into deepest point */
            if (collapse0) {
                if (v2_dot(normal, vab0) > 0) {
                    a0 = b0;
                } else {
                    /* TODO: Remove this (debugging) */
                    b0 = a0;
                }
            }
            if (collapse1) {
                if (v2_dot(normal, vab1) < 0) {
                    a1 = b1;
                } else {
                    /* TODO: Remove this (debugging) */
                    b1 = a1;
                }
            }

            f32 a_sep = F32_INFINITY;
            f32 b_sep = F32_INFINITY;
            struct v2 a_midpoint = ZI;
            struct v2 b_midpoint = ZI;
            b32 ignore_a = true;
            b32 ignore_b = true;
            if (!collapse0 && !collapse1) {
                /* Clip line to line */
                struct clip_line_to_line_result clip_res = clip_line_to_line(a0.p, b0.p, a1.p, b1.p, normal);
                struct v2 a0_clipped = clip_res.a0_clipped;
                struct v2 a1_clipped = clip_res.a1_clipped;
                struct v2 b0_clipped = clip_res.b0_clipped;
                struct v2 b1_clipped = clip_res.b1_clipped;
                /* Calc midpoint between clipped a & b */
                struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped);
                struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped);
                a_sep = v2_dot(va0a1_clipped, normal);
                b_sep = v2_dot(vb0b1_clipped, normal);
                a_midpoint = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f));
                b_midpoint = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f));
                ignore_a = false;
                ignore_b = false;
                struct v2 vfin = v2_sub(b_midpoint, a_midpoint);
                if (v2_len_sq(vfin) < (0.005 * 0.005)) {
                    if (a_sep > b_sep) {
                        ignore_a = true;
                    } else {
                        ignore_b = true;
                    }
                }
                res.a0_clipped = a0_clipped;
                res.a1_clipped = a1_clipped;
                res.b0_clipped = b0_clipped;
                res.b1_clipped = b1_clipped;
            } else {
                struct v2 p0 = a0.p;
                struct v2 p1 = a1.p;
                /* TODO: Choose ID based on closest clipped point */
                if (collapse1 && !collapse0) {
                    /* Project a1 onto vab0 */
                    p0 = clip_point_to_line(a0.p, b0.p, a1.p, normal);
                }
                if (collapse0 && !collapse1) {
                    /* Project a0 onto vab1 */
                    p1 = clip_point_to_line(a1.p, b1.p, a0.p, normal);
                }
                /* Calc midpoint */
                struct v2 vsep = v2_sub(p1, p0);
                a_midpoint = v2_add(p0, v2_mul(vsep, 0.5f));
                a_sep = v2_dot(normal, p1) - v2_dot(normal, p0);
                ignore_a = false;
                res.a0_clipped = p0;
                res.a1_clipped = p1;
                res.b0_clipped = p0;
                res.b1_clipped = p1;
            }

            /* Insert points */
            if (!ignore_a && a_sep < tolerance) {
                struct collider_collision_point *point = &points[num_points++];
                point->id = a0.i | (a1.i << 4);
                point->separation = a_sep;
                point->point = a_midpoint;
            }
            if (!ignore_b && b_sep < tolerance) {
                struct collider_collision_point *point = &points[num_points++];
                point->id = b0.i | (b1.i << 4);
                point->separation = b_sep;
                point->point = b_midpoint;
            }

            res.a0 = a0.p;
            res.a1 = a1.p;
            res.b0 = b0.p;
            res.b1 = b1.p;
        }
    }

#if COLLIDER_DEBUG
    res.solved = true;
    abort:
    res.simplex = gjk_res.simplex;
    res.prototype.len = epa_res.prototype.len;
    MEMCPY(res.prototype.points, epa_res.prototype.points, sizeof(res.prototype.points[0]) * res.prototype.len);
#endif
    res.normal = normal;
    res.points[0] = points[0];
    res.points[1] = points[1];
    res.num_points = num_points;
    return res;
}

/* ========================== *
 * Closest points
 * ========================== */

 /* TODO: De-duplicate code between collider_closest_points & collider_collision_points */

struct collider_closest_points_result collider_closest_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1)
{
    struct collider_closest_points_result res = ZI;

    const f32 tolerance = COLLISION_TOLERANCE;
    const f32 min_unique_pt_dist_sq = MIN_UNIQUE_PT_DIST_SQ;
    const u32 max_epa_iterations = MAX_EPA_ITERATIONS;

    struct v2 p0 = ZI;
    struct v2 p1 = ZI;
    b32 colliding = false;

#if COLLIDER_DEBUG
    u32 dbg_step = 0;
#endif

    struct gjk_result gjk_res = ZI;
    struct epa_result epa_res = ZI;

    /* Run GJK */
#if COLLIDER_DEBUG
    gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq, dbg_step);
    dbg_step = gjk_res.dbg_step;
#else
    gjk_res = gjk_get_simplex(shape0, shape1, xf0, xf1, min_unique_pt_dist_sq);
#endif
    DBGSTEP;

    /* Run EPA */
#if COLLIDER_DEBUG
    epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations, dbg_step);
    dbg_step = epa_res.dbg_step;
#else
    epa_res = epa_get_normal_from_gjk(shape0, shape1, xf0, xf1, gjk_res, min_unique_pt_dist_sq, max_epa_iterations);
#endif
    DBGSTEP;

    /* ========================== *
     * Resolve points
     * ========================== */

    colliding = gjk_res.overlapping;
    struct collider_menkowski_feature f = epa_res.closest_feature;
    if (f.len == 1) {
        p0 = f.a.s0.p;
        p1 = f.a.s1.p;
        colliding = gjk_res.overlapping || v2_len_sq(v2_neg(f.a.p)) <= (tolerance * tolerance);
    } else {
        ASSERT(f.len == 2);
        /* FIXME: Winding order dependent? */
        f32 ratio;
        {
            /* Determine ratio between edge a & b that projected origin lies */
            struct v2 vab = v2_sub(f.b.p, f.a.p);
            struct v2 vao = v2_neg(f.a.p);
            ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1);
        }
        /* Shape 0 */
        p0 = v2_sub(f.b.s0.p, f.a.s0.p);
        p0 = v2_mul(p0, ratio);
        p0 = v2_add(p0, f.a.s0.p);
        /* Shape 1 */
        p1 = v2_sub(f.b.s1.p, f.a.s1.p);
        p1 = v2_mul(p1, ratio);
        p1 = v2_add(p1, f.a.s1.p);
        colliding = gjk_res.overlapping || v2_len_sq(v2_sub(p1, p0)) <= (tolerance * tolerance);
    }

#if COLLIDER_DEBUG
    res.solved = true;
    abort:
    res.simplex = gjk_res.simplex;
    res.prototype.len = epa_res.prototype.len;
    MEMCPY(res.prototype.points, epa_res.prototype.points, sizeof(res.prototype.points[0]) *res.prototype.len);
    res.simplex = gjk_res.simplex;
#endif
    res.p0 = p0;
    res.p1 = p1;
    res.colliding = colliding;
    return res;
}

/* ========================== *
 * Time of impact
 * ========================== */

 /* Takes 2 shapes and their xforms at t=0 and t=1.
  * Returns time of impact in range [0, 1]. */
f32 collider_time_of_impact(struct collider_shape *c0, struct collider_shape *c1,
                            struct xform xf0_t0, struct xform xf1_t0,
                            struct xform xf0_t1, struct xform xf1_t1,
                            f32 tolerance, u32 max_iterations)
{
    f32 t0 = 0;
    f32 t1 = 1;
    f32 t0_sep = 0;
    f32 t1_sep = 0;
    f32 t = 0;
    f32 t_sep = F32_INFINITY;

    /* Find direction p0 -> p1 at t=0 */
    struct v2 dir;
    struct v2 dir_neg;
    {
        struct collider_closest_points_result closest_points_res = collider_closest_points(c0, c1, xf0_t0, xf1_t0);
        if (closest_points_res.colliding) {
            /* Shapes are penetrating at t=0 */
            return 0;
        }
        dir = v2_sub(closest_points_res.p1, closest_points_res.p0);
        t0_sep = v2_len(dir);
        dir = v2_div(dir, t0_sep);  /* Normalize */
        dir_neg = v2_neg(dir);
    }

    {
        struct v2 p0 = collider_get_support_point(c0, xf0_t1, dir).p;
        struct v2 p1 = collider_get_support_point(c1, xf1_t1, dir_neg).p;
        t1_sep = v2_dot(dir, v2_sub(p1, p0));
        if (t1_sep > 0) {
            /* Shapes are not penetrating at t=1 */
            return 1;
        }
    }

    u32 iteration = 0;
    while (math_fabs(t_sep) > tolerance) {
        if (iteration >= max_iterations) {
            break;
        }

        /* Use mix of bisection & false position method to find root
         * (as described in https://box2d.org/files/ErinCatto_ContinuousCollision_GDC2013.pdf) */
        if (iteration & 1) {
            /* Bisect */
            t = (t1 + t0) / 2.0;
        } else {
            /* False position (fastest for linear case) */
            f32 m = (t1_sep - t0_sep) / (t1 - t0);
            t = (-t1_sep / m) + t1;
        }

        struct xform xf0 = xform_lerp(xf0_t0, xf0_t1, t);
        struct xform xf1 = xform_lerp(xf1_t0, xf1_t1, t);

        struct v2 p0 = collider_get_support_point(c0, xf0, dir).p;
        struct v2 p1 = collider_get_support_point(c1, xf1, dir_neg).p;
        t_sep = v2_dot(dir, v2_sub(p1, p0));

        /* Update bracket */
        if (t_sep > 0) {
            t0 = t;
            t0_sep = t_sep;
        } else {
            t1 = t;
            t1_sep = t_sep;
        }

        ++iteration;
    }

    return t;
}

/* ========================== *
 * 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 collider_menkowski_point m = get_menkowski_point(shape0, shape1, xf0, xf1, dir);
        if (res.count == 0 || !v2_eq(m.p, res.points[res.count - 1])) {
            *arena_push_no_zero(arena, struct v2) = m.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_no_zero(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 = get_menkowski_point(shape0, shape1, dir);

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