#include "math/Frustum.h"

Frustum::Frustum(float fieldOfView_, float nearClip_, float farClip_)
    : fieldOfView(fieldOfView_), nearClip(nearClip_), farClip(farClip_) {
}

const Matrix& Frustum::updateProjection(const IntVector2& size) {
    float tan = tanf(fieldOfView * (0.5f * static_cast<float>(M_PI) / 180.0f));
    float q = 1.0f / tan;
    float aspect = static_cast<float>(size[0]) / static_cast<float>(size[1]);
    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,
                           const IntVector2& size) {
    float tan = tanf(fieldOfView * (0.5f * static_cast<float>(M_PI) / 180.0f));
    float aspect = static_cast<float>(size[0]) / static_cast<float>(size[1]);

    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;
}