#include "core/math/Frustum.hpp"

Core::Frustum::Frustum(float fieldOfView, float nearClip_, float farClip_)
    : projection(), planes(),
      tan(tanf(Core::Math::degreeToRadian(fieldOfView) * 0.5f)),
      nearClip(nearClip_), farClip(farClip_) {
    float diff = 1.0f / (nearClip - farClip);
    projection.set(1, Vector4(0.0f, 1.0f / tan, 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));
}

const Core::Matrix& Core::Frustum::updateProjection(const IntVector2& size) {
    projection.set(0, Vector4(static_cast<float>(size[1]) /
                                  (tan * static_cast<float>(size[0])),
                              0.0f, 0.0f, 0.0f));
    return projection;
}

void Core::Frustum::updatePlanes(const Vector3& pos, const Vector3& right,
                                 const Vector3& up, const Vector3& front,
                                 const IntVector2& size) {
    float aspect = static_cast<float>(size[0]) / static_cast<float>(size[1]);

    float hNearHeight = tan * nearClip;
    float hNearWidth = hNearHeight * aspect;

    float hFarHeight = tan * farClip;
    float hFarWidth = hFarHeight * aspect;

    Vector3 fCenter = pos + front * farClip;
    Vector3 fTopLeft = fCenter + (up * hFarHeight) - (right * hFarWidth);
    Vector3 fTopRight = fCenter + (up * hFarHeight) + (right * hFarWidth);
    Vector3 fBottomRight = fCenter - (up * hFarHeight) + (right * hFarWidth);

    Vector3 nCenter = pos + front * nearClip;
    Vector3 nTopLeft = nCenter + (up * hNearHeight) - (right * hNearWidth);
    Vector3 nBottomLeft = nCenter - (up * hNearHeight) - (right * hNearWidth);
    Vector3 nBottomRight = nCenter - (up * hNearHeight) + (right * hNearWidth);

    planes[0] = Plane(nBottomRight, nTopLeft, nBottomLeft);     // n plane
    planes[1] = Plane(fTopRight, fBottomRight, fTopLeft);       // f plane
    planes[2] = Plane(nBottomRight, nBottomLeft, fBottomRight); // bottom plane
    planes[3] = Plane(fTopLeft, nTopLeft, fTopRight);           // top plane
    planes[4] = Plane(nBottomLeft, nTopLeft, fTopLeft);         // left plane
    planes[5] = Plane(fBottomRight, fTopRight, nBottomRight);   // right plane
}

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

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