#include "math/Frustum.h"

Frustum::Frustum(float fieldOfView, float nearClip, float farClip, const Size& size)
    : size(size), fieldOfView(fieldOfView), nearClip(nearClip), farClip(farClip) {
}

Matrix& Frustum::updateProjection() {
    float tan = tanf(fieldOfView * (0.5f * M_PI / 180.0f));
    float q = 1.0f / tan;
    float aspect = static_cast<float>(size.width) / size.height;
    float diff = 1.0f / (nearClip - farClip);

    projection.set(0, Vector4(q / aspect, 0.0f, 0.0f, 0.0f));
    projection.set(1, Vector4(0.0f, q, 0.0f, 0.0f));
    projection.set(2, Vector4(0.0f, 0.0f, (nearClip + farClip) * diff, (2.0f * nearClip * farClip) * diff));
    projection.set(3, Vector4(0.0f, 0.0f, -1.0f, 0.0f));
    return projection;
}

void Frustum::updatePlanes(const Vector3& pos, const Vector3& right, const Vector3& up, const Vector3& front) {
    float tan = tanf(fieldOfView * (0.5f * M_PI / 180.0f));
    float aspect = static_cast<float>(size.width) / size.height;

    float halfNearHeight = tan * nearClip;
    float halfNearWidth = halfNearHeight * aspect;

    float halfFarHeight = tan * farClip;
    float halfFarWidth = halfFarHeight * aspect;

    Vector3 farCenter = pos + front * farClip;
    Vector3 farTopLeft = farCenter + (up * halfFarHeight) - (right * halfFarWidth);
    Vector3 farTopRight = farCenter + (up * halfFarHeight) + (right * halfFarWidth);
    Vector3 farBottomRight = farCenter - (up * halfFarHeight) + (right * halfFarWidth);

    Vector3 nearCenter = pos + front * nearClip;
    Vector3 nearTopLeft = nearCenter + (up * halfNearHeight) - (right * halfNearWidth);
    Vector3 nearBottomLeft = nearCenter - (up * halfNearHeight) - (right * halfNearWidth);
    Vector3 nearBottomRight = nearCenter - (up * halfNearHeight) + (right * halfNearWidth);

    planes[0] = Plane(nearBottomRight, nearTopLeft, nearBottomLeft);    // near plane
    planes[1] = Plane(farTopRight, farBottomRight, farTopLeft);         // far plane
    planes[2] = Plane(nearBottomRight, nearBottomLeft, farBottomRight); // bottom plane
    planes[3] = Plane(farTopLeft, nearTopLeft, farTopRight);            // top plane
    planes[4] = Plane(nearBottomLeft, nearTopLeft, farTopLeft);         // left plane
    planes[5] = Plane(farBottomRight, farTopRight, nearBottomRight);    // right plane
}

bool Frustum::isInside(const Vector3& pos) const {
    for(const Plane& p : planes) {
        if(p.getSignedDistance(pos) < 0.0f) {
            return false;
        }
    }
    return true;
}

bool Frustum::isInside(const Vector3& pos, float radius) const {
    for(const Plane& p : planes) {
        if(p.getSignedDistance(pos) < -radius) {
            return false;
        }
    }
    return true;
}