// todo: kill myself
#include <math.h>

#include "include/render.h"
#include "include/matrix.h"

#define __EPSILON 0.04f
#define __COLL_FAR 10


struct Triangle {
    vec4 A, B, C;
};

struct plane {
    float a,b,c,d;
};

struct intersection {
    vec4 point;
    float length;
};

static inline float triangleArea(float len_a, float len_b, float len_c) {
    float s = len_a+len_b+len_c;
    s /=2;
    return sqrt(s*(s-len_a)*(s-len_b)*(s-len_c));
}

vec4 rayTriangleIntersection(vec4 ray_dir, vec4 ray_pos, struct Triangle triangle) {
    struct plane plane;
    vec4 edgeAB = subVec4(triangle.B, triangle.A);
    vec4 edgeAC = subVec4(triangle.C, triangle.A);
    vec4 plane_normal = normalizeVec4(crossVec4(edgeAB, edgeAC));

    if (dot(plane_normal, ray_dir) < __EPSILON) {
        return (vec4) {0, 0, 0, -1}; // almost/is parallel
    }

    plane.a = plane_normal.x+__EPSILON; // to avoid division by 0
    plane.b = plane_normal.y+__EPSILON; // to avoid division by 0
    plane.c = plane_normal.z+__EPSILON; // to avoid division by 0

    plane.d = -(triangle.A.x*plane.a + triangle.A.y*plane.b + triangle.A.z*plane.c);

    float t;
    t = (-plane.a*ray_pos.x - plane.b*ray_pos.y - plane.c*ray_pos.z - plane.d)/(plane.a*ray_dir.x + plane.b*ray_dir.y + plane.c*ray_dir.z);
    if (t>__COLL_FAR || t < 0) { // t < 0 when collision is behind ray
        return (vec4) {0,0,0,-1}; // plane intersection is too far, triangle intersection highly unlikely/irrelevant
    }

    vec4 out = {ray_pos.x+ray_dir.x*t, ray_pos.y+ray_dir.y*t, ray_pos.z+ray_dir.z*t};

    vec4 edgeBC = subVec4(triangle.C, triangle.B);
    float ABC = triangleArea(lenVec4(edgeBC), lenVec4(edgeAC), lenVec4(edgeAB));

    float ABP = triangleArea(lenVec4(subVec4(out, triangle.B)), lenVec4(subVec4(out, triangle.A)), lenVec4(edgeAB));
    float APC = triangleArea(lenVec4(subVec4(out, triangle.A)), lenVec4(edgeAC), lenVec4(subVec4(out, triangle.C)));
    float PBC = triangleArea(lenVec4(edgeBC), lenVec4(subVec4(out, triangle.B)), lenVec4(subVec4(out, triangle.C)));

    #define _DELTA 0.05

    if (ABP + APC + PBC > ABC-_DELTA && ABP+APC+PBC < ABC+_DELTA) {
        return out;
    } else {
        //printf("\r collision miss at %f %f %f...", out.x, out.y, out.z);
        return (vec4) {0, 0, 0,-1};
    }

    /* 
    first we calculate the parametric equation of the ray, then the general equation of the plane and then we fill the ray equation into the plane equation
    getting us the distance from the ray srcpoint to the plane
    
    then we fill this value back into the ray equation getting us the intersection coordinates
    and finally we calculate whether the intersection is inside or outside of the target triangle

    this is done by the incredibly inefficient method of calculating the areas of ABP, ACP, BCP and ABC and if ABP+ACP+BCP is equal to ABC, then naturally P lies inside the triangle

    ray:
        x = ray_pos.x+ray_dir.x*t
        y = ray_pos.y+ray_dir.y*t
        z = ray_pos.z+ray_dir.z*t
    plane:
        x*plane_normal.x + y*plane_normal.y + z*plane_normal.z + d = 0

        plane_normal.x*ray_pos.x + plane_normal.x*ray_dir.x*t + plane_normal.y*ray_pos.y + plane_normal.y*ray_dir.y*t + plane_normal.z*ray_pos.z + plane_normal.z*ray_dir.z*t + d = 0
        (plane_normal.x*ray_dir.x + plane_normal.y*ray_dir.y + plane_normal.z*ray_dir.z)*t = -plane_normal.x*ray_pos.x - plane_normal.y*ray_pos.y - plane_normal.z*ray_pos.z - d
        t = (-plane_normal.x*ray_pos.x - plane_normal.y*ray_pos.y - plane_normal.z*ray_pos.z - d)/(plane_normal.x*ray_dir.x + plane_normal.y*ray_dir.y + plane_normal.z*ray_dir.z)
        
        t = (-plane.a*ray_pos.x - plane.b*ray_pos.y - plane.c*ray_pos.z - plane.d)/(plane.a*ray_dir.x + plane.b*ray_dir.y + plane.c*ray_dir.z);
    */
}

float Collision(struct object object, vec4 direction, vec4 srcpoint) {
    struct Triangle triangle;

    float distance;
    vec4 intersectPoint;

    float smallestDistance = 10000;

    for (int i =0; i<object.indices_size; i+=3) {
        // get triangle
        triangle.A = (vec4) {object.vertices[object.indices[i]*3],object.vertices  [object.indices[i]*3+1],object.vertices  [object.indices[i]*3+2]};
        triangle.A = addVec4(object.position, rotateVec4(triangle.A, object.rotation));
        triangle.B = (vec4) {object.vertices[object.indices[i+1]*3],object.vertices[object.indices[i+1]*3+1],object.vertices[object.indices[i+1]*3+2]};
        triangle.B = addVec4(object.position, rotateVec4(triangle.B, object.rotation));
        triangle.C = (vec4) {object.vertices[object.indices[i+2]*3],object.vertices[object.indices[i+2]*3+1],object.vertices[object.indices[i+2]*3+2]};
        triangle.C = addVec4(object.position, rotateVec4(triangle.C, object.rotation));

        intersectPoint = rayTriangleIntersection(direction, srcpoint, triangle);
        if (intersectPoint.w != -1) {
            distance = lenVec4(subVec4(intersectPoint, srcpoint));
            if (distance < smallestDistance) {
                smallestDistance = distance;
            }
        }
    }
    return smallestDistance;
}