#ifndef PHYS_H
#define PHYS_H

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

struct entity_store;
struct entity_lookup;

/* Structure containing data used for a single physics step */
struct phys_ctx {
    u64 tick_id;
    struct entity_store *store;
    struct entity_lookup *contact_lookup;
    struct entity_lookup *debug_lookup;

    struct v2 dbg_cursor_pos;
    b32 dbg_start_dragging;
    b32 dbg_stop_dragging;
};

struct phys_hit_event {
    struct entity_handle e0;
    struct entity_handle e1;
    struct v2 point;
    struct v2 normal;
    struct v2 vrel;     /* Relative velocity */
    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 collision occurred) */
};

struct phys_hit_event_node {
    struct phys_hit_event *e;
    struct phys_hit_event_node *next;
};

/* ========================== *
 * 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 */
    struct v2 dbg_pt;
};

struct phys_contact_constraint {
    u64 last_updated_tick;  /* To avoid checking collisions for the same constraint twice in one tick */
    b32 skip_solve;
    struct entity_handle e0;
    struct entity_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 entity_handle e0;
    struct entity_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;
};

void phys_create_contacts(struct phys_ctx *ctx, f32 elapsed_dt);
void phys_prepare_contacts(struct phys_ctx *ctx);
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 entity_handle e0;
    struct entity_handle e1;
    f32 correction_rate;
    f32 max_force;
    f32 max_torque;
};

struct phys_motor_joint {
    struct entity_handle e0;
    struct entity_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 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 {
    struct entity_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;
};

void phys_create_mouse_joints(struct phys_ctx *ctx);
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);

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

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

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

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

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

void phys_step(struct phys_ctx *ctx, f32 timestep);

#endif