typedef struct CollisionData CollisionData;
struct CollisionData {
    EntId e0;
    EntId e1;
    V2 point;
    V2 normal;   /* Normal of the collision from e0 to e1  */
    V2 vrel;     /* Relative velocity at point of collision */
    b32 is_start;       /* Did this collision just begin */
    f32 dt;             /* How much time elapsed in the step when this event occurred (this will equal the physics timestep unless an early time of impact occurred) */
};

/* Callback can return 1 to prevent the physics system from resolving */
typedef struct SimStepCtx SimStepCtx;
#define PHYS_COLLISION_CALLBACK_FUNC_DEF(name, arg_collision_data, arg_sim_step_ctx) b32 name(CollisionData *arg_collision_data, SimStepCtx *arg_sim_step_ctx)
typedef PHYS_COLLISION_CALLBACK_FUNC_DEF(phys_collision_callback_func, collision_data, ctx);

/* Structure containing data used for a single physics step */
typedef struct PhysStepCtx PhysStepCtx;
struct PhysStepCtx {
    SimStepCtx *sim_step_ctx;
    phys_collision_callback_func *collision_callback;
};

/* ========================== *
 * Contact
 * ========================== */

typedef struct ContactPoint ContactPoint;
struct ContactPoint {
    /* Contact point relative to the center of each entity.
     *
     * NOTE: We use fixed (non-rotated) points relative to the entities
     * rather than points fully in local space because contact manifolds
     * shouldn't really be affected by rotation accross substeps
     * (imagine re-building the manifold of a rotated shape, it would still be
     *  on the same side of the shape that it originally occured on) */
    V2 vcp0;
    V2 vcp1;

    u32 id;                     /* ID generated during clipping */
    f32 starting_separation;    /* How far are original points along normal */
    f32 normal_impulse;         /* Accumulated impulse along normal */
    f32 tangent_impulse;        /* Accumulated impulse along tangent */

    f32 inv_normal_mass;
    f32 inv_tangent_mass;

    /* Debugging */
#if DeveloperIsEnabled
    V2 dbg_pt;
#endif
};

typedef struct ContactConstraint ContactConstraint;
struct ContactConstraint {
    u64 last_phys_iteration;  /* To avoid checking collisions for the same constraint twice in one tick */
    b32 skip_solve;
    b32 wrong_dir;
    EntId e0;
    EntId e1;
    f32 inv_m0;
    f32 inv_m1;
    f32 inv_i0;
    f32 inv_i1;

    V2 normal;   /* Normal vector of collision from e0 -> e1 */
    u64 last_iteration;
    ContactPoint points[2];
    u32 num_points;

    f32 friction;

    f32 pushout_velocity;
};

typedef struct CollisionDebugData CollisionDebugData;
struct CollisionDebugData {
    EntId e0;
    EntId e1;
    CLD_CollisionResult res;

    ContactPoint points[2];
    u32 num_points;

    V2 closest0;
    V2 closest1;

    Xform xf0;
    Xform xf1;
};

void phys_create_and_update_contacts(PhysStepCtx *ctx, f32 elapsed_dt, u64 phys_iteration);
void phys_prepare_contacts(PhysStepCtx *ctx, u64 phys_iteration);
void phys_warm_start_contacts(PhysStepCtx *ctx);
void phys_solve_contacts(PhysStepCtx *ctx, f32 dt, b32 apply_bias);

/* ========================== *
 * Motor joint
 * ========================== */

typedef struct MotorJointDesc MotorJointDesc;
struct MotorJointDesc {
    EntId e0;
    EntId e1;
    f32 correction_rate;
    f32 max_force;
    f32 max_torque;
};

typedef struct MotorJoint MotorJoint;
struct MotorJoint {
    EntId e0;
    EntId e1;
    f32 correction_rate;
    f32 max_force;
    f32 max_torque;

    f32 inv_m0;
    f32 inv_m1;
    f32 inv_i0;
    f32 inv_i1;

    V2 linear_impulse;
    f32 angular_impulse;

    V2 point_local_e0;
    V2 point_local_e1;

    Xform linear_mass_xf;
    f32 angular_mass;
};

MotorJointDesc phys_motor_joint_def_init(void);
MotorJoint phys_motor_joint_from_def(MotorJointDesc def);
void phys_prepare_motor_joints(PhysStepCtx *ctx);
void phys_warm_start_motor_joints(PhysStepCtx *ctx);
void phys_solve_motor_joints(PhysStepCtx *ctx, f32 dt);

/* ========================== *
 * Mouse joint
 * ========================== */

typedef struct MouseJointDesc MouseJointDesc;
struct MouseJointDesc {
    EntId target;
    V2 point_local_start;
    V2 point_end;
    f32 linear_spring_hz;
    f32 linear_spring_damp;
    f32 angular_spring_hz;
    f32 angular_spring_damp;
    f32 max_force;
};

typedef struct MouseJoint MouseJoint;
struct MouseJoint {
    EntId target;
    V2 point_local_start;
    V2 point_end;
    f32 linear_spring_hz;
    f32 linear_spring_damp;
    f32 angular_spring_hz;
    f32 angular_spring_damp;
    f32 max_force;

    f32 inv_m;
    f32 inv_i;

    V2 linear_impulse;
    f32 angular_impulse;

    Xform linear_mass_xf;
};

MouseJointDesc phys_mouse_joint_def_init(void);
MouseJoint phys_mouse_joint_from_def(MouseJointDesc def);
void phys_prepare_mouse_joints(PhysStepCtx *ctx);
void phys_warm_start_mouse_joints(PhysStepCtx *ctx);
void phys_solve_mouse_joints(PhysStepCtx *ctx, f32 dt);

/* ========================== *
 * Weld joint
 * ========================== */

typedef struct WeldJointDesc WeldJointDesc;
struct WeldJointDesc {
    EntId e0;
    EntId e1;

    /* The xform that transforms a point in e0's space into the desired e1 space
     * (IE `xf` * V2FromXY(0, 0) should evaluate to the local point that e1's origin will lie) */
    Xform xf;

    f32 linear_spring_hz;
    f32 linear_spring_damp;
    f32 angular_spring_hz;
    f32 angular_spring_damp;
};

typedef struct WeldJoint WeldJoint;
struct WeldJoint {
    EntId e0;
    EntId e1;
    Xform xf0_to_xf1;

    f32 linear_spring_hz;
    f32 linear_spring_damp;
    f32 angular_spring_hz;
    f32 angular_spring_damp;

    f32 inv_m0;
    f32 inv_m1;
    f32 inv_i0;
    f32 inv_i1;

    V2 linear_impulse0;
    V2 linear_impulse1;
    f32 angular_impulse0;
    f32 angular_impulse1;
};

WeldJointDesc phys_weld_joint_def_init(void);
WeldJoint phys_weld_joint_from_def(WeldJointDesc def);
void phys_prepare_weld_joints(PhysStepCtx *ctx);
void phys_warm_start_weld_joints(PhysStepCtx *ctx);
void phys_solve_weld_joints(PhysStepCtx *ctx, f32 dt);

/* ========================== *
 * Integration
 * ========================== */

void phys_integrate_forces(PhysStepCtx *ctx, f32 dt);
void phys_integrate_velocities(PhysStepCtx *ctx, f32 dt);

/* ========================== *
 * Earliest time of impact
 * ========================== */

f32 phys_determine_earliest_toi(PhysStepCtx *ctx, f32 step_dt, f32 tolerance, u32 max_iterations);

/* ========================== *
 * Space
 * ========================== */

void phys_update_aabbs(PhysStepCtx *ctx);

/* ========================== *
 * Step
 * ========================== */

void phys_step(PhysStepCtx *ctx, f32 timestep);