#include <js.h>
#include <math.h>
#include <memory.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <vec2.h>

#define TYPE_EMPTY 0
#define TYPE_CIRCLE 1
#define TYPE_PLANE 2

////////// Types

typedef struct rigid_body_t {
    // Public
    uint8_t type;
    uint8_t reserved[3];
    vec2 pos;
    vec2 vel;
    float mass;
    union {
        struct {
            float radius;
        } circle;
        struct {
            vec2 normal;
        } plane;
    };
    // Private
    vec2 force;
} rigid_body;

typedef size_t rigid_body_index;
typedef void (*rigid_body_collision_callback_t)(rigid_body_index a, rigid_body_index b);

////////// Prototypes

void rigid_body_resolve_collision(rigid_body_index rb);
void rigid_body_handle_collision(rigid_body_index a, rigid_body_index b);
float point_to_line_dist(vec2 point, vec2 line_point, vec2 line_norm);
rigid_body_index rigid_body_find_empty();

////////// Globals

static rigid_body* rigid_bodies = NULL;
static size_t rigid_bodies_cap = 0;
static rigid_body_collision_callback_t rigid_body_collision_callback = NULL;

////////// Functions

JS_EXPORT rigid_body* rigid_body_get(rigid_body_index idx) {
    return (rigid_bodies + (idx));
}

size_t rigid_body_new(float x, float y, float vx, float vy, float mass) {
    rigid_body_index idx = rigid_body_find_empty();
    rigid_body* rb = rigid_body_get(idx);
    rb->type = TYPE_EMPTY;

    rb->pos.x = x;
    rb->pos.y = y;

    rb->vel.x = vx;
    rb->vel.y = vy;

    rb->force.x = 0;
    rb->force.y = 0;

    rb->mass = mass;

    return idx;
}

JS_EXPORT rigid_body_index rigid_body_new_circle(float x, float y, float vx, float vy, float mass, float radius) {
    rigid_body_index idx = rigid_body_new(x, y, vx, vy, mass);
    rigid_body* rb = rigid_body_get(idx);

    rb->type = TYPE_CIRCLE;
    rb->circle.radius = radius;

    return idx;
}

JS_EXPORT rigid_body_index rigid_body_new_plane(float x, float y, float nx, float ny) {
    rigid_body_index idx = rigid_body_new(x, y, 0, 0, INFINITY);
    rigid_body* rb = rigid_body_get(idx);

    rb->type = TYPE_PLANE;
    rb->plane.normal = vec2_normalize((vec2){nx, ny});

    return idx;
}

JS_EXPORT void rigid_body_free(rigid_body_index idx) {
    memset(rigid_body_get(idx), 0, sizeof(rigid_body));
}

JS_EXPORT void rigid_body_update(rigid_body_index idx, float dt) {
    rigid_body_resolve_collision(idx);
    rigid_body* rb = rigid_body_get(idx);

    if (!isinf(rb->mass)) {
        rb->vel.x += rb->force.x * dt / rb->mass;
        rb->vel.y += rb->force.y * dt / rb->mass;

        rb->pos.x += rb->vel.x * dt;
        rb->pos.y += rb->vel.y * dt;
    }

    rb->force.x = 0;
    rb->force.y = 0;
}

JS_EXPORT void rigid_body_update_all(float dt) {
    for (rigid_body_index idx = 0; idx < rigid_bodies_cap; idx++) {
        rigid_body* current = rigid_body_get(idx);
        if (current->type == TYPE_EMPTY) {
            continue;
        }
        rigid_body_update(idx, dt);
    }
}

JS_EXPORT void rigid_body_add_force(rigid_body_index idx, float fx, float fy) {
    rigid_body* rb = rigid_body_get(idx);
    rb->force.x += fx;
    rb->force.y += fy;
}

JS_EXPORT void rigid_body_add_global_force(float fx, float fy) {
    for (rigid_body_index idx = 0; idx < rigid_bodies_cap; idx++) {
        rigid_body* current = rigid_body_get(idx);
        if (current->type == TYPE_EMPTY) {
            continue;
        }
        rigid_body_add_force(idx, fx, fy);
    }
}

JS_EXPORT void rigid_body_set_collision_callback(rigid_body_collision_callback_t callback) {
    rigid_body_collision_callback = callback;
}

void rigid_body_resolve_collision(rigid_body_index idx) {
    rigid_body* rb = rigid_body_get(idx);
    int is_static = isinf(rb->mass);
    for (rigid_body_index i = idx + 1; i < rigid_bodies_cap; i++) {
        rigid_body* current = rigid_body_get(i);
        if (!is_static || !isinf(current->mass)) {
            rigid_body_handle_collision(idx, i);
        }
    }
}

void rigid_body_handle_collision(rigid_body_index idx1, rigid_body_index idx2) {
    rigid_body* rb1 = rigid_body_get(idx1);
    rigid_body* rb2 = rigid_body_get(idx2);
    if (rb1->type == TYPE_CIRCLE && rb2->type == TYPE_CIRCLE) {
        vec2 d = vec2_sub(rb2->pos, rb1->pos);
        float distance = vec2_mag(d);

        float overlap = rb1->circle.radius + rb2->circle.radius - distance;
        if (overlap < 0) {
            return;
        }

        if (rigid_body_collision_callback) {
            rigid_body_collision_callback(idx1, idx2);
        }

        vec2 n = vec2_normalize(d);
        float factor = (isinf(rb1->mass) || isinf(rb2->mass)) ? 1 : 0.5;
        vec2 n_overlap = vec2_mul(n, overlap * factor);

        if (!isinf(rb1->mass)) {
            rb1->pos = vec2_sub(rb1->pos, n_overlap);
        }
        if (!isinf(rb2->mass)) {
            rb2->pos = vec2_add(rb2->pos, n_overlap);
        }

        float v1n = vec2_dot(rb1->vel, n);
        float v2n = vec2_dot(rb2->vel, n);

        float v1n_new = v1n;
        float v2n_new = v2n;

        if (!isinf(rb1->mass) && !isinf(rb2->mass)) {
            v1n_new = v1n - 2.0f * (v1n - v2n) * rb1->mass / (rb1->mass + rb2->mass);
            v2n_new = v2n + 2.0f * (v1n - v2n) * rb2->mass / (rb1->mass + rb2->mass);
        } else if (isinf(rb1->mass)) {
            v2n_new = -v2n;
        } else if (isinf(rb2->mass)) {
            v1n_new = -v1n;
        } else {
            return;
        }

        vec2 v1t = vec2_sub(rb1->vel, vec2_mul(n, v1n));
        vec2 v2t = vec2_sub(rb2->vel, vec2_mul(n, v2n));

        if (!isinf(rb1->mass)) {
            rb1->vel = vec2_add(v1t, vec2_mul(n, v1n_new));
        }
        if (!isinf(rb2->mass)) {
            rb2->vel = vec2_add(v2t, vec2_mul(n, v2n_new));
        }
    } else if (rb1->type == TYPE_PLANE && rb2->type == TYPE_CIRCLE) {
        float distance = point_to_line_dist(rb2->pos, rb1->pos, rb1->plane.normal) - rb2->circle.radius;

        float overlap = -distance;
        if (overlap < 0) {
            return;
        }

        if (rigid_body_collision_callback) {
            rigid_body_collision_callback(idx1, idx2);
        }

        vec2 n = rb1->plane.normal;
        vec2 n_overlap = vec2_mul(n, overlap);

        rb2->pos = vec2_add(rb2->pos, n_overlap);
        rb2->vel = vec2_reflect(rb2->vel, n);
    } else if (rb1->type == TYPE_CIRCLE && rb2->type == TYPE_PLANE) {
        rigid_body_handle_collision(idx2, idx1);
    }
}

float point_to_line_dist(vec2 p, vec2 line_point, vec2 normal) {
    vec2 diff = vec2_sub(p, line_point);
    return vec2_dot(diff, normal);
}

rigid_body_index rigid_body_find_empty() {
    if (rigid_bodies) {
        for (rigid_body_index idx = 0; idx < rigid_bodies_cap; idx++) {
            rigid_body* current = rigid_body_get(idx);
            if (current->type == TYPE_EMPTY) {
                memset(current, 0, sizeof(rigid_body));
                return idx;
            }
        }
        size_t new_cap = rigid_bodies_cap * 2;
        rigid_bodies = realloc(rigid_bodies, new_cap * sizeof(rigid_body));
        memset(rigid_bodies + rigid_bodies_cap, 0, rigid_bodies_cap * sizeof(rigid_body));
        rigid_bodies_cap = new_cap;
        return rigid_body_find_empty();
    } else {
        rigid_bodies_cap = 2;
        rigid_bodies = malloc(rigid_bodies_cap * sizeof(rigid_body));
        memset(rigid_bodies, 0, rigid_bodies_cap * sizeof(rigid_body));
        return 0;
    }
}
