#define TRS

#include "graf.h"

#define TRS_MAX_MATRIX_HEAP 32

int TRS_HeapPosition, TRS_MatrixMode;

#define TRS_IMatrix TRS_IMatrixHeap[TRS_HeapPosition]
#define TRS_VMatrix TRS_VMatrixHeap[TRS_HeapPosition]

TRS_MATRIX TRS_IMatrixHeap[TRS_MAX_MATRIX_HEAP];
TRS_MATRIX TRS_VMatrixHeap[TRS_MAX_MATRIX_HEAP];

TRS_MATRIX TRS_PMatrix;
TRS_MATRIX TRS_GMatrix;

TRS_MATRIX TRS_IdentMatrix =
{
    1.0, 0.0, 0.0, 0.0,
    0.0, 1.0, 0.0, 0.0,
    0.0, 0.0, 1.0, 0.0,
    0.0, 0.0, 0.0, 1.0
};

FLOAT TRS_ViewpointX;
FLOAT TRS_ViewpointY;
FLOAT TRS_ViewpointZ;
FLOAT TRS_ViewpointW;

FLOAT TRS_ViewportXL;
FLOAT TRS_ViewportXH;
FLOAT TRS_ViewportYL;
FLOAT TRS_ViewportYH;

FLOAT TRS_PiDiv180;

extern int ZBF_NX, ZBF_NY;
extern int ZBF_VX_L,ZBF_VX_H;
extern int ZBF_VY_L,ZBF_VY_H;

void trs_matrix_mode( int type )
{
    TRS_MatrixMode = type;
}

void trs_get_matrix( TRS_MATRIX M )
{
    int i, j;

    switch( TRS_MatrixMode ) {

    case TRS_MATRIX_VIEWING:

        for( i = 0; i < 4; i++ )
            for( j = 0; j < 4; j++ ) M[i][j] = TRS_VMatrix[i][j];

    break;

    case TRS_MATRIX_PROJECTION:

        for( i = 0; i < 4; i++ )
            for( j = 0; j < 4; j++ ) M[i][j] = TRS_PMatrix[i][j];

    break;
    }
}

void trs_load_matrix( TRS_MATRIX M )
{
    int i, j;

    switch( TRS_MatrixMode ) {

    case TRS_MATRIX_PROJECTION:

         memcpy( TRS_PMatrix, M, sizeof( TRS_MATRIX ) );

    break;

    case TRS_MATRIX_VIEWING:

         if ( M != TRS_VMatrix )
         {
             memcpy( TRS_VMatrix, M, sizeof( TRS_MATRIX ) );
         }

         TRS_IMatrix[0][0]  = TRS_VMatrix[1][1]*TRS_VMatrix[2][2];
         TRS_IMatrix[0][0] -= TRS_VMatrix[2][1]*TRS_VMatrix[1][2];

         TRS_IMatrix[0][1]  = TRS_VMatrix[2][0]*TRS_VMatrix[1][2];
         TRS_IMatrix[0][1] -= TRS_VMatrix[1][0]*TRS_VMatrix[2][2];

         TRS_IMatrix[0][2]  = TRS_VMatrix[1][0]*TRS_VMatrix[2][1];
         TRS_IMatrix[0][2] -= TRS_VMatrix[2][0]*TRS_VMatrix[1][1];

         TRS_IMatrix[1][0]  = TRS_VMatrix[2][1]*TRS_VMatrix[0][2];
         TRS_IMatrix[1][0] -= TRS_VMatrix[0][1]*TRS_VMatrix[2][2];

         TRS_IMatrix[1][1]  = TRS_VMatrix[0][0]*TRS_VMatrix[2][2];
         TRS_IMatrix[1][1] -= TRS_VMatrix[2][0]*TRS_VMatrix[0][2];

         TRS_IMatrix[1][2]  = TRS_VMatrix[2][0]*TRS_VMatrix[0][1];
         TRS_IMatrix[1][2] -= TRS_VMatrix[0][0]*TRS_VMatrix[2][1];

         TRS_IMatrix[2][0]  = TRS_VMatrix[0][1]*TRS_VMatrix[1][2];
         TRS_IMatrix[2][0] -= TRS_VMatrix[1][1]*TRS_VMatrix[0][2];

         TRS_IMatrix[2][1]  = TRS_VMatrix[1][0]*TRS_VMatrix[0][2];
         TRS_IMatrix[2][1] -= TRS_VMatrix[0][0]*TRS_VMatrix[1][2];

         TRS_IMatrix[2][2]  = TRS_VMatrix[0][0]*TRS_VMatrix[1][1];
         TRS_IMatrix[2][2] -= TRS_VMatrix[1][0]*TRS_VMatrix[0][1];

    break;
    }
}

void trs_mult_matrix( TRS_MATRIX M )
{
     TRS_MATRIX N;

     double s;
     int i,j,k;

     for( i=0; i<4; i++ )
     for( j=0; j<4; j++ )
     {
         s = 0.0;
         for( k=0; k<4; k++ ) s += M[i][k]*TRS_VMatrix[k][j];
         N[i][j] = s;
     }

     trs_load_matrix( N );
}

void trs_mult_matrix_right( TRS_MATRIX M )
{
     TRS_MATRIX N;

     double s;
     int i,j,k;

     for( i=0; i<4; i++ )
     for( j=0; j<4; j++ )
     {
         s = 0.0;
         for( k=0; k<4; k++ ) s += TRS_VMatrix[i][k]*M[k][j];
         N[i][j] = s;
     }

     trs_load_matrix( N );
}

void trs_push_matrix()
{
    TRS_MATRIX *V, *I;

    if ( TRS_HeapPosition >= TRS_MAX_MATRIX_HEAP-1 ) {
        fprintf( stderr, "trs_push_matrix: too many matrix pushes.\n" );
        return;
        }

     V = &TRS_VMatrix;
     I = &TRS_IMatrix;

     TRS_HeapPosition++;

     memcpy( TRS_VMatrix, V, sizeof(TRS_MATRIX) );
     memcpy( TRS_IMatrix, I, sizeof(TRS_MATRIX) );
}


void trs_pop_matrix()
{
    if ( TRS_HeapPosition )
    {
        TRS_HeapPosition--;
    }  else {
        fprintf( stderr, "trs_pop_matrix: too many matrix pops." );
    }
}

void trs_init_transformations( )
{
    TRS_PiDiv180 = acos(0.0) / 90.0;

    TRS_HeapPosition = 0;
    trs_ortho( 0.0, 1.0, 0.0, 1.0, 0.0, 1.0 );
    trs_matrix_mode( TRS_MATRIX_VIEWING );
    trs_load_matrix( TRS_IdentMatrix );
}

void trs_translate( FLOAT x, FLOAT y, FLOAT z )
{
     int i;

     for( i = 0; i < 3; i++ )
     {
         TRS_VMatrix[3][i] += x*TRS_VMatrix[0][i];
         TRS_VMatrix[3][i] += y*TRS_VMatrix[1][i];
         TRS_VMatrix[3][i] += z*TRS_VMatrix[2][i];
     }

     trs_load_matrix( TRS_VMatrix );
}

void trs_scale( FLOAT x, FLOAT y, FLOAT z )
{
     int i;

     for( i = 0; i < 4; i++ )
     {
         TRS_VMatrix[0][i] *= x;
         TRS_VMatrix[1][i] *= y;
         TRS_VMatrix[2][i] *= z;
     }

     trs_load_matrix( TRS_VMatrix );
}


trs_internal_rotate( FLOAT s, FLOAT c, int axis )
{
     FLOAT t;

     int i;

     switch( axis ) {
     case 'x':
         for( i = 0; i < 4; i++ ) {
             t = TRS_VMatrix[1][i];
             TRS_VMatrix[1][i] = c*t + s*TRS_VMatrix[2][i];
             TRS_VMatrix[2][i] = c*TRS_VMatrix[2][i] - s*t;
             }
     break;
 
     case 'y':
         for( i = 0; i < 4; i++ ) {
             t = TRS_VMatrix[0][i];
             TRS_VMatrix[0][i] = c*t - s*TRS_VMatrix[2][i];
             TRS_VMatrix[2][i] = c*TRS_VMatrix[2][i] + s*t;
             }
     break;

     case 'z':
         for( i = 0; i < 4; i++ ) {
             t = TRS_VMatrix[0][i];
             TRS_VMatrix[0][i] = c*t + s*TRS_VMatrix[1][i];
             TRS_VMatrix[1][i] = c*TRS_VMatrix[1][i] - s*t;
             }
     break;
     }
 
     trs_load_matrix( TRS_VMatrix );
}

void trs_rotate( FLOAT a, int axis )
{
     a *= TRS_PiDiv180; 
     trs_internal_rotate( sin(a), cos(a), axis );
}

void trs_lookat( FLOAT vx, FLOAT vy, FLOAT vz,
                  FLOAT px, FLOAT py, FLOAT pz, FLOAT twist )
{
     FLOAT s, c, l, t;

     l = sqrt((px-vx)*(px-vx)+(pz-vz)*(pz-vz)); 
     t = 1.0/sqrt((px-vx)*(px-vx)+(py-vy)*(py-vy)+(pz-vz)*(pz-vz)); 

     trs_rotate( -twist, 'z' );

     s = t * (vy - py);
     c = t * l;
     trs_internal_rotate( s, c, 'x' );

     s = (px-vx) / l;
     c = (vz-pz) / l;
     trs_internal_rotate( s, c, 'y' );

     trs_translate( -vx, -vy, -vz );

     TRS_ViewpointX = vx;
     TRS_ViewpointY = vy;
     TRS_ViewpointZ = vz;
     TRS_ViewpointW = 1.0;
}

void trs_polarview( FLOAT dist, FLOAT azim, FLOAT inc, FLOAT twist )
{
     trs_translate( 0.0, 0.0, -dist );
     trs_rotate( -twist, 'z' );
     trs_rotate( -inc,   'x' );
     trs_rotate( -azim,  'z' );
}

void trs_perspective( FLOAT fov, FLOAT aspect, FLOAT near, FLOAT far )
{
    FLOAT s, t;

    fov *= TRS_PiDiv180;

    s = 1.0 / tan(fov / 2.0);
    t = 1.0 / (far - near);

    TRS_PMatrix[0][0] =  s / aspect;
    TRS_PMatrix[0][1] =  0;
    TRS_PMatrix[0][2] =  0;
    TRS_PMatrix[0][3] =  0;
    TRS_PMatrix[1][0] =  0;
    TRS_PMatrix[1][1] =  s;
    TRS_PMatrix[1][2] =  0;
    TRS_PMatrix[1][3] =  0;
    TRS_PMatrix[2][0] =  0;
    TRS_PMatrix[2][1] =  0;
    TRS_PMatrix[2][2] = -t*(far + near);
    TRS_PMatrix[2][3] = -1;
    TRS_PMatrix[3][0] =  0;
    TRS_PMatrix[3][1] =  0;
    TRS_PMatrix[3][2] = -2*t*far*near;
    TRS_PMatrix[3][3] =  0;
}

void trs_ortho( FLOAT x0, FLOAT x1, FLOAT y0, FLOAT y1, FLOAT z0, FLOAT z1 )
{
    FLOAT x, y, z;

    x = 1.0 / (x1 - x0);
    y = 1.0 / (y1 - y0);
    z = 1.0 / (z1 - z0);

    TRS_PMatrix[0][0] =  2*x;
    TRS_PMatrix[0][1] =  0;
    TRS_PMatrix[0][2] =  0;
    TRS_PMatrix[0][3] =  0;
    TRS_PMatrix[1][0] =  0;
    TRS_PMatrix[1][1] =  2*y;
    TRS_PMatrix[1][2] =  0;
    TRS_PMatrix[1][3] =  0;
    TRS_PMatrix[2][0] =  0;
    TRS_PMatrix[2][1] =  0;
    TRS_PMatrix[2][2] = -2*z;
    TRS_PMatrix[2][3] =  0;
    TRS_PMatrix[3][0] = -x*(x0 + x1);
    TRS_PMatrix[3][1] = -y*(y0 + y1);
    TRS_PMatrix[3][2] = -z*(z0 + z1);
    TRS_PMatrix[3][3] =  1;
}

void trs_normal_transform( int n, ZBF_VERTEX *P )
{
    FLOAT u, v, w, r;

    int i;

    for( i=0; i<n; i++, P++ )
    {
        u = P->NormalX;
        v = P->NormalY;
        w = P->NormalZ;

        P->NormalX=TRS_IMatrix[0][0]*u+TRS_IMatrix[1][0]*v+TRS_IMatrix[2][0]*w;
        P->NormalY=TRS_IMatrix[0][1]*u+TRS_IMatrix[1][1]*v+TRS_IMatrix[2][1]*w;
        P->NormalZ=TRS_IMatrix[0][2]*u+TRS_IMatrix[1][2]*v+TRS_IMatrix[2][2]*w;

        normalize( P->NormalX, P->NormalY, P->NormalZ );
    }
}

void trs_viewing_transform( int n, ZBF_VERTEX *P )
{
    FLOAT x, y, z;

    int i;

    for( i = 0; i < n; i++, P++ )
    {
        x = P->CoordX;
        y = P->CoordY;
        z = P->CoordZ;

        P->CoordX = TRS_VMatrix[0][0] * x + TRS_VMatrix[1][0] * y +
                    TRS_VMatrix[2][0] * z + TRS_VMatrix[3][0];

        P->CoordY = TRS_VMatrix[0][1] * x + TRS_VMatrix[1][1] * y +
                    TRS_VMatrix[2][1] * z + TRS_VMatrix[3][1];

        P->CoordZ = TRS_VMatrix[0][2] * x + TRS_VMatrix[1][2] * y +
                    TRS_VMatrix[2][2] * z + TRS_VMatrix[3][2];
    }
}

void trs_projection_transform( int n, ZBF_VERTEX *P )
{
    FLOAT x, y, z, w;

    int i;

    for( i=0; i<n; i++, P++ )
    {
        x = P->CoordX;
        y = P->CoordY;
        z = P->CoordZ;

        P->CoordW = TRS_PMatrix[0][3] * x + TRS_PMatrix[1][3] * y +
                    TRS_PMatrix[2][3] * z + TRS_PMatrix[3][3];

        P->CoordX = TRS_PMatrix[0][0] * x + TRS_PMatrix[1][0] * y +
                    TRS_PMatrix[2][0] * z + TRS_PMatrix[3][0];

        P->CoordY = TRS_PMatrix[0][1] * x + TRS_PMatrix[1][1] * y +
                    TRS_PMatrix[2][1] * z + TRS_PMatrix[3][1];

        P->CoordZ = TRS_PMatrix[0][2] * x + TRS_PMatrix[1][2] * y +
                    TRS_PMatrix[2][2] * z + TRS_PMatrix[3][2];
    }
}

void trs_scale_transform( int n, ZBF_VERTEX *P )
{
    int i;

    FLOAT Sx,Sy,Tx,Ty,w;

    Sx = (ZBF_VX_H - ZBF_VX_L)/2;
    Tx = (ZBF_VX_H + ZBF_VX_L)/2;

    Sy = (ZBF_VY_H - ZBF_VY_L)/2;
    Ty = (ZBF_VY_H + ZBF_VY_L)/2;

    for( i = 0; i < n; i++, P++ )
    {
        w = P->CoordW;
        if ( w == 0.0 ) fprintf( stderr, "jippii\n"), w=1;

        P->CoordX  = ((int)(Sx*P->CoordX/w + Tx)) + 0.5;
        P->CoordY  = ((int)(Sy*P->CoordY/w + Ty)) + 0.5;
        P->CoordZ /= w;
    }
}

void trs_view_from( FLOAT x, FLOAT y, FLOAT z, FLOAT w )
{
    if ( w != 0.0 )
    {
        TRS_ViewpointW = 1.0; 
    } else
    {
        w = sqrt( x*x + y*y + z*z );
        TRS_ViewpointW = 0.0; 
    } 

    TRS_ViewpointX = x / w;
    TRS_ViewpointY = y / w;
    TRS_ViewpointZ = z / w;
}

void trs_viewport( int xl, int xh, int yl, int yh)
{
     ZBF_VX_L = MAX(0,xl);
     ZBF_VX_H = MIN(xh,ZBF_NX-1);

     ZBF_VY_L = MAX(0,yl);
     ZBF_VY_H = MIN(yh,ZBF_NY-1);
}
