#include "rigidbody.h"
#include "camera.h"
#include "debug.h"
#include "program.h"
#include "collision.h"
#include "transformable.h"

struct RigidBody {
    Transformable transformable;

    float mass;

    float bounce;

    Vector last_linear_force;
    Vector next_linear_force;
    Vector linear_velocity;
    Transform internal_transform;

    int is_static;

    List contacts;
};

RigidBody* rigidbody_make(Transformable transform) {
    RigidBody* self = malloc(sizeof(RigidBody));
    ASSERT_RETURN(self != NULL, NULL, "Failed to allocate space for rigidbody");
    *self = (RigidBody){
        .transformable = transform,
        .mass = 1.0f,
        .bounce = 0.0f,

        .linear_velocity = ZeroVector,
        .next_linear_force = ZeroVector,
        .last_linear_force = ZeroVector,

        .internal_transform = *transform.tc->get_transform(transform.data),

        .is_static = 0,
        .contacts = list_from_type(Contact),
    };
    return self;
}

void rigidbody_destroy(RigidBody* self) {
    list_empty(&self->contacts);
    free(self);
}

void rigidbody_add_contact(RigidBody* self, Collision hit) {
    list_add(&self->contacts,
    &(Contact) {
        .hit = hit,
        .duration = delta_time()
    });
}

void rigidbody_collect_contacts(RigidBody* self) {
    list_empty(&self->contacts);
}

List* rigidbody_get_contacts(RigidBody* self) {
    return &self->contacts;
}

static inline
void _internal_debug_draw_collision_edge(RigidBody* self, Contact* contact) {
#if !NDEBUG
    Vector left = contact->hit.edge_left;
    Vector right = contact->hit.edge_right;
    Vector point = transform_point(&self->internal_transform, contact->hit.point);
    Vector a = camera_world_to_pixel_point(&g_camera, left);
    Vector b = camera_world_to_pixel_point(&g_camera, right);
    Vector n = transform_direction(&g_camera.transform, contact->hit.normal);
    SDL_SetRenderDrawColor(g_renderer, 255, 2, 255, 255);
    SDL_RenderDrawLine(g_renderer, a.x, a.y, b.x, b.y);
    a = camera_world_to_pixel_point(&g_camera, point);
    b = vaddf(a, vmulff(n, 100.f));
    SDL_SetRenderDrawColor(g_renderer, 255, 0, 0, 255);
    SDL_RenderDrawLine(g_renderer, a.x, a.y, b.x, b.y);
#endif
}

void rigidbody_integrate_forces(RigidBody* self) {
    const float dt = delta_time();

    Vector position = self->internal_transform.position;
    Vector velocity = self->linear_velocity;
    Vector acceleration = vmulff(self->last_linear_force, 0.5f * dt * dt);

    position = vaddf(position, vmulff(velocity, dt));
    position = vaddf(position, acceleration);

    acceleration = vmulff(vaddf(self->next_linear_force, acceleration), dt * 0.5f);
    velocity = vaddf(velocity, acceleration);

    self->linear_velocity = velocity;
    self->internal_transform.position = position;
    transformable_set_position(self->transformable, position);

    self->last_linear_force = self->next_linear_force;
    self->next_linear_force = ZeroVector;
}

float rigidbody_get_mass(const RigidBody* self) {
    return self->mass;
}

void rigidbody_set_mass(RigidBody* self, float mass) {
    self->mass = mass;
}

float rigidbody_get_bounce(const RigidBody* self) {
    return self->bounce;
}

void rigidbody_set_bounce(RigidBody* self, float bounce) {
    self->bounce = bounce;
}

void rigidbody_add_impulse(RigidBody* self, Vector force, int use_mass) {
    rigidbody_accelerate(self, vmulff(force, 1.0f/delta_time()), use_mass);
}

void rigidbody_accelerate(RigidBody* self, Vector force, int use_mass) {
    if(use_mass)
        force = vmulff(force, 1.0f / self->mass);
    if(vsqrmagnitudef(force) > powf(0.01f, 2))
        self->next_linear_force = vaddf(self->next_linear_force, force);
}

int rigidbody_is_static(const RigidBody* self) {
    return self->is_static;
}

void rigidbody_set_static(RigidBody* self, int is_static) {
    self->is_static = is_static;
}

Vector rigidbody_get_velocity(const RigidBody* self) {
    return self->linear_velocity;
}

void rigidbody_set_velocity(RigidBody* self, Vector velocity) {
    self->next_linear_force = vaddf(self->next_linear_force, vsubf(velocity, self->next_linear_force));
    self->linear_velocity = velocity;
}

Vector rigidbody_get_force(RigidBody* self) {
    return self->next_linear_force;
}

void rigidbody_debug_draw_contacts(RigidBody* self) {
    list_foreach(Contact*, contact, &self->contacts) {
        _internal_debug_draw_collision_edge(self, contact);
    }
}

Transform* rigidbody_get_transform(RigidBody* self) {
    return &self->internal_transform;
}