#include <stdio.h>
#include <libps.h>
#include <rand.h>
#include "pad.h"
#include "box.h"
#include "cubetmd.h"

#define	SPEED	16


box_t		box_array[3][3][3];

int		axis;
int		row;
int		rot_countdown;
int		direction;


void
box_init()
{
	int	x, y, z;

	for ( z=0 ; z<3 ; z++ ) {
		for ( y=0 ; y<3 ; y++ ) {
			for ( x=0 ; x<3 ; x++ ) {
				box_init_object( &box_array[z][y][x] );
				box_array[z][y][x].local_position.vx = 
					(x-1) * 80;
				box_array[z][y][x].local_position.vy =
					(y-1) * 80;
				box_array[z][y][x].local_position.vz =
					(z-1) * 80;
			}
		}
	}

	rot_countdown = (4096/4) / SPEED;
	axis = rand() % 3;
	row = rand() % 3;
	switch ( rand() % 2 ) {
		case 0:		direction = -SPEED;	break;
		case 1:		direction =  SPEED;	break;
	}
}


void
box_init_object( box_t *box )
{
	u_long		*model_ptr;

	box->world_position.vx = 0;
	box->world_position.vy = 0;
	box->world_position.vz = 0;

	box->world_rotation.vx = 0;
	box->world_rotation.vy = 0;
	box->world_rotation.vz = 0;

	GsInitCoordinate2( WORLD, &box->world_coord );

	box->local_position.vx = 0;
	box->local_position.vy = 0;
	box->local_position.vz = 0;

	box->local_rotation.vx = 0;
	box->local_rotation.vy = 0;
	box->local_rotation.vz = 0;

	GsInitCoordinate2( &box->world_coord, &box->local_coord );

	box->handler.coord2 = &(box->world_coord);

	box->tmd_address = (u_long)cube_tmd_data;
	box->which_model = 0;

	model_ptr = (u_long *)cube_tmd_data;
	model_ptr++;
	GsMapModelingData( model_ptr );
	model_ptr += 2;
	GsLinkObject4( (u_long)model_ptr, &box->handler, box->which_model );
}
 

void
box_execute( u_long pad_data )
{
	int	old_axis;
	int	x, y, z;

	rot_countdown--;
	if ( rot_countdown <= 0 ) {
		box_fixup();
		rot_countdown = (4096/4) / SPEED;
		row = rand() % 3;
		old_axis = axis;
		axis = rand() % 3;
		if ( axis == old_axis ) {
			axis++;
			if ( axis > 2 ) {
				axis = 0;
			}
		}
		switch ( rand() % 2 ) {
			case 0:		direction = -SPEED;	break;
			case 1:		direction =  SPEED;	break;
		}
	}

	for ( z=0 ; z<3 ; z++ ) {
		for ( y=0 ; y<3 ; y++ ) {
			for ( x=0 ; x<3 ; x++ ) {

				if ( (axis == 0) && (row == x) ) {
					box_array[z][y][x].world_rotation.vx
						+= direction;
				} else if ( (axis == 1) && (row == y) ) {
					box_array[z][y][x].world_rotation.vy
						+= direction;
				} else if ( (axis == 2) && (row == z) ) {
					box_array[z][y][x].world_rotation.vz
						+= direction;
				}

				box_local_update( &box_array[z][y][x] );
				box_world_update( &box_array[z][y][x] );
			}
		}
	}
}


void
box_local_update( box_t *box )
{
	MATRIX		tempMatrix;

	/* get rotation matrix from rotation vector */
	RotMatrix( &box->local_rotation, &tempMatrix );

	/* assign new matrix to coordinate system */
	box->local_coord.coord = tempMatrix;
	
	/* set position by absolute coordinates	*/
	box->local_coord.coord.t[0] = box->local_position.vx;
	box->local_coord.coord.t[1] = box->local_position.vy;
	box->local_coord.coord.t[2] = box->local_position.vz;

	// tell GTE that coordinate system has been updated
	box->local_coord.flg = 0;

}


/*
 * This function performs world-relative movement and rotation.
 */
void
box_world_update( box_t *box )
{
	MATRIX		tempMatrix;

	/* get rotation matrix from rotation vector */
	RotMatrix( &box->world_rotation, &tempMatrix );

	/* assign new matrix to coordinate system */
	box->world_coord.coord = tempMatrix;
	
	/* set position by absolute coordinates	*/
	box->world_coord.coord.t[0] = box->world_position.vx;
	box->world_coord.coord.t[1] = box->world_position.vy;
	box->world_coord.coord.t[2] = box->world_position.vz;

	// tell GTE that coordinate system has been updated
	box->world_coord.flg = 0;
}


void
box_draw( void )
{
	int	x, y, z;

	for ( z=0 ; z<3 ; z++ ) {
		for ( y=0 ; y<3 ; y++ ) {
			for ( x=0 ; x<3 ; x++ ) {
				box_drawbox( &box_array[z][y][x] );
			}
		}
	}
}


void
box_drawbox( box_t *box )
{
	MATRIX		tmpls;
	extern GsOT	WorldOT[];
	extern int	ActiveBuff;

	GsGetLs( &box->local_coord, &tmpls );
	GsSetLightMatrix( &tmpls );
	GsSetLsMatrix( &tmpls );

	GsSortObject4( &box->handler, &WorldOT[ActiveBuff], 3,
		getScratchAddr(0) );
}


void
box_fixup( void )
{
	int	x, y, z;

	for ( z=0 ; z<3 ; z++ ) {
		for ( y=0 ; y<3 ; y++ ) {
			for ( x=0 ; x<3 ; x++ ) {
				box_array[z][y][x].local_position.vx = 
					(x-1) * 80;
				box_array[z][y][x].local_position.vy =
					(y-1) * 80;
				box_array[z][y][x].local_position.vz =
					(z-1) * 80;
				box_array[z][y][x].world_rotation.vx = 0;
				box_array[z][y][x].world_rotation.vy = 0;
				box_array[z][y][x].world_rotation.vz = 0;
			}
		}
	}
}