#include "common.h"
#include "frustum.h"
#include "display.h"

#include <inline_c.h>

static SVECTOR *left, *right, *top, *bottom, *front;

static INLINE void frustum_buildPlane(MATRIX *rot_matrix, VECTOR *normal, SVECTOR *outPlane)
{
    ApplyMatrixLV(rot_matrix, normal, normal);
    VectorNormalS(normal, outPlane);
    outPlane->pad = -m_dot12(&cam_pos, outPlane);
}

void frustum_update(int width, int height)
{
    VECTOR l, r, t, b, f;

    int near;
    gte_ReadGeomScreen(&near);
    
    // FntPrint(-1, "N = %d, W = %d, H = %d\n", near, width, height);

    // 2 * ONE * near
    near <<= 13;

    l.vx = near / width;
    l.vy = ONE;
    l.vz = 0;
    
    b.vx = 0;
    b.vy = ONE;
    b.vz = near / height;
    
    r.vx = -l.vx;
    r.vy = ONE;
    r.vz = 0;
    
    t.vx = 0;
    t.vy = ONE;
    t.vz = -b.vz;

    f.vx = 0;
    f.vy = ONE;
    f.vz = 0;

    MATRIX rot_matrix;
    RotMatrixQ(&cam_rot, &rot_matrix);

    // Place the frustum planes at the start of the fast RAM buffer
    left =      (SVECTOR*)(scratchpad + sizeof(SVECTOR) * 0);
    right =     (SVECTOR*)(scratchpad + sizeof(SVECTOR) * 1);
    top =       (SVECTOR*)(scratchpad + sizeof(SVECTOR) * 2);
    bottom =    (SVECTOR*)(scratchpad + sizeof(SVECTOR) * 3);
    front =     (SVECTOR*)(scratchpad + sizeof(SVECTOR) * 4);

    frustum_buildPlane(&rot_matrix, &l, left);
    frustum_buildPlane(&rot_matrix, &r, right);
    frustum_buildPlane(&rot_matrix, &t, top);
    frustum_buildPlane(&rot_matrix, &b, bottom);
    frustum_buildPlane(&rot_matrix, &f, front);
    front->pad -= near >> 1;

    // FntPrint(-1, "l = %d %d, r = %d %d, t = %d %d, b = %d %d\n", 
    //     left.vx, left.pad, right.vx, right.pad, top.vz, top.pad, bottom.vz, bottom.pad);
}

static INLINE u_char frustum_pointInside_(const SVECTOR p)
{
    if (m_dot12(left, &p) + left->pad < 0) return 0;
    if (m_dot12(right, &p) + right->pad < 0) return 0;
    if (m_dot12(top, &p) + top->pad < 0) return 0;
    if (m_dot12(bottom, &p) + bottom->pad < 0) return 0;
    return 1;
}

u_char frustum_pointInside(const SVECTOR *point)
{
    if (m_dot12(left, point) + left->pad < 0) return 0;
    if (m_dot12(right, point) + right->pad < 0) return 0;
    if (m_dot12(top, point) + top->pad < 0) return 0;
    if (m_dot12(bottom, point) + bottom->pad < 0) return 0;
    return 1;
}

u_char frustum_sphereInside(const SVECTOR *sphere)
{
    short radius = -sphere->pad;
    if (m_dot12(left, sphere) + left->pad < radius) return 0;
    if (m_dot12(right, sphere) + right->pad < radius) return 0;
    if (m_dot12(top, sphere) + top->pad < radius) return 0;
    if (m_dot12(bottom, sphere) + bottom->pad < radius) return 0;
    return 1;
}

u_char frustum_boxInside(const SVECTOR *min, const SVECTOR *max)
{
    if (frustum_pointInside_((SVECTOR) { min->vx, min->vy, min->vz })) return 1;
    if (frustum_pointInside_((SVECTOR) { min->vx, min->vy, max->vz })) return 1;
    if (frustum_pointInside_((SVECTOR) { min->vx, max->vy, min->vz })) return 1;
    if (frustum_pointInside_((SVECTOR) { min->vx, max->vy, max->vz })) return 1;
    if (frustum_pointInside_((SVECTOR) { max->vx, min->vy, min->vz })) return 1;
    if (frustum_pointInside_((SVECTOR) { max->vx, min->vy, max->vz })) return 1;
    if (frustum_pointInside_((SVECTOR) { max->vx, max->vy, min->vz })) return 1;
    if (frustum_pointInside_((SVECTOR) { max->vx, max->vy, max->vz })) return 1;
    return 0;
}

int frustum_pointDepth(const SVECTOR *point)
{
    return m_dot12(front, point) + front->pad;
}