#include "core/Frustum.h"

#include <math.h>

#include "core/Generic.h"

void initFrustum(Frustum* f, float fieldOfView, float nearClip, float farClip) {
    f->tan = tanf(fieldOfView * 0.5f);
    f->nearClip = nearClip;
    f->farClip = farClip;

    float diff = 1.0f / (nearClip - farClip);
    f->projection.data[0] = V(0.0f, 0.0f, 0.0f, 0.0f);
    f->projection.data[1] = V(0.0f, 1.0f / f->tan, 0.0f, 0.0f);
    f->projection.data[2] =
        V(0.0f, 0.0f, (nearClip + farClip) * diff,
          (2.0f * nearClip * farClip) * diff);
    f->projection.data[3] = V(0.0f, 0.0f, -1.0f, 0.0f);
}

const Matrix* updateProjection(Frustum* f, const IntVector2* size) {
    float x = (float)size->y / (f->tan * (float)size->x);
    f->projection.data[0] = V(x, 0.0f, 0.0f, 0.0f);
    return &f->projection;
}

void updateFrustumPlanes(
    Frustum* f, const Vector3* pos, const Vector3* right, const Vector3* up,
    const Vector3* front, const IntVector2* size) {
    float aspect = (float)size->x / (float)size->y;

    float hNearHeight = f->tan * f->nearClip;
    float hNearWidth = hNearHeight * aspect;

    float hFarHeight = f->tan * f->farClip;
    float hFarWidth = hFarHeight * aspect;

    Vector3 fCenter;
    add(&fCenter, pos, mul(front, f->farClip));
    Vector3 upFar;
    mul(&upFar, up, hFarHeight);
    Vector3 rightFar;
    mul(&rightFar, right, hFarWidth);

    Vector3 fTopLeft;
    add(&fTopLeft, &fCenter, sub(&upFar, &rightFar));
    Vector3 fTopRight;
    add(&fTopRight, &fCenter, add(&upFar, &rightFar));
    Vector3 fBottomRight;
    sub(&fBottomRight, &fCenter, sub(&upFar, &rightFar));

    Vector3 nCenter;
    add(&nCenter, pos, mul(front, f->nearClip));
    Vector3 upNear;
    mul(&upNear, up, hNearHeight);
    Vector3 rightNear;
    mul(&rightNear, right, hNearWidth);

    Vector3 nTopLeft;
    add(&nTopLeft, &nCenter, sub(&upNear, &rightNear));
    Vector3 nBottomLeft;
    sub(&nBottomLeft, &nCenter, add(&upNear, &rightNear));
    Vector3 nBottomRight;
    sub(&nBottomRight, &nCenter, sub(&upNear, &rightNear));

    // near plane
    initPlane(f->planes + 0, &nBottomRight, &nTopLeft, &nBottomLeft);
    // far plane
    initPlane(f->planes + 1, &fTopRight, &fBottomRight, &fTopLeft);
    // bottom plane
    initPlane(f->planes + 2, &nBottomRight, &nBottomLeft, &fBottomRight);
    // top plane
    initPlane(f->planes + 3, &fTopLeft, &nTopLeft, &fTopRight);
    // left plane
    initPlane(f->planes + 4, &nBottomLeft, &nTopLeft, &fTopLeft);
    // right plane
    initPlane(f->planes + 5, &fBottomRight, &fTopRight, &nBottomRight);
}

bool isInsideFrustum(const Frustum* f, const Vector3* pos) {
    for(size_t i = 0; i < ARRAY_LENGTH(f->planes); i++) {
        if(signedDistance(f->planes + i, pos) < 0.0f) {
            return false;
        }
    }
    return true;
}

bool isInsideFrustumRadius(const Frustum* f, const Vector3* pos, float radius) {
    for(size_t i = 0; i < ARRAY_LENGTH(f->planes); i++) {
        if(signedDistance(f->planes + i, pos) < -radius) {
            return false;
        }
    }
    return true;
}