#include "types.h"

#include "maths.h"
#include "maths3D.h"

#include "vdp.h"
#include "bitmap.h"


#define MATOBJ      1
#define MATVIEW     2
#define MATPROJ     4
#define MATTRANS    8


vect3D_f16 pts_3D[MAT3D_MAXPOINTS];
vect2D_s16 pts_2D[MAT3D_MAXPOINTS];
//u16 face_index[MAT3D_MAXFACE];

static vect3D_f16 light;

vect3D_f16 light_trans;
vect3D_f16 camview_trans;

static trans3D_f16 Trans;
static mat3D_f16 Mat;
static mat3D_f16 MatInv;

static fix16 CamDist;

static u16 RebuildMat;


void reset3D()
{
    CamDist = FIX16(30);

    light.x = FIX16(1);
    light.y = FIX16(0);
    light.z = FIX16(0);

    resetMat3D();
}


void setCamDist3D(fix16 value)
{
    CamDist = FIX16(value);
}

void setLight3D(vect3D_f16 *value)
{
    light.x = value->x;
    light.y = value->y;
    light.z = value->z;
}


void resetMat3D()
{
    Trans.Tx = 0;
    Trans.Ty = 0;
    Trans.Tz = 0;
    Trans.Rx = 0;
    Trans.Ry = 0;
    Trans.Rz = 0;

    RebuildMat = 1;
}


void setTXMat3D(fix16 tx)
{
    Trans.Tx = tx;

    RebuildMat = 1;
}

void setTYMat3D(fix16 ty)
{
    Trans.Ty = ty;

    RebuildMat = 1;
}

void setTZMat3D(fix16 tz)
{
    Trans.Tz = tz;

    RebuildMat = 1;
}

void setTXYZMat3D(fix16 tx, fix16 ty, fix16 tz)
{
    Trans.Tx = tx;
    Trans.Ty = ty;
    Trans.Tz = tz;

    RebuildMat = 1;
}

void setTransMat3D(vect3D_f16 *trans)
{
    Trans.Tx = trans->x;
    Trans.Ty = trans->y;
    Trans.Tz = trans->z;

    RebuildMat = 1;
}


void setRXMat3D(fix16 rx)
{
    Trans.Rx = rx;

    RebuildMat = 1;
}

void setRYMat3D(fix16 ry)
{
    Trans.Ry = ry;

    RebuildMat = 1;
}

void setRZMat3D(fix16 rz)
{
    Trans.Rz = rz;

    RebuildMat = 1;
}

void setRXYZMat3D(fix16 rx, fix16 ry, fix16 rz)
{
    Trans.Rx = rx;
    Trans.Ry = ry;
    Trans.Rz = rz;

    RebuildMat = 1;
}

void setRotMat3D(vect3D_f16 *rot)
{
    Trans.Rx = rot->x;
    Trans.Ry = rot->y;
    Trans.Rz = rot->z;

    RebuildMat = 1;
}


static void calculateMat3D()
{
    fix16 sx, sy, sz;
    fix16 cx, cy, cz;
    fix16 sxsy, cxsy;

    sx = fix16Sin(fix16ToInt(Trans.Rx));
    sy = fix16Sin(fix16ToInt(Trans.Ry));
    sz = fix16Sin(fix16ToInt(Trans.Rz));
    cx = fix16Cos(fix16ToInt(Trans.Rx));
    cy = fix16Cos(fix16ToInt(Trans.Ry));
    cz = fix16Cos(fix16ToInt(Trans.Rz));

    sxsy = fix16Mul(sx, sy);
    cxsy = fix16Mul(cx, sy);

    Mat.a.x = fix16Mul(cy, cz);
    Mat.b.x = -fix16Mul(cy, sz);
    Mat.c.x = sy;

    Mat.a.y = fix16Mul(sxsy, cz) + fix16Mul(cx, sz);
    Mat.b.y = fix16Mul(cx, cz) - fix16Mul(sxsy, sz);
    Mat.c.y = -fix16Mul(sx, cy);

    Mat.a.z = fix16Mul(sx, sz) - fix16Mul(cxsy, cz);
    Mat.b.z = fix16Mul(cxsy, sz) + fix16Mul(sx, cz);
    Mat.c.z = fix16Mul(cx, cy);

    MatInv.a.x = Mat.a.x;
    MatInv.b.x = Mat.a.y;
    MatInv.c.x = Mat.a.z;

    MatInv.a.y = Mat.b.x;
    MatInv.b.y = Mat.b.y;
    MatInv.c.y = Mat.b.z;

    MatInv.a.z = Mat.c.x;
    MatInv.b.z = Mat.c.y;
    MatInv.c.z = Mat.c.z;

    RebuildMat = 0;
}


void transform3D(const vect3D_f16 *src, vect3D_f16 *dest, u16 numv)
{
    const vect3D_f16 *s;
    vect3D_f16 *d;
    u16 i;

    if (RebuildMat) calculateMat3D();

    s = src;
    d = dest;
    i = numv;
    while (i--)
    {
        d->x = fix16Mul(s->x, Mat.a.x) + fix16Mul(s->y, Mat.a.y) + fix16Mul(s->z, Mat.a.z) + Trans.Tx;
        d->y = fix16Mul(s->x, Mat.b.x) + fix16Mul(s->y, Mat.b.y) + fix16Mul(s->z, Mat.b.z) + Trans.Ty;
        d->z = fix16Mul(s->x, Mat.c.x) + fix16Mul(s->y, Mat.c.y) + fix16Mul(s->z, Mat.c.z) + Trans.Tz;

        s++;
        d++;
    }

    // transform light vector
    light_trans.x = fix16Mul(light.x, MatInv.a.x) + fix16Mul(light.y, MatInv.a.y) + fix16Mul(light.z, MatInv.a.z);
    light_trans.y = fix16Mul(light.x, MatInv.b.x) + fix16Mul(light.y, MatInv.b.y) + fix16Mul(light.z, MatInv.b.z);
    light_trans.z = fix16Mul(light.x, MatInv.c.x) + fix16Mul(light.y, MatInv.c.y) + fix16Mul(light.z, MatInv.c.z);

    // transform camview vector (0, 0, 1)
    camview_trans.x = fix16Mul(FIX16(1), MatInv.a.z);
    camview_trans.y = fix16Mul(FIX16(1), MatInv.b.z);
    camview_trans.z = fix16Mul(FIX16(1), MatInv.c.z);
}

void project3DFixed(const vect3D_f16 *src, vect2D_f16 *dest, u16 numv)
{
    const vect3D_f16 *s;
    vect2D_f16 *d;
    fix16 zi;
    fix16 wi, hi;
    u16 i;

    wi = intToFix16(bitmap_width_pixel / 2);
    hi = intToFix16(bitmap_height_pixel / 2);
    s = src;
    d = dest;
    i = numv;
    while (i--)
    {
        if (s->z >> 3)
        {
            zi = fix16Div(CamDist, s->z >> 3);
            d->x = wi + fix16Mul(s->x / 2, zi);
            d->y = hi - fix16Mul(s->y, zi);
        }
        else
        {
            d->x = FIX16(-1);
            d->y = FIX16(-1);
        }

        s++;
        d++;
    }
}

void project3DInt(const vect3D_f16 *src, vect2D_s16 *dest, u16 numv)
{
    const vect3D_f16 *s;
    vect2D_s16 *d;
    fix16 zi;
    u16 wi, hi;
    u16 i;

    wi = bitmap_width_pixel / 2;
    hi = bitmap_height_pixel / 2;
    s = src;
    d = dest;
    i = numv;
    while (i--)
    {
        if (s->z)
        {
            zi = fix16Div(CamDist, s->z >> 3);
            d->x = wi + fix16ToInt(fix16Mul(s->x / 2, zi));
            d->y = hi - fix16ToInt(fix16Mul(s->y, zi));
        }
        else
        {
            d->x = -1;
            d->y = -1;
        }

        s++;
        d++;
    }
}
