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

struct vector3_t
{
    float x, y, z;
};

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

struct aabb_t
{
    struct vector3_t min;
    struct vector3_t max;
};

struct plane_t
{
    float x, y, z, w;
};

struct frustum_t
{
    struct plane_t planes[6];
};

inline void transform_point(struct vector3_t* p, const struct matrix43_t* mat)
{
    struct vector3_t op = *p;

#define COMP(c) p->c = op.x * mat->row0.c + op.y * mat->row1.c + op.z * mat->row2.c + mat->row3.c

    COMP(x); COMP(y); COMP(z);

#undef COMP
}

inline float dot(const struct vector3_t* v, const struct plane_t* p)
{
    return v->x * p->x + v->y * p->y + v->z * p->z + p->w;
}

__attribute__((noinline)) bool is_visible(struct matrix43_t* transform, struct aabb_t* aabb, struct frustum_t* frustum)
{
    // get aabb points
    struct vector3_t points[] =
    {
        { aabb->min.x, aabb->min.y, aabb->min.z },
        { aabb->max.x, aabb->min.y, aabb->min.z },
        { aabb->max.x, aabb->max.y, aabb->min.z },
        { aabb->min.x, aabb->max.y, aabb->min.z },

        { aabb->min.x, aabb->min.y, aabb->max.z },
        { aabb->max.x, aabb->min.y, aabb->max.z },
        { aabb->max.x, aabb->max.y, aabb->max.z },
        { aabb->min.x, aabb->max.y, aabb->max.z }
    };

    // transform points to world space
    for (int i = 0; i < 8; ++i)
    {
        transform_point(points + i, transform);
    }

    // for each plane...
    for (int i = 0; i < 6; ++i)
    {
        bool inside = false;

        for (int j = 0; j < 8; ++j)
        {
            if (dot(points + j, frustum->planes + i) > 0)
            {
                inside = true;
                break;
            }
        }

        if (!inside)
        {
            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, -1, 1 }
};

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