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

typedef struct {
    Collision hit;
    float warming;
    int expiry;
} Contact;

struct RigidBody {
    Transformable transformable;

    float mass;

    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,

        .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) {
    LOG_INFO("contact between rigidbody %p and %p detected", self->transformable.data, hit.other.data);
    // update an existing contact
    list_foreach(Contact, contact, &self->contacts) {
        if(contact->hit.other.data == hit.other.data) {
            ++contact->expiry;
            hit.velocity = contact->hit.velocity;
            contact->hit = hit;
            contact->warming += delta_time();
            return;
        }
    }

    // create a new contact
    list_add(&self->contacts,
    &(Contact) {
        .expiry = 1,
        .hit = hit,
        .warming = delta_time()
    });

    Contact* c = list_at(&self->contacts, self->contacts.len - 1);
    LOG_INFO("added contact %p with exp %d", c->hit.other.data, c->expiry);
}

static inline
void _internal_rigidbody_collect_contacts(RigidBody* self) {
    for(size_t i = 0; i < self->contacts.len; ++i) {
        Contact* contact = list_at(&self->contacts, i);

        if(contact->expiry <= 0) {
            if(vdotf(vnormalizedf(self->linear_velocity), contact->hit.normal) >= -0.01f || contact->expiry <= -1) {
                list_erase(&self->contacts, i);
                i--;
                continue;
            }
        }
        --(contact->expiry);
    }
}

static inline
void _internal_debug_draw_collision_edge(Vector left, Vector right, Vector normal) {
#if !NDEBUG
    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, normal);
    SDL_SetRenderDrawColor(g_renderer, 255, 255, 255, 255);
    SDL_RenderDrawLine(g_renderer, a.x, a.y, b.x, b.y);
    a = vlerpf(a, b, 0.5f);
    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
}

static  inline
Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) {
    Collision hit = contact->hit;

    if(vsqrmagnitudef(hit.penetration_vector) < powf(0.01f, 2))
        return ZeroVector;


    const float warming = fminf(1.f, contact->warming * 1000.f);
    const float d = 1.0f;
    const float k = 0.3f;
    const float b = 0.3f;

    const Vector damping = vmulff(hit.normal, k * d);
    const Vector bounce = vmulff(hit.normal, b * vdotf(hit.normal, hit.velocity));

    float dot = -vdotf(contact->hit.normal, vaddf(self->next_linear_force, self->linear_velocity));
    if(dot < 0)
        dot = 0;
    Vector counter_force = vmulff(contact->hit.normal, dot * vsqrmagnitudef(hit.penetration_vector));

    return vaddf(vmulff(vsubf(damping, bounce), warming), counter_force);
}

static inline
void _internal_rigidbody_solve_contact(RigidBody* self, Contact* contact, Vector* solve_forces) {
    Collision hit = contact->hit;
    if(vdotf(hit.normal, self->linear_velocity) > 0.0)
        return;

    float normalmag = vmagnitudef(contact->hit.normal);
    ASSERT_RETURN(fabsf(normalmag - 1.f) < 0.99999f,, "Normal of collision not one");

    Vector force = _internal_calculate_contact_force(self, contact);
    float dot = vdotf(vnormalizedf(*solve_forces), force);
    if (veqf(*solve_forces, ZeroVector) || dot * dot > vsqrmagnitudef(*solve_forces) || dot <= 0.0f) {
        #if !NDEBUG
            LOG_INFO("warming: %f", contact->warming);
            LOG_INFO("force: %f %f", force.x, force.y);
            LOG_INFO("dot: %f", dot);
            LOG_INFO("mag: %f", vmagnitudef(self->next_linear_force));
        #endif
        *solve_forces = vaddf(*solve_forces, force);
        rigidbody_add_impulse(self, force);
    }

    ASSERT_RETURN(!visnanf(force), , "Force contains NaN (1)");
}

void rigidbody_solve_contacts(RigidBody* self) {
    ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (0)");

    _internal_rigidbody_collect_contacts(self);

    Vector solve_forces = ZeroVector;
    list_foreach(Contact, contact, &self->contacts) {
        _internal_rigidbody_solve_contact(self, contact, &solve_forces);
    }

    ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (1)");
}

static inline
void _internal_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);

    velocity = vaddf(velocity, acceleration);
    acceleration = vmulff(self->next_linear_force, dt/2);
    velocity = vaddf(velocity, acceleration);

    self->linear_velocity = velocity;
    self->internal_transform.position = position;
    if(vsqrdistf(position, transformable_get_position(self->transformable)) > 0.001f)
        transformable_set_position(self->transformable, position);

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

void rigidbody_apply_physics(RigidBody* self) {
    rigidbody_solve_contacts(self);
    _internal_rigidbody_integrate_forces(self);
}

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

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

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

void rigidbody_accelerate(RigidBody* self, Vector force) {
    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;
}

void rigidbody_debug_draw_contacts(RigidBody* self) {
    list_foreach(Contact, contact, &self->contacts) {
        _internal_debug_draw_collision_edge(contact->hit.edge_left, contact->hit.edge_right, contact->hit.normal);
    }
}

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