#include "physics.h"
#include "object.h"
#include "world.h"
#include "math/vec.h"

static inline
float fclampf(float x, float min_, float max_) {
    return fminf(max_, fmaxf(min_, x));
}


physics_t physics_default() {
    return (physics_t) {
        .type=COLLIDERTYPE_NONE,
        .velocity_x = 0.f,
        .velocity_y = 0.f,
        .solver = &solve_collision_slide
    };
}

void object_broadcast_collision(object_t* this, object_t* other) {
    if(this->physics.evt_collision != NULL) {
        this->physics.evt_collision(this, other);
    }
}
short can_collide(const object_t* this) {
    return object_is_valid(this);
}

static inline
int _rect_overlap(float aminx, float aminy, float amaxx, float amaxy, float bminx, float bminy, float bmaxx, float bmaxy) {
    return
    (
        (aminx < bmaxx && aminx > bminx) 
        ||
        (bminx < amaxx  && bminx > aminx)
    ) && (
        (aminy < bmaxy && aminy > bminy)
        ||
        (bminy < amaxy && bminy > aminy)
    );
}

static inline
short _collision_aabb_aabb(const object_t* a, const object_t* b) {
    const float aminx = a->physics.aabb.x + a->sprite.x,
                  aminy = a->physics.aabb.y + a->sprite.y;
    const float amaxx = aminx + a->physics.aabb.w,
                  amaxy = aminy + a->physics.aabb.h;

    const float bminx = b->physics.aabb.x + b->sprite.x,
                bminy = b->physics.aabb.y + b->sprite.y;
    const float bmaxx = bminx + b->physics.aabb.w,
                bmaxy = bminy + b->physics.aabb.h;

    return _rect_overlap(aminx, aminy, amaxx, amaxy, bminx, bminy, bmaxx, bmaxy);
}

static inline
short _collision_circle_circle(const object_t* a, const object_t* b) {
    const float ax = a->sprite.x + a->physics.circle.x,
                  ay = a->sprite.y + a->physics.circle.y,
                  bx = b->sprite.x + b->physics.circle.x,
                by = b->sprite.y + b->physics.circle.y;
    const float dx = fabsf(ax-bx), dy = fabsf(ay-by);
    const float sqrdist = dx*dx+dy*dy;
    const float mindist = a->physics.circle.radius + b->physics.circle.radius;
    const float mindistsqr = mindist*mindist;
    return sqrdist < mindistsqr;
}

static inline
short _collision_circle_aabb(const object_t* circle, const object_t* aabb) {
    // generate a point on the edge of the rectangle that is closest to the circle
    const float bbminx = aabb->physics.aabb.x + aabb->sprite.x, bbmaxx = bbminx + aabb->physics.aabb.w,
                bbminy = aabb->physics.aabb.y + aabb->sprite.y, bbmaxy = bbminy + aabb->physics.aabb.h;
    const float cx = circle->sprite.x + circle->physics.circle.x,
                cy = circle->sprite.y + circle->physics.circle.y;
    const float x = fclampf(cx, bbminx, bbmaxx),
                y = fclampf(cy, bbminy, bbmaxy);
    const float dx = fabsf(cx - x), dy = fabsf(cy - y);

    // calculate the square distance from the centre of the circle to the edge of the aabb
    const float distsqr = dx*dx+dy*dy;
    const float rsqr = circle->physics.circle.radius*circle->physics.circle.radius;

    // return if the square distance is larger than the square of the radius
    return distsqr < rsqr;
}

static inline
short _collision_check(const object_t* a, const object_t* b) {
    if(a->physics.type == COLLIDERTYPE_AABB && b->physics.type == COLLIDERTYPE_AABB) {
        return _collision_aabb_aabb(a, b);
    } else if(a->physics.type == COLLIDERTYPE_CIRCLE && b->physics.type == COLLIDERTYPE_CIRCLE) {
        return _collision_circle_circle(a, b);
    } else if(a->physics.type == COLLIDERTYPE_CIRCLE && b->physics.type == COLLIDERTYPE_AABB) {
        return _collision_circle_aabb(a, b);
    } else if(a->physics.type == COLLIDERTYPE_AABB && b->physics.type == COLLIDERTYPE_CIRCLE) {
        return _collision_circle_aabb(b, a);
    }
    return 0;
}

static inline
float _solve_circle_aabb(const object_t* circle, const object_t* aabb, float* out_px, float* out_py) {
    // generate a point on the edge of the rectangle that is closest to the circle
    const float bbminx = aabb->physics.aabb.x + aabb->sprite.x, bbmaxx = bbminx + aabb->physics.aabb.w,
                bbminy = aabb->physics.aabb.y + aabb->sprite.y, bbmaxy = bbminy + aabb->physics.aabb.h;
    // the centre of the circle in world space
    const float cx = circle->sprite.x + circle->physics.circle.x,
                cy = circle->sprite.y + circle->physics.circle.y;
    // the point on the rectangle closest to the centre of the circle
    const float x = fclampf(cx, bbminx, bbmaxx),
                y = fclampf(cy, bbminy, bbmaxy);
    // the relative position of the point on the rectangle 
    const float dif_x = cx - x,
                dif_y = cy - y;
    // absolute difference for use in calculating euclidean distance 
    const float dist_x = fabsf(dif_x),
                dist_y = fabsf(dif_y);
    // euclidean distance
    const float dist = sqrt(dist_x*dist_x + dist_y*dist_y);
    const float solve_distance = circle->physics.circle.radius - dist;
    // distance to solve collision
    float solve_x, solve_y;
    normalize(dif_x, dif_y, &solve_x, &solve_y);
    *out_px = solve_x * solve_distance;
    *out_py = solve_y * solve_distance;
    return solve_distance;
}

static inline
float _solve_circle_circle(const object_t* a, const object_t* b, float* out_px, float* out_py) {
    const float x1 = a->physics.circle.x + a->sprite.x, y1 = a->physics.circle.y + a->sprite.y;
    const float x2 = b->physics.circle.x + b->sprite.x, y2 = b->physics.circle.y + b->sprite.y;
    const float dif_x = x1 - x2, dif_y = y1 - y2;
    const float difference = sqrtf(fabsf(dif_x*dif_x) + fabsf(dif_y*dif_y));
    const float target_difference = a->physics.circle.radius + b->physics.circle.radius;
    float dir_x, dir_y;
    normalize(dif_x, dif_y, &dir_x, &dir_y);
    *out_px = dir_x * target_difference;
    *out_py = dir_y * target_difference;
    return target_difference;
}

static inline
float _solve_aabb_aabb(const object_t* a, const object_t* b, float* out_px, float* out_py) {
    float aminx = a->physics.aabb.x + a->sprite.x;
    float amaxx = aminx + a->physics.aabb.w;
    float bminx = b->physics.aabb.x + b->sprite.x;
    float bmaxx = bminx + b->physics.aabb.w;

    float aminy = a->physics.aabb.y + a->sprite.y;
    float amaxy = aminy + a->physics.aabb.h;
    float bminy = b->physics.aabb.y + b->sprite.y;
    float bmaxy = bminy + b->physics.aabb.h;

    float right = bmaxx - aminx;
    float left = bminx - amaxx;
    float top = bminy - amaxy;
    float bottom = bmaxy - aminy;

    float ret = right;
    *out_px = right;
    *out_py = 0.f;
    if(fabsf(left) < fabsf(ret)) {
        *out_px = left;
        *out_py = 0.f;
        ret = left;
    }
    if(fabsf(top) < fabsf(ret)) {
        *out_px = 0.f;
        *out_py = top;
        ret = top;
    }

    if(fabsf(bottom) < fabsf(ret)) {
        *out_px = 0.f;
        *out_py = bottom;
        return bottom;
    }

    return ret;
}

float get_solve_force(const object_t* a, const object_t* b, float* out_px, float* out_py) {
    if(a->physics.type == COLLIDERTYPE_AABB && b->physics.type == COLLIDERTYPE_AABB) {
        return _solve_aabb_aabb(a, b, out_px, out_py);
    } else if(a->physics.type == COLLIDERTYPE_AABB && b->physics.type == COLLIDERTYPE_CIRCLE) {
        float penetration_distance = _solve_circle_aabb(b, a, out_px, out_py);
        *out_px = -(*out_px);
        *out_py = -(*out_py);
    } else if(a->physics.type == COLLIDERTYPE_CIRCLE && b->physics.type == COLLIDERTYPE_AABB) {
        return _solve_circle_aabb(a, b, out_px, out_py);
    } else if(a->physics.type == COLLIDERTYPE_CIRCLE && b->physics.type == COLLIDERTYPE_CIRCLE) {
        return _solve_circle_circle(a, b, out_px, out_py);
    }
}

void solve_collision_slide(object_t* left, object_t* right) {
    float dx, dy;
    get_solve_force(left, right, &dx, &dy);
    left->sprite.x += dx;
    left->sprite.y += dy;
}

static inline
void _solve_move(object_t* this) {
    // loop over all objects and check collision if applicable
    for(int i = 0; i < world_num_objects(); ++i) {
        // get pointer to other object
        object_t* other = world_get_object(i);
        // check collision, return if found
        if(can_collide(other) && this != other && _collision_check(other, this)) {
            object_broadcast_collision(other, this);
            object_broadcast_collision(this, other);
            this->physics.solver(this, other);
        }
    }
}

void physics_move(object_t* this, float delta_time) {
    const float max_step_size = this->physics.max_interpolate_step_size;
    // calculate step delta
    float dx = this->physics.velocity_x * delta_time, dy = this->physics.velocity_y * delta_time;
    const float target_x = this->sprite.x + dx,
                target_y = this->sprite.y + dy;
    if(dx == 0 && dy == 0)
        return;

    // calculate direction x,y
    float m = sqrtf(dx*dx + dy*dy);
    dx = dx / m * max_step_size;
    dy = dy / m * max_step_size;

    const int step_count = max_step_size / m;

    // ensure this object would ever collide
    // if it wouldn't collide anyway, just set position
    if(!can_collide(this)) {
        this->sprite.x = target_x;
        this->sprite.y = target_y;
        return;
    }

    if(step_count == 0) {
        this->sprite.x = target_x;
        this->sprite.y = target_y;
        _solve_move(this);
        return;
    }

    /*
     * 1. move towards target
     * 2. check collision with every other object
     */
    for(int steps = 0; steps <= step_count && (this->sprite.x != target_x || this->sprite.y != target_y); ++steps) {
        // move towards target, snap to target if distance is too low
        const float distx = fabsf(this->sprite.x - target_x), disty = fabsf(this->sprite.y - target_y);
        const float sqdist = distx*distx + disty*disty;
        if(sqdist > max_step_size*max_step_size) {
            this->sprite.x += dx;
            this->sprite.y += dy;
        } else {
            this->sprite.x = target_x;
            this->sprite.y = target_y;
        }

        _solve_move(this);
    }
}