#include <stdbool.h>
#include <spu_intrinsics.h>

// shuffle helpers
#define L0 0x00010203
#define L1 0x04050607
#define L2 0x08090a0b
#define L3 0x0c0d0e0f

#define R0 0x10111213
#define R1 0x14151617
#define R2 0x18191a1b
#define R3 0x1c1d1e1f

#define SHUFFLE(l, r, x, y, z, w) si_shufb(l, r, ((qword)(vec_uint4){x, y, z, w}))

// splat helper
#define SPLAT(v, idx) si_shufb(v, v, (qword)(vec_uint4)(L ## idx))

struct matrix43_t
{
    vec_float4 row0;
    vec_float4 row1;
    vec_float4 row2;
    vec_float4 row3;
};

struct aabb_t
{
    vec_float4 min;
    vec_float4 max;
};

struct frustum_t
{
    vec_float4 planes[6];
};

static inline void transform_points_4(qword* dest, qword x, qword y, qword z, const struct matrix43_t* mat)
{
#define COMP(c) \
    qword res_ ## c = SPLAT((qword)mat->row3, c); \
    res_ ## c = si_fma(z, SPLAT((qword)mat->row2, c), res_ ## c); \
    res_ ## c = si_fma(y, SPLAT((qword)mat->row1, c), res_ ## c); \
    res_ ## c = si_fma(x, SPLAT((qword)mat->row0, c), res_ ## c); \
    dest[c] = res_ ## c;

    COMP(0);
    COMP(1);
    COMP(2);

#undef COMP
}

static inline qword dot4(qword v, qword x, qword y, qword z)
{
    qword result = SPLAT(v, 3);

    result = si_fma(SPLAT(v, 2), z, result);
    result = si_fma(SPLAT(v, 1), y, result);
    result = si_fma(SPLAT(v, 0), x, result);

    return result;
}

__attribute__((noinline)) bool is_visible(const struct matrix43_t* transform, const struct aabb_t* aabb, const struct frustum_t* frustum)
{
    qword min = (qword)aabb->min;
    qword max = (qword)aabb->max;

    // get aabb points (SoA)
    qword minmax_x = SHUFFLE(min, max, L0, R0, L0, R0); // x X x X
    qword minmax_y = SHUFFLE(min, max, L1, L1, R1, R1); // y y Y Y
    qword minmax_z_0 = SPLAT(min, 2); // z z z z
    qword minmax_z_1 = SPLAT(max, 2); // Z Z Z Z

    // transform points to world space
    qword points_ws_0[3];
    qword points_ws_1[3];

    transform_points_4(points_ws_0, minmax_x, minmax_y, minmax_z_0, transform);
    transform_points_4(points_ws_1, minmax_x, minmax_y, minmax_z_1, transform);

    // for each plane...
    for (int i = 0; i < 6; ++i)
    {
        qword plane = (qword)frustum->planes[i];

        // calculate 8 dot products
        qword dp0 = dot4(plane, points_ws_0[0], points_ws_0[1], points_ws_0[2]);
        qword dp1 = dot4(plane, points_ws_1[0], points_ws_1[1], points_ws_1[2]);

        // get signs
        qword dp0neg = si_fcgt((qword)(0), dp0);
        qword dp1neg = si_fcgt((qword)(0), dp1);

        if (si_to_uint(si_gb(si_and(dp0neg, dp1neg))) == 15)
        {
            return false;
        }
    }

    return true;
}

// simple ortho frustum
struct frustum_t frustum =
{
    {
        { 1, 0, 0, 10 },
        { -1, 0, 0, 10 },
        { 0, 1, 0, 10 },
        { 0, -1, 0, 10 },
        { 0, 0, 1, 10 },
        { 0, 0, -1, 10 }
    }
};

// small box
struct aabb_t aabb =
{
    { -1, -2, -3 },
    { 1, 2, 3 }
};

// and some weird matrix
struct matrix43_t transform =
{
    { 0.123f, 0.456f, 0.789f },
    { 0.456f, 0.123f, 0.789f },
    { 0.789f, 0.123f, 0.456f },
    { 1.f, -1.f, 1.f }
};

void _start()
{
    is_visible(&transform, &aabb, &frustum);
    si_stop(0);
}