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

struct RigidBody {
    PhysicsEntity owner;

    float mass;
    float bounce;

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

    PhysicsMask layers;
    PhysicsMask collision_mask;
    
    List colliders;

    int is_static;

    List contacts;
};

impl_Transformable_for(RigidBody,
    rigidbody_get_transform
)

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

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

        .internal_transform = *owner.transformable->get_transform(owner.data),

        .layers = 0x1,
        .collision_mask = 0x1,
        
        .colliders = list_from_type(Collider*),
        
        .is_static = 0,
        
        .contacts = list_from_type(Contact),
    };
    self->internal_transform.scale = OneVector;
    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_RenderDrawLineF(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_RenderDrawLineF(g_renderer, a.x, a.y, b.x, b.y);
#endif
}

void rigidbody_integrate_forces(RigidBody* self) {
    if(self->is_static)
        return;
    
    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;
    Transform* owner_trans = self->owner.transformable->get_transform(self->owner.data);
    owner_trans->position = 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);
}

PhysicsMask rigidbody_get_layers(RigidBody* self) {
    return self->layers;
}

void rigidbody_set_layers(RigidBody* self, PhysicsMask layers) {
    self->layers = layers;
}

PhysicsMask rigidbody_get_collision_mask(RigidBody* self) {
    return self->collision_mask;
}

void rigidbody_set_collision_mask(RigidBody* self, PhysicsMask mask) {
    self->collision_mask = mask;
}

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

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

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_add_collider(RigidBody *self, Collider* collider) {
    list_add(&self->colliders, &collider);
}

void rigidbody_remove_collider(RigidBody *self, Collider* collider) {
    for(size_t i = 0; i < self->colliders.len; ++i) {
        if(collider == *list_at_as(Collider*, &self->colliders, i)) {
            list_erase(&self->colliders, i);
            return;
        }
    }
}

List* rigidbody_get_colliders(RigidBody* self) {
    return &self->colliders;
}

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;
}