#ifndef PHYS_H
#define PHYS_H

#include "collider.h"
#include "math.h"

struct space;
struct sim_ent_lookup;

struct phys_contact_constraint;
struct phys_collision_data {
    struct phys_contact_constraint *constraint;
    struct sim_ent_handle e0;
    struct sim_ent_handle e1;
    struct v2 point;
    struct v2 normal;   /* Normal of the collision from e0 to e1  */
    struct v2 vrel;     /* Relative velocity at point of collision */
    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) */
};

struct phys_collision_data_array {
    struct phys_collision_data *a;
    u64 count;
};

struct phys_collision_data;
#define PHYS_COLLISION_CALLBACK_FUNC_DEF(name, arg_collision_data, arg_udata) void name(struct phys_collision_data_array arg_collision_data, void *arg_udata)
typedef PHYS_COLLISION_CALLBACK_FUNC_DEF(phys_collision_callback_func, collision_data, udata);

/* Structure containing data used for a single physics step */
struct phys_ctx {
    struct sim_snapshot *ss;
    struct space *space;
    struct sim_ent_lookup *contact_lookup;

    phys_collision_callback_func *pre_solve_callback;
    phys_collision_callback_func *post_solve_callback;
    void *pre_solve_callback_udata;
    void *post_solve_callback_udata;

    struct sim_ent_lookup *debug_lookup;

};

/* ========================== *
 * Startup
 * ========================== */

struct phys_startup_receipt { i32 _; };
struct phys_startup_receipt phys_startup(void);

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

struct phys_contact_point {
    /* Contact point in local space of each entity */
    struct v2 point_local_e0;
    struct v2 point_local_e1;

    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 DEVELOPER
    struct v2 dbg_pt;
#endif
};

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

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

    f32 friction;

    struct math_spring_result softness;
    f32 pushout_velocity;
};

struct phys_collision_debug {
    struct sim_ent_handle e0;
    struct sim_ent_handle e1;
    struct collider_collision_points_result res;

    struct phys_contact_point points[2];
    u32 num_points;

    struct v2 closest0;
    struct v2 closest1;

    struct xform xf0;
    struct xform xf1;
};

struct phys_collision_data_array phys_create_and_update_contacts(struct arena *arena, struct phys_ctx *ctx, f32 elapsed_dt, u64 phys_iteration);
void phys_prepare_contacts(struct phys_ctx *ctx, u64 phys_iteration);
void phys_warm_start_contacts(struct phys_ctx *ctx);
void phys_solve_contacts(struct phys_ctx *ctx, f32 dt, b32 apply_bias);

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

struct phys_motor_joint_def {
    struct sim_ent_handle e0;
    struct sim_ent_handle e1;
    f32 correction_rate;
    f32 max_force;
    f32 max_torque;
};

struct phys_motor_joint {
    struct sim_ent_handle e0;
    struct sim_ent_handle e1;
    f32 correction_rate;
    f32 max_force;
    f32 max_torque;

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

    struct v2 linear_impulse;
    f32 angular_impulse;

    struct v2 point_local_e0;
    struct v2 point_local_e1;

    struct xform linear_mass_xf;
    f32 angular_mass;
};

struct phys_motor_joint phys_motor_joint_from_def(struct phys_motor_joint_def def);
void phys_prepare_motor_joints(struct phys_ctx *ctx);
void phys_warm_start_motor_joints(struct phys_ctx *ctx);
void phys_solve_motor_joints(struct phys_ctx *ctx, f32 dt);

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

struct phys_mouse_joint_def {
    struct sim_ent_handle target;
    struct v2 point_local_start;
    struct v2 point_local_end;
    f32 max_force;
};

struct phys_mouse_joint {
    struct sim_ent_handle target;
    struct v2 point_local_start;
    struct v2 point_local_end;
    struct math_spring_result linear_softness;
    struct math_spring_result angular_softness;
    f32 max_force;

    f32 inv_m;
    f32 inv_i;

    struct v2 linear_impulse;
    f32 angular_impulse;

    struct xform linear_mass_xf;
};

struct phys_mouse_joint phys_mouse_joint_from_def(struct phys_mouse_joint_def def);
void phys_prepare_mouse_joints(struct phys_ctx *ctx);
void phys_warm_start_mouse_joints(struct phys_ctx *ctx);
void phys_solve_mouse_joints(struct phys_ctx *ctx, f32 dt);

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

void phys_integrate_forces(struct phys_ctx *ctx, f32 dt);
void phys_integrate_velocities(struct phys_ctx *ctx, f32 dt);

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

f32 phys_determine_earliest_toi_for_bullets(struct phys_ctx *ctx, f32 step_dt, f32 tolerance, u32 max_iterations);

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

void phys_update_aabbs(struct phys_ctx *ctx);

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

u64 phys_step(struct phys_ctx *ctx, f32 timestep, u64 phys_iteration);

#endif