#include "frustum.h"
#include <math.h>

void frustum_from_matrix(const Mat4* vp, Vec4 planes[6]) {
    // 行主序存储：clip = vec4(pos,1) * vp
    // 因此 clip 分量 = dot(pos, vp 的列)
    // left   = col3 + col0
    // right  = col3 - col0
    // bottom = col3 + col1
    // top    = col3 - col1
    // near   = col3 + col2
    // far    = col3 - col2
    planes[0] = (Vec4){ vp->m[0][3] + vp->m[0][0],
                        vp->m[1][3] + vp->m[1][0],
                        vp->m[2][3] + vp->m[2][0],
                        vp->m[3][3] + vp->m[3][0] };
    planes[1] = (Vec4){ vp->m[0][3] - vp->m[0][0],
                        vp->m[1][3] - vp->m[1][0],
                        vp->m[2][3] - vp->m[2][0],
                        vp->m[3][3] - vp->m[3][0] };
    planes[2] = (Vec4){ vp->m[0][3] + vp->m[0][1],
                        vp->m[1][3] + vp->m[1][1],
                        vp->m[2][3] + vp->m[2][1],
                        vp->m[3][3] + vp->m[3][1] };
    planes[3] = (Vec4){ vp->m[0][3] - vp->m[0][1],
                        vp->m[1][3] - vp->m[1][1],
                        vp->m[2][3] - vp->m[2][1],
                        vp->m[3][3] - vp->m[3][1] };
    planes[4] = (Vec4){ vp->m[0][3] + vp->m[0][2],
                        vp->m[1][3] + vp->m[1][2],
                        vp->m[2][3] + vp->m[2][2],
                        vp->m[3][3] + vp->m[3][2] };
    planes[5] = (Vec4){ vp->m[0][3] - vp->m[0][2],
                        vp->m[1][3] - vp->m[1][2],
                        vp->m[2][3] - vp->m[2][2],
                        vp->m[3][3] - vp->m[3][2] };

    for (int i = 0; i < 6; i++) {
        float len = sqrtf(planes[i].x * planes[i].x +
                          planes[i].y * planes[i].y +
                          planes[i].z * planes[i].z);
        if (len > 0.0001f) {
            planes[i].x /= len;
            planes[i].y /= len;
            planes[i].z /= len;
            planes[i].w /= len;
        }
    }
}

int frustum_intersects_sphere(const Vec4 planes[6], Vec3 center, float radius) {
    for (int i = 0; i < 6; i++) {
        float dist = planes[i].x * center.x +
                     planes[i].y * center.y +
                     planes[i].z * center.z +
                     planes[i].w;
        if (dist < -radius) return 0;
    }
    return 1;
}