#ifndef PHYS_H
#define PHYS_H

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

struct space;
struct sim_ent_lookup;
struct sim_step_ctx;

struct phys_contact_constraint;
struct phys_collision_data {
    struct sim_ent_id e0;
    struct sim_ent_id e1;
    struct v2 point;
    struct v2 normal;   /* Normal of the collision from e0 to e1  */
    struct 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 true to prevent the physics system from resolving */
#define PHYS_COLLISION_CALLBACK_FUNC_DEF(name, arg_collision_data, arg_sim_step_ctx) b32 name(struct phys_collision_data *arg_collision_data, struct sim_step_ctx *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 */
struct phys_step_ctx {
    struct sim_step_ctx *sim_step_ctx;
    phys_collision_callback_func *collision_callback;
};

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

struct phys_contact_point {
    /* 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) */
    struct v2 vcp0;
    struct 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 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;
    b32 wrong_dir;
    struct sim_ent_id e0;
    struct sim_ent_id 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;

    f32 pushout_velocity;
};

struct phys_collision_debug {
    struct sim_ent_id e0;
    struct sim_ent_id 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_and_update_contacts(struct phys_step_ctx *ctx, f32 elapsed_dt, u64 phys_iteration);
void phys_prepare_contacts(struct phys_step_ctx *ctx, u64 phys_iteration);
void phys_warm_start_contacts(struct phys_step_ctx *ctx);
void phys_solve_contacts(struct phys_step_ctx *ctx, f32 dt, b32 apply_bias);

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

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

struct phys_motor_joint {
    struct sim_ent_id e0;
    struct sim_ent_id 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_def phys_motor_joint_def_init(void);
struct phys_motor_joint phys_motor_joint_from_def(struct phys_motor_joint_def def);
void phys_prepare_motor_joints(struct phys_step_ctx *ctx);
void phys_warm_start_motor_joints(struct phys_step_ctx *ctx);
void phys_solve_motor_joints(struct phys_step_ctx *ctx, f32 dt);

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

struct phys_mouse_joint_def {
    struct sim_ent_id target;
    struct v2 point_local_start;
    struct v2 point_end;
    f32 linear_spring_hz;
    f32 linear_spring_damp;
    f32 angular_spring_hz;
    f32 angular_spring_damp;
    f32 max_force;
};

struct phys_mouse_joint {
    struct sim_ent_id target;
    struct v2 point_local_start;
    struct 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;

    struct v2 linear_impulse;
    f32 angular_impulse;

    struct xform linear_mass_xf;
};

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

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

struct phys_weld_joint_def {
    struct sim_ent_id e0;
    struct sim_ent_id e1;

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

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

struct phys_weld_joint {
    struct sim_ent_id e0;
    struct sim_ent_id e1;
    struct 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;

    struct v2 linear_impulse0;
    struct v2 linear_impulse1;
    f32 angular_impulse0;
    f32 angular_impulse1;
};

struct phys_weld_joint_def phys_weld_joint_def_init(void);
struct phys_weld_joint phys_weld_joint_from_def(struct phys_weld_joint_def def);
void phys_prepare_weld_joints(struct phys_step_ctx *ctx);
void phys_warm_start_weld_joints(struct phys_step_ctx *ctx);
void phys_solve_weld_joints(struct phys_step_ctx *ctx, f32 dt);

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

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

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

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

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

void phys_update_aabbs(struct phys_step_ctx *ctx);

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

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

#endif