#include "frustum.h"
#include "_internal/private_frustum.h"

#include <math.h>
#include <stdint.h>

/**
 * Extracts the six clipping planes from a combined View-Projection matrix.
 * * This function utilizes the Gribb-Hartmann method to derive plane equations
 * directly from the matrix rows. The resulting planes are normalized to ensure
 * that the distance calculations in visibility tests are accurate.
 */
void extract_frustum_planes(frustum_plane_struct planes[number_of_planes], 
                                Matrix view_projection)
{
    /* Left Clipping Plane */
    planes[left].normal.x = view_projection.m3  + view_projection.m0;
    planes[left].normal.y = view_projection.m7  + view_projection.m4;
    planes[left].normal.z = view_projection.m11 + view_projection.m8;
    planes[left].distance = view_projection.m15 + view_projection.m12;

    /* Right Clipping Plane */
    planes[right].normal.x = view_projection.m3  - view_projection.m0;
    planes[right].normal.y = view_projection.m7  - view_projection.m4;
    planes[right].normal.z = view_projection.m11 - view_projection.m8;
    planes[right].distance = view_projection.m15 - view_projection.m12;

    /* Bottom Clipping Plane */
    planes[bottom].normal.x = view_projection.m3  + view_projection.m1;
    planes[bottom].normal.y = view_projection.m7  + view_projection.m5;
    planes[bottom].normal.z = view_projection.m11 + view_projection.m9;
    planes[bottom].distance = view_projection.m15 + view_projection.m13;

    /* Top Clipping Plane */
    planes[top].normal.x = view_projection.m3  - view_projection.m1;
    planes[top].normal.y = view_projection.m7  - view_projection.m5;
    planes[top].normal.z = view_projection.m11 - view_projection.m9;
    planes[top].distance = view_projection.m15 - view_projection.m13;

    /* Near Clipping Plane */
    planes[near].normal.x = view_projection.m3  + view_projection.m2;
    planes[near].normal.y = view_projection.m7  + view_projection.m6;
    planes[near].normal.z = view_projection.m11 + view_projection.m10;
    planes[near].distance = view_projection.m15 + view_projection.m14;

    /* Far Clipping Plane */
    planes[far].normal.x = view_projection.m3  - view_projection.m2;
    planes[far].normal.y = view_projection.m7  - view_projection.m6;
    planes[far].normal.z = view_projection.m11 - view_projection.m10;
    planes[far].distance = view_projection.m15 - view_projection.m14;

    /* Normalize all planes so that normal length equals 1.0 */
    for (uint8_t plane_index = 0; plane_index < number_of_planes; plane_index++)
    {
        float magnitude = sqrtf(
            planes[plane_index].normal.x * planes[plane_index].normal.x +
            planes[plane_index].normal.y * planes[plane_index].normal.y +
            planes[plane_index].normal.z * planes[plane_index].normal.z
        );

        if (magnitude > 0.0f)
        {
            float inverse_magnitude = 1.0f / magnitude;
            planes[plane_index].normal.x *= inverse_magnitude;
            planes[plane_index].normal.y *= inverse_magnitude;
            planes[plane_index].normal.z *= inverse_magnitude;
            planes[plane_index].distance *= inverse_magnitude;
        }
    }
}

/**
 * Checks if an Axis-Aligned Bounding Box (AABB) is within the view frustum.
 * * This function performs a fast visibility test by finding the "positive vertex" 
 * of the box (the corner most aligned with the plane normal). If this vertex 
 * is outside any of the six planes, the entire box is considered culled.
 */
bool is_axis_aligned_bounding_box_in_frustum( const frustum_plane_struct planes
                                                [
                                                        number_of_planes
                                                ], 
                                                BoundingBox bounds)
{
    for (uint8_t plane_index = 0; plane_index < number_of_planes; plane_index++)
    {
        Vector3 positive_vertex;

        positive_vertex.x = 
                (planes[plane_index].normal.x >= 0.0f) ? bounds.max.x : bounds.min.x;
                
        positive_vertex.y = 
                (planes[plane_index].normal.y >= 0.0f) ? bounds.max.y : bounds.min.y;
                
        positive_vertex.z = 
                (planes[plane_index].normal.z >= 0.0f) ? bounds.max.z : bounds.min.z;

        /* Calculate the signed distance from the plane to the positive vertex */
        float vertex_distance = 
            planes[plane_index].normal.x * positive_vertex.x +
            planes[plane_index].normal.y * positive_vertex.y +
            planes[plane_index].normal.z * positive_vertex.z +
            planes[plane_index].distance;

        /* If the most positive point is behind the plane, the box is outside the frustum */
        if (vertex_distance < 0.0f)
        {
            return false;
        }
    }

    return true;
}