#include "Gfx.h"
#include "Vector.h"

static double rx, ry, rz;
static int    numpts;
static int    border;

static int    startx, starty;

typedef struct{
    signed int x; signed int y; signed int z;
    unsigned char r; unsigned char g; unsigned char b;
}  point_3d ;

static point_3d *poinz     = (void*)0;
static point_3d *poinz_rot = (void*)0;

void Vector_init( int points, int borderlen ) {
    int i, j, k;
    rx = ry = rz = 0;
    numpts = points;
    border = borderlen;

    poinz     = (void*)malloc( points * points * points * sizeof( point_3d) );
    poinz_rot = (void*)malloc( points * points * points * sizeof( point_3d) );

    for( i = 0; i<points; i++)
        for( j = 0; j<points; j++)
            for( k = 0; k<points; k++) {
                poinz[i+j*points+k*points*points].x = ( i * borderlen ) / ( points - 1 ) - ( borderlen >> 1);
                poinz[i+j*points+k*points*points].y = ( j * borderlen ) / ( points - 1 ) - ( borderlen >> 1);
                poinz[i+j*points+k*points*points].z = ( k * borderlen ) / ( points - 1 ) - ( borderlen >> 1);
                poinz[i+j*points+k*points*points].r = 255;
                poinz[i+j*points+k*points*points].g = 255;
                poinz[i+j*points+k*points*points].b = 0;
            }
}

void Vector_destroy( ) {
    if( poinz )
        free( poinz );
    if( poinz_rot )
        free( poinz_rot );
}

void Vector_reset( void ) {
    rx = 0; ry = 0; rz = 0;
}

void Vector_angle( double dx, double dy, double dz ) {
    rx += dx, ry += dy, rz += dz;
}

void Vector_move( signed int dx, signed int dy ) {
    double rotation[3][3] = {
        { cos(-ry)*cos(-rz), -cos(-rx)*sin(-rz)+sin(-rx)*sin(-ry)*cos(-rz),  sin(-rx)*sin(-rz)+cos(-rx)*sin(-ry)*cos(-rz) },
        { cos(-ry)*sin(-rz),  cos(-rx)*cos(-rz)+sin(-rx)*sin(-ry)*sin(-rz), -sin(-rx)*cos(-rz)+cos(-rx)*sin(-ry)*sin(-rz) },
        {-sin(-ry),                             sin(-rx)*cos(-ry)         ,                    cos(-rx)*cos(-ry)          }
    };

    int xold = rotation[0][2] * border;
    int yold = rotation[1][2] * border;
    int zold = rotation[2][2] * border;

    int xnew = rotation[0][0] * dx + rotation[0][1] * dy + rotation[0][2] * border;
    int ynew = rotation[1][0] * dx + rotation[1][1] * dy + rotation[1][2] * border;
    int znew = rotation[2][0] * dx + rotation[2][1] * dy + rotation[2][2] * border;

    rx += 0.01 * (xold - xnew);
    ry += 0.01 * (yold - ynew);
    rz += 0.01 * (zold - znew);
}

static int compare_points( const void *a, const void *b) {
    return ((point_3d*)b)->z - ((point_3d*)a)->z ;
}

void Vector_paint( void ) {
    double rotation[3][3] = {
        {         cos(ry)*cos(rz)                 ,         -cos(ry)*sin(rz)                 ,          sin(ry) },
        { sin(rx)*sin(ry)*cos(rz)+cos(rx)*sin(rz) , -sin(rx)*sin(ry)*sin(rz)+cos(rx)*cos(rz) , -sin(rx)*cos(ry) },
        {-cos(rx)*sin(ry)*cos(rz)+sin(rx)*sin(rz) ,  cos(rx)*sin(ry)*sin(rz)+sin(rx)*cos(rz) ,  cos(rx)*cos(ry) }
    };
    int i;

    for( i = 0; i < numpts*numpts*numpts; i++) {
        poinz_rot[i].x = (rotation[0][0] * poinz[i].x + rotation[0][1] * poinz[i].y + rotation[0][2] * poinz[i].z)*65536;
        poinz_rot[i].y = (rotation[1][0] * poinz[i].x + rotation[1][1] * poinz[i].y + rotation[1][2] * poinz[i].z)*65536;
        poinz_rot[i].z =  rotation[2][0] * poinz[i].x + rotation[2][1] * poinz[i].y + rotation[2][2] * poinz[i].z;
//        poinz_rot[i].r = poinz[i].r; poinz_rot[i].g = poinz[i].g; poinz_rot[i].b = poinz[i].b;
        poinz_rot[i].r = random(); poinz_rot[i].g = random(); poinz_rot[i].b = random();
    }

    /* Sort by z-order */
    qsort( poinz_rot, numpts*numpts*numpts, sizeof(point_3d), compare_points);

    Gfx_clear();

    for( i = 0; i < numpts*numpts*numpts; i++) {
        int size;
        poinz_rot[i].x /= poinz_rot[i].z + 1.5*border;
        poinz_rot[i].y /= poinz_rot[i].z + 1.5*border;
        poinz_rot[i].x >>= 8;
        poinz_rot[i].y >>= 8;
        poinz_rot[i].x += g_width>>1;
        poinz_rot[i].y += g_height>>1;

        size = 10 * border / ( (poinz_rot[i].z) + border );

        Gfx_kuller( poinz_rot[i].x-(size>>1), poinz_rot[i].y-(size>>1), size, poinz_rot[i].r, poinz_rot[i].g, poinz_rot[i].b);
    }
}