#include "N64.h"
#include "RSP.h"
#include "GBI.h"
#include "gDP.h"
#include "Types.h"
#include "Debug.h"

void RDP_Unknown( u32 w0, u32 w1 )
{
#ifdef DEBUG
	DebugMsg( DEBUG_UNKNOWN, "RDP_Unknown\r\n" );
	DebugMsg( DEBUG_UNKNOWN, "\tUnknown RDP opcode %02X\r\n", _SHIFTR( w0, 24, 8 ) );
#endif
}

void RDP_NoOp( u32 w0, u32 w1 )
{
	gDPNoOp();
}

void RDP_SetCImg( u32 w0, u32 w1 )
{
	gDPSetColorImage( _SHIFTR( w0, 21,  3 ),		// fmt
		              _SHIFTR( w0, 19,  2 ),		// siz
                      _SHIFTR( w0,  0, 12 ) + 1,	// width
					  w1 );							// img
}

void RDP_SetZImg( u32 w0, u32 w1 )
{
	gDPSetDepthImage( w1 );	// img
}

void RDP_SetTImg( u32 w0, u32 w1 )
{
	gDPSetTextureImage( _SHIFTR( w0, 21,  3),		// fmt
						_SHIFTR( w0, 19,  2 ),		// siz
						_SHIFTR( w0,  0, 12 ) + 1,	// width
						w1 );						// img
}

void RDP_SetCombine( u32 w0, u32 w1 )
{
	gDPSetCombine( _SHIFTR( w0, 0, 24 ),	// muxs0
				   w1 );					// muxs1
}

void RDP_SetEnvColor( u32 w0, u32 w1 )
{
	gDPSetEnvColor( _SHIFTR( w1, 24, 8 ),		// r
					_SHIFTR( w1, 16, 8 ),		// g
					_SHIFTR( w1,  8, 8 ),		// b
					_SHIFTR( w1,  0, 8 ) );		// a
}

void RDP_SetPrimColor( u32 w0, u32 w1 )
{
	gDPSetPrimColor( _SHIFTL( w0,  8, 8 ),		// m
		             _SHIFTL( w0,  0, 8 ),		// l
					 _SHIFTR( w1, 24, 8 ),		// r
					 _SHIFTR( w1, 16, 8 ),		// g
					 _SHIFTR( w1,  8, 8 ),		// b
					 _SHIFTR( w1,  0, 8 ) );	// a

}

void RDP_SetBlendColor( u32 w0, u32 w1 )
{
	gDPSetBlendColor( _SHIFTR( w1, 24, 8 ),		// r
			 		  _SHIFTR( w1, 16, 8 ),		// g
					  _SHIFTR( w1,  8, 8 ),		// b
					  _SHIFTR( w1,  0, 8 ) );	// a
}

void RDP_SetFogColor( u32 w0, u32 w1 )
{
	gDPSetFogColor( _SHIFTR( w1, 24, 8 ),		// r
					_SHIFTR( w1, 16, 8 ),		// g
					_SHIFTR( w1,  8, 8 ),		// b
					_SHIFTR( w1,  0, 8 ) );		// a
}

void RDP_SetFillColor( u32 w0, u32 w1 )
{
	gDPSetFillColor( w1 );
}

void RDP_FillRect( u32 w0, u32 w1 )
{
	gDPFillRectangle( _SHIFTR( w1, 14, 10 ),	// ulx
					  _SHIFTR( w1,  2, 10 ),	// uly
		              _SHIFTR( w0, 14, 10 ),	// lrx
					  _SHIFTR( w0,  2, 10 ) );	// lry
}

void RDP_SetTile( u32 w0, u32 w1 )
{
	gDPSetTile( _SHIFTR( w0, 21, 3 ),	// fmt
	            _SHIFTR( w0, 19, 2 ),	// siz
	            _SHIFTR( w0,  9, 9 ),	// line
				_SHIFTR( w0,  0, 9 ),	// tmem
				_SHIFTR( w1, 24, 3 ),	// tile
				_SHIFTR( w1, 20, 4 ),	// palette
				_SHIFTR( w1, 18, 2 ),	// cmt
				_SHIFTR( w1,  8, 2 ),	// cms
				_SHIFTR( w1, 14, 4 ),	// maskt
				_SHIFTR( w1,  4, 4 ),	// masks
				_SHIFTR( w1, 10, 4 ),	// shiftt
				_SHIFTR( w1,  0, 4 ) );	// shifts
}

void RDP_LoadTile( u32 w0, u32 w1 )
{
	gDPLoadTile( _SHIFTR( w1, 24,  3 ),		// tile
		         _SHIFTR( w0, 12, 12 ),		// uls
				 _SHIFTR( w0,  0, 12 ),		// ult
				 _SHIFTR( w1, 12, 12 ),		// lrs
				 _SHIFTR( w1,  0, 12 ) );	// lrt
}

void RDP_LoadBlock( u32 w0, u32 w1 )
{
	gDPLoadBlock( _SHIFTR( w1, 24,  3 ),	// tile
		          _SHIFTR( w0, 12, 12 ),	// uls
				  _SHIFTR( w0,  0, 12 ),	// ult
				  _SHIFTR( w1, 12, 12 ),	// lrs
				  _SHIFTR( w1,  0, 12 ) );	// dxt
}

void RDP_SetTileSize( u32 w0, u32 w1 )
{
	gDPSetTileSize( _SHIFTR( w1, 24,  3 ),		// tile
		            _SHIFTR( w0, 12, 12 ),		// uls
				    _SHIFTR( w0,  0, 12 ),		// ult
				    _SHIFTR( w1, 12, 12 ),		// lrs
				    _SHIFTR( w1,  0, 12 ) );	// lrt
}

void RDP_LoadTLUT( u32 w0, u32 w1 )
{
	gDPLoadTLUT( _SHIFTR( w1, 24,  3 ),	// tile
		          _SHIFTR( w0, 12, 12 ),	// uls
				  _SHIFTR( w0,  0, 12 ),	// ult
				  _SHIFTR( w1, 12, 12 ),	// lrs
				  _SHIFTR( w1,  0, 12 ) );	// lrt
}

void RDP_SetOtherMode( u32 w0, u32 w1 )
{
	gDPSetOtherMode( _SHIFTR( w0, 0, 24 ),	// mode0
					 w1 );					// mode1
}

void RDP_SetPrimDepth( u32 w0, u32 w1 )
{
	gDPSetPrimDepth( _SHIFTR( w1, 16, 16 ),		// z
		             _SHIFTR( w1,  0, 16 ) );	// dz
}

void RDP_SetScissor( u32 w0, u32 w1 )
{
	gDPSetScissor( _SHIFTR( w1, 24, 2 ),						// mode
		           _FIXED2FLOAT( _SHIFTR( w0, 12, 12 ), 2 ),	// ulx
				   _FIXED2FLOAT( _SHIFTR( w0,  0, 12 ), 2 ),	// uly
				   _FIXED2FLOAT( _SHIFTR( w1, 12, 12 ), 2 ),	// lrx
				   _FIXED2FLOAT( _SHIFTR( w1,  0, 12 ), 2 ) );	// lry
}

void RDP_SetConvert( u32 w0, u32 w1 )
{
	gDPSetConvert( _SHIFTR( w0, 13, 9 ),	// k0
		           _SHIFTR( w0,  4, 9 ),	// k1
				   _SHIFTL( w0,  5, 4 ) | _SHIFTR( w1, 25, 5 ),	// k2
				   _SHIFTR( w1, 18, 9 ),	// k3
				   _SHIFTR( w1,  9, 9 ),	// k4
				   _SHIFTR( w1,  0, 9 ) );	// k5
}

void RDP_SetKeyR( u32 w0, u32 w1 )
{
	gDPSetKeyR( _SHIFTR( w1,  8,  8 ),		// cR
			    _SHIFTR( w1,  0,  8 ),		// sR
				_SHIFTR( w1, 16, 12 ) );	// wR
}

void RDP_SetKeyGB( u32 w0, u32 w1 )
{
	gDPSetKeyGB( _SHIFTR( w1, 24,  8 ),		// cG
		         _SHIFTR( w1, 16,  8 ),		// sG
				 _SHIFTR( w0, 12, 12 ),		// wG
				 _SHIFTR( w1,  8,  8 ),		// cB
				 _SHIFTR( w1,  0,  8 ),		// SB
				 _SHIFTR( w0,  0, 12 ) );	// wB
}

void RDP_FullSync( u32 w0, u32 w1 )
{
	gDPFullSync();
}

void RDP_TileSync( u32 w0, u32 w1 )
{
	gDPTileSync();
}

void RDP_PipeSync( u32 w0, u32 w1 )
{
	gDPPipeSync();
}

void RDP_LoadSync( u32 w0, u32 w1 )
{
	gDPLoadSync();
}

void RDP_TexRectFlip( u32 w0, u32 w1 )
{
	u32 w2 = *(u32*)&RDRAM[RSP.PC[RSP.PCi] + 4];
	RSP.PC[RSP.PCi] += 8;

	u32 w3 = *(u32*)&RDRAM[RSP.PC[RSP.PCi] + 4];
	RSP.PC[RSP.PCi] += 8;

	gDPTextureRectangleFlip( _FIXED2FLOAT( _SHIFTR( w1, 12, 12 ), 2 ),			// ulx
							 _FIXED2FLOAT( _SHIFTR( w1,  0, 12 ), 2 ),			// uly
							 _FIXED2FLOAT( _SHIFTR( w0, 12, 12 ), 2 ),			// lrx
							 _FIXED2FLOAT( _SHIFTR( w0,  0, 12 ), 2 ),			// lry
							 _SHIFTR( w1, 24,  3 ),								// tile
							 _FIXED2FLOAT( (s16)_SHIFTR( w2, 16, 16 ), 5 ),		// s
							 _FIXED2FLOAT( (s16)_SHIFTR( w2,  0, 16 ), 5 ),		// t
							 _FIXED2FLOAT( (s16)_SHIFTR( w3, 16, 16 ), 10 ),	// dsdx
							 _FIXED2FLOAT( (s16)_SHIFTR( w3,  0, 16 ), 10 ) );	// dsdy
}

void RDP_TexRect( u32 w0, u32 w1 )
{
	u32 w2 = *(u32*)&RDRAM[RSP.PC[RSP.PCi] + 4];
	RSP.PC[RSP.PCi] += 8;

	u32 w3 = *(u32*)&RDRAM[RSP.PC[RSP.PCi] + 4];
	RSP.PC[RSP.PCi] += 8;

	gDPTextureRectangle( _FIXED2FLOAT( _SHIFTR( w1, 12, 12 ), 2 ),			// ulx
						 _FIXED2FLOAT( _SHIFTR( w1,  0, 12 ), 2 ),			// uly
						 _FIXED2FLOAT( _SHIFTR( w0, 12, 12 ), 2 ),			// lrx
						 _FIXED2FLOAT( _SHIFTR( w0,  0, 12 ), 2 ),			// lry
						 _SHIFTR( w1, 24,  3 ),								// tile
						 _FIXED2FLOAT( (s16)_SHIFTR( w2, 16, 16 ), 5 ),		// s
						 _FIXED2FLOAT( (s16)_SHIFTR( w2,  0, 16 ), 5 ),		// t
						 _FIXED2FLOAT( (s16)_SHIFTR( w3, 16, 16 ), 10 ),	// dsdx
						 _FIXED2FLOAT( (s16)_SHIFTR( w3,  0, 16 ), 10 ) );	// dsdy
}

void RDP_Init()
{
	// Initialize RDP commands to RDP_UNKNOWN
	for (int i = 0xC8; i <= 0xCF; i++)
		GBI.cmd[i] = RDP_Unknown;

	// Initialize RDP commands to RDP_UNKNOWN
	for (int i = 0xE4; i <= 0xFF; i++)
		GBI.cmd[i] = RDP_Unknown;

	// Set known GBI commands
	GBI.cmd[G_NOOP]				= RDP_NoOp;
	GBI.cmd[G_SETCIMG]			= RDP_SetCImg;
	GBI.cmd[G_SETZIMG]			= RDP_SetZImg;
	GBI.cmd[G_SETTIMG]			= RDP_SetTImg;
	GBI.cmd[G_SETCOMBINE]		= RDP_SetCombine;
	GBI.cmd[G_SETENVCOLOR]		= RDP_SetEnvColor;
	GBI.cmd[G_SETPRIMCOLOR]		= RDP_SetPrimColor;
	GBI.cmd[G_SETBLENDCOLOR]	= RDP_SetBlendColor;
	GBI.cmd[G_SETFOGCOLOR]		= RDP_SetFogColor;
	GBI.cmd[G_SETFILLCOLOR]		= RDP_SetFillColor;
	GBI.cmd[G_FILLRECT]			= RDP_FillRect;
	GBI.cmd[G_SETTILE]			= RDP_SetTile;
	GBI.cmd[G_LOADTILE]			= RDP_LoadTile;
	GBI.cmd[G_LOADBLOCK]		= RDP_LoadBlock;
	GBI.cmd[G_SETTILESIZE]		= RDP_SetTileSize;
	GBI.cmd[G_LOADTLUT]			= RDP_LoadTLUT;
	GBI.cmd[G_RDPSETOTHERMODE]	= RDP_SetOtherMode;
	GBI.cmd[G_SETPRIMDEPTH]		= RDP_SetPrimDepth;
	GBI.cmd[G_SETSCISSOR]		= RDP_SetScissor;
	GBI.cmd[G_SETCONVERT]		= RDP_SetConvert;
	GBI.cmd[G_SETKEYR]			= RDP_SetKeyR;
	GBI.cmd[G_SETKEYGB]			= RDP_SetKeyGB;
	GBI.cmd[G_RDPFULLSYNC]		= RDP_FullSync;
	GBI.cmd[G_RDPTILESYNC]		= RDP_TileSync;
	GBI.cmd[G_RDPPIPESYNC]		= RDP_PipeSync;
	GBI.cmd[G_RDPLOADSYNC]		= RDP_LoadSync;
	GBI.cmd[G_TEXRECTFLIP]		= RDP_TexRectFlip;
	GBI.cmd[G_TEXRECT]			= RDP_TexRect;
}