#include "util.h"

AxisAngle AxisAngleIdentity() {
	return (AxisAngle){Vector3Zero(), 0.0};
}

float signum(float n) {
	if (n > 0.0)
		return 1.0;
	else if (n < 0.0)
		return -1.0;
	else
		return 0.0;
}

void printVector3(Vector3 v) {
	printf("%f %f %f\n", v.x, v.y, v.z);
}

void printVector2(Vector2 v) {
	printf("%f %f\n", v.x, v.y);
}

// Warning. Mostly chatgpt written.

static void projectTriangleOntoAxis(const Triangle3D triangle, const Vector3 axis, float* min, float* max) {
    float dot1 = Vector3DotProduct(triangle[0], axis);
    float dot2 = Vector3DotProduct(triangle[1], axis);
    float dot3 = Vector3DotProduct(triangle[2], axis);

    *min = fminf(fminf(dot1, dot2), dot3);
    *max = fmaxf(fmaxf(dot1, dot2), dot3);
}

static bool trianglesIntersectOnAxis(const Triangle3D triangleA, const Triangle3D triangleB, Vector3 axis) {
    float minA, maxA;
    float minB, maxB;

    projectTriangleOntoAxis(triangleA, axis, &minA, &maxA);
    projectTriangleOntoAxis(triangleB, axis, &minB, &maxB);

    return (minA <= maxB && maxA >= minB);
}

bool checkTriangleCollision3D(const Triangle3D triangleA, const Triangle3D triangleB, Vector3 normalA, Vector3 normalB) {
    // Test triangle normals
    if (!trianglesIntersectOnAxis(triangleA, triangleB, normalA))
        return false;

    if (!trianglesIntersectOnAxis(triangleA, triangleB, normalB))
        return false;

    // Test cross products of triangle edges
    Vector3 edgesA[3] = {
        Vector3Subtract(triangleA[1], triangleA[0]),
        Vector3Subtract(triangleA[2], triangleA[1]),
        Vector3Subtract(triangleA[0], triangleA[2])
    };

    Vector3 edgesB[3] = {
        Vector3Subtract(triangleB[1], triangleB[0]),
        Vector3Subtract(triangleB[2], triangleB[1]),
        Vector3Subtract(triangleB[0], triangleB[2])
    };

    for (int i = 0; i < 3; i++) {
        Vector3 axis = Vector3CrossProduct(normalA, edgesA[i]);
        if (!trianglesIntersectOnAxis(triangleA, triangleB, axis))
            return false;

        axis = Vector3CrossProduct(normalB, edgesB[i]);
        if (!trianglesIntersectOnAxis(triangleA, triangleB, axis))
            return false;
    }

    return true;
}

void copyTriangle3D(Triangle3D a, const Triangle3D b) {
	a[0] = b[0];
	a[1] = b[1];
	a[2] = b[2];
}