/*
Copyright (C) 2003 ZSNES Team ( zsknight@zsnes.com / _demo_@zsnes.com )

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either
version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

//http://users.tpg.com.au/trauma/dsp/st010.html

#ifdef __LINUX__
#include "gblhdr.h"
#else
#include <math.h>
#endif

#define ABS(x) ((x)<0?-(x):(x))

#define SRAM setaramdata

#ifdef __WIN32__
extern "C" { 
#endif
extern unsigned char *setaramdata;
void ST010DoCommand(void);
#ifdef __WIN32__
}
#endif


//C++ style code in C
#define bool unsigned char
#define true 1
#define false 0

typedef signed char     int8;
typedef unsigned char   uint8;
typedef signed short    int16;
typedef unsigned short  uint16;
typedef int             int32;
typedef unsigned int    uint32;

// Mode 7 scaling constants for all raster lines
const int16 ST010_M7Scale[176] = {
	0x0380,  0x0325,  0x02da,  0x029c,  0x0268,  0x023b,  0x0215,  0x01f3,
	0x01d5,  0x01bb,  0x01a3,  0x018e,  0x017b,  0x016a,  0x015a,  0x014b,
	0x013e,  0x0132,  0x0126,  0x011c,  0x0112,  0x0109,  0x0100,  0x00f8,
	0x00f0,  0x00e9,  0x00e3,  0x00dc,  0x00d6,  0x00d1,  0x00cb,  0x00c6,
	0x00c1,  0x00bd,  0x00b8,  0x00b4,  0x00b0,  0x00ac,  0x00a8,  0x00a5,
	0x00a2,  0x009e,  0x009b,  0x0098,  0x0095,  0x0093,  0x0090,  0x008d,
	0x008b,  0x0088,  0x0086,  0x0084,  0x0082,  0x0080,  0x007e,  0x007c,
	0x007a,  0x0078,  0x0076,  0x0074,  0x0073,  0x0071,  0x006f,  0x006e,
	0x006c,  0x006b,  0x0069,  0x0068,  0x0067,  0x0065,  0x0064,  0x0063,
	0x0062,  0x0060,  0x005f,  0x005e,  0x005d,  0x005c,  0x005b,  0x005a,
	0x0059,  0x0058,  0x0057,  0x0056,  0x0055,  0x0054,  0x0053,  0x0052,
	0x0051,  0x0051,  0x0050,  0x004f,  0x004e,  0x004d,  0x004d,  0x004c,
	0x004b,  0x004b,  0x004a,  0x0049,  0x0048,  0x0048,  0x0047,  0x0047,
	0x0046,  0x0045,  0x0045,  0x0044,  0x0044,  0x0043,  0x0042,  0x0042,
	0x0041,  0x0041,  0x0040,  0x0040,  0x003f,  0x003f,  0x003e,  0x003e,
	0x003d,  0x003d,  0x003c,  0x003c,  0x003b,  0x003b,  0x003a,  0x003a,
	0x003a,  0x0039,  0x0039,  0x0038,  0x0038,  0x0038,  0x0037,  0x0037,
	0x0036,  0x0036,  0x0036,  0x0035,  0x0035,  0x0035,  0x0034,  0x0034,
	0x0034,  0x0033,  0x0033,  0x0033,  0x0032,  0x0032,  0x0032,  0x0031,
	0x0031,  0x0031,  0x0030,  0x0030,  0x0030,  0x0030,  0x002f,  0x002f,
	0x002f,  0x002e,  0x002e,  0x002e,  0x002e,  0x002d,  0x002d,  0x002d,
	0x002d,  0x002c,  0x002c,  0x002c,  0x002c,  0x002b,  0x002b,  0x002b
};

const int16 ST010_SinTable[256] = {
	 0x0000,  0x0324,  0x0648,  0x096a,  0x0c8c,  0x0fab,  0x12c8,  0x15e2,
	 0x18f9,  0x1c0b,  0x1f1a,  0x2223,  0x2528,  0x2826,  0x2b1f,  0x2e11,
	 0x30fb,  0x33df,  0x36ba,  0x398c,  0x3c56,  0x3f17,  0x41ce,  0x447a,
	 0x471c,  0x49b4,  0x4c3f,  0x4ebf,  0x5133,  0x539b,  0x55f5,  0x5842,
	 0x5a82,  0x5cb3,  0x5ed7,  0x60eb,  0x62f1,  0x64e8,  0x66cf,  0x68a6,
	 0x6a6d,  0x6c23,  0x6dc9,  0x6f5e,  0x70e2,  0x7254,  0x73b5,  0x7504,
	 0x7641,  0x776b,  0x7884,  0x7989,  0x7a7c,  0x7b5c,  0x7c29,  0x7ce3,
	 0x7d89,  0x7e1d,  0x7e9c,  0x7f09,  0x7f61,  0x7fa6,  0x7fd8,  0x7ff5,
	 0x7fff,  0x7ff5,  0x7fd8,  0x7fa6,  0x7f61,  0x7f09,  0x7e9c,  0x7e1d,
	 0x7d89,  0x7ce3,  0x7c29,  0x7b5c,  0x7a7c,  0x7989,  0x7884,  0x776b,
	 0x7641,  0x7504,  0x73b5,  0x7254,  0x70e2,  0x6f5e,  0x6dc9,  0x6c23,
	 0x6a6d,  0x68a6,  0x66cf,  0x64e8,  0x62f1,  0x60eb,  0x5ed7,  0x5cb3,
	 0x5a82,  0x5842,  0x55f5,  0x539b,  0x5133,  0x4ebf,  0x4c3f,  0x49b4,
	 0x471c,  0x447a,  0x41ce,  0x3f17,  0x3c56,  0x398c,  0x36ba,  0x33df,
	 0x30fb,  0x2e11,  0x2b1f,  0x2826,  0x2528,  0x2223,  0x1f1a,  0x1c0b,
	 0x18f8,  0x15e2,  0x12c8,  0x0fab,  0x0c8c,  0x096a,  0x0648,  0x0324,
	 0x0000, -0x0324, -0x0648, -0x096b, -0x0c8c, -0x0fab, -0x12c8, -0x15e2,
	-0x18f9, -0x1c0b, -0x1f1a, -0x2223, -0x2528, -0x2826, -0x2b1f, -0x2e11,
	-0x30fb, -0x33df, -0x36ba, -0x398d, -0x3c56, -0x3f17, -0x41ce, -0x447a,
	-0x471c, -0x49b4, -0x4c3f, -0x4ebf, -0x5133, -0x539b, -0x55f5, -0x5842,
	-0x5a82, -0x5cb3, -0x5ed7, -0x60ec, -0x62f1, -0x64e8, -0x66cf, -0x68a6,
	-0x6a6d, -0x6c23, -0x6dc9, -0x6f5e, -0x70e2, -0x7254, -0x73b5, -0x7504,
	-0x7641, -0x776b, -0x7884, -0x7989, -0x7a7c, -0x7b5c, -0x7c29, -0x7ce3,
	-0x7d89, -0x7e1d, -0x7e9c, -0x7f09, -0x7f61, -0x7fa6, -0x7fd8, -0x7ff5,
	-0x7fff, -0x7ff5, -0x7fd8, -0x7fa6, -0x7f61, -0x7f09, -0x7e9c, -0x7e1d,
	-0x7d89, -0x7ce3, -0x7c29, -0x7b5c, -0x7a7c, -0x7989, -0x7883, -0x776b,
	-0x7641, -0x7504, -0x73b5, -0x7254, -0x70e2, -0x6f5e, -0x6dc9, -0x6c23,
	-0x6a6d, -0x68a6, -0x66cf, -0x64e8, -0x62f1, -0x60eb, -0x5ed7, -0x5cb3,
	-0x5a82, -0x5842, -0x55f5, -0x539a, -0x5133, -0x4ebf, -0x4c3f, -0x49b3,
	-0x471c, -0x447a, -0x41cd, -0x3f17, -0x3c56, -0x398c, -0x36b9, -0x33de,
	-0x30fb, -0x2e10, -0x2b1f, -0x2826, -0x2527, -0x2223, -0x1f19, -0x1c0b,
	-0x18f8, -0x15e2, -0x12c8, -0x0fab, -0x0c8b, -0x096a, -0x0647, -0x0324};

const unsigned char ST010_ArcTan[32][32] = {
	{ 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80,
	  0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80},
	{ 0x80, 0xa0, 0xad, 0xb3, 0xb6, 0xb8, 0xb9, 0xba, 0xbb, 0xbb, 0xbc, 0xbc, 0xbd, 0xbd, 0xbd, 0xbd,
	  0xbd, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbf, 0xbf, 0xbf, 0xbf},
	{ 0x80, 0x93, 0xa0, 0xa8, 0xad, 0xb0, 0xb3, 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xb9, 0xba, 0xba, 0xbb,
	  0xbb, 0xbb, 0xbb, 0xbc, 0xbc, 0xbc, 0xbc, 0xbc, 0xbd, 0xbd, 0xbd, 0xbd, 0xbd, 0xbd, 0xbd, 0xbd},
	{ 0x80, 0x8d, 0x98, 0xa0, 0xa6, 0xaa, 0xad, 0xb0, 0xb1, 0xb3, 0xb4, 0xb5, 0xb6, 0xb7, 0xb7, 0xb8,
	  0xb8, 0xb9, 0xb9, 0xba, 0xba, 0xba, 0xba, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbc, 0xbc, 0xbc, 0xbc},
	{ 0x80, 0x8a, 0x93, 0x9a, 0xa0, 0xa5, 0xa8, 0xab, 0xad, 0xaf, 0xb0, 0xb2, 0xb3, 0xb4, 0xb5, 0xb5,
	  0xb6, 0xb7, 0xb7, 0xb8, 0xb8, 0xb8, 0xb9, 0xb9, 0xb9, 0xba, 0xba, 0xba, 0xba, 0xba, 0xbb, 0xbb},
	{ 0x80, 0x88, 0x90, 0x96, 0x9b, 0xa0, 0xa4, 0xa7, 0xa9, 0xab, 0xad, 0xaf, 0xb0, 0xb1, 0xb2, 0xb3,
	  0xb4, 0xb4, 0xb5, 0xb6, 0xb6, 0xb6, 0xb7, 0xb7, 0xb8, 0xb8, 0xb8, 0xb9, 0xb9, 0xb9, 0xb9, 0xb9},
	{ 0x80, 0x87, 0x8d, 0x93, 0x98, 0x9c, 0xa0, 0xa3, 0xa6, 0xa8, 0xaa, 0xac, 0xad, 0xae, 0xb0, 0xb0,
	  0xb1, 0xb2, 0xb3, 0xb4, 0xb4, 0xb5, 0xb5, 0xb6, 0xb6, 0xb6, 0xb7, 0xb7, 0xb7, 0xb8, 0xb8, 0xb8},
	{ 0x80, 0x86, 0x8b, 0x90, 0x95, 0x99, 0x9d, 0xa0, 0xa3, 0xa5, 0xa7, 0xa9, 0xaa, 0xac, 0xad, 0xae,
	  0xaf, 0xb0, 0xb1, 0xb2, 0xb2, 0xb3, 0xb3, 0xb4, 0xb4, 0xb5, 0xb5, 0xb6, 0xb6, 0xb6, 0xb7, 0xb7},
	{ 0x80, 0x85, 0x8a, 0x8f, 0x93, 0x97, 0x9a, 0x9d, 0xa0, 0xa2, 0xa5, 0xa6, 0xa8, 0xaa, 0xab, 0xac,
	  0xad, 0xae, 0xaf, 0xb0, 0xb0, 0xb1, 0xb2, 0xb2, 0xb3, 0xb3, 0xb4, 0xb4, 0xb5, 0xb5, 0xb5, 0xb5},
	{ 0x80, 0x85, 0x89, 0x8d, 0x91, 0x95, 0x98, 0x9b, 0x9e, 0xa0, 0xa0, 0xa4, 0xa6, 0xa7, 0xa9, 0xaa,
	  0xab, 0xac, 0xad, 0xae, 0xaf, 0xb0, 0xb0, 0xb1, 0xb1, 0xb2, 0xb2, 0xb3, 0xb3, 0xb4, 0xb4, 0xb4},
	{ 0x80, 0x84, 0x88, 0x8c, 0x90, 0x93, 0x96, 0x99, 0x9b, 0x9e, 0xa0, 0xa2, 0xa4, 0xa5, 0xa7, 0xa8,
	  0xa9, 0xaa, 0xab, 0xac, 0xad, 0xae, 0xaf, 0xaf, 0xb0, 0xb0, 0xb1, 0xb2, 0xb2, 0xb2, 0xb3, 0xb3},
	{ 0x80, 0x84, 0x87, 0x8b, 0x8e, 0x91, 0x94, 0x97, 0x9a, 0x9c, 0x9e, 0xa0, 0xa2, 0xa3, 0xa5, 0xa6,
	  0xa7, 0xa9, 0xaa, 0xab, 0xac, 0xac, 0xad, 0xae, 0xae, 0xaf, 0xb0, 0xb0, 0xb1, 0xb1, 0xb2, 0xb2},
	{ 0x80, 0x83, 0x87, 0x8a, 0x8d, 0x90, 0x93, 0x96, 0x98, 0x9a, 0x9c, 0x9e, 0xa0, 0xa2, 0xa3, 0xa5,
	  0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xab, 0xac, 0xac, 0xad, 0xae, 0xae, 0xaf, 0xb0, 0xb0, 0xb0, 0xb1},
	{ 0x80, 0x83, 0x86, 0x89, 0x8c, 0x8f, 0x92, 0x94, 0x96, 0x99, 0x9b, 0x9d, 0x9e, 0xa0, 0xa2, 0xa3,
	  0xa4, 0xa5, 0xa7, 0xa8, 0xa9, 0xa9, 0xaa, 0xab, 0xac, 0xac, 0xad, 0xae, 0xae, 0xaf, 0xaf, 0xb0},
	{ 0x80, 0x83, 0x86, 0x89, 0x8b, 0x8e, 0x90, 0x93, 0x95, 0x97, 0x99, 0x9b, 0x9d, 0x9e, 0xa0, 0xa1,
	  0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xaa, 0xab, 0xac, 0xad, 0xad, 0xae, 0xae, 0xaf},
	{ 0x80, 0x83, 0x85, 0x88, 0x8b, 0x8d, 0x90, 0x92, 0x94, 0x96, 0x98, 0x9a, 0x9b, 0x9d, 0x9f, 0xa0,
	  0xa1, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa8, 0xa9, 0xaa, 0xab, 0xab, 0xac, 0xad, 0xad, 0xae},
	{ 0x80, 0x83, 0x85, 0x88, 0x8a, 0x8c, 0x8f, 0x91, 0x93, 0x95, 0x97, 0x99, 0x9a, 0x9c, 0x9d, 0x9f,
	  0xa0, 0xa1, 0xa2, 0xa3, 0xa5, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xaa, 0xab, 0xab, 0xac, 0xad},
	{ 0x80, 0x82, 0x85, 0x87, 0x89, 0x8c, 0x8e, 0x90, 0x92, 0x94, 0x96, 0x97, 0x99, 0x9b, 0x9c, 0x9d,
	  0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa8, 0xa9, 0xaa, 0xaa, 0xab, 0xac},
	{ 0x80, 0x82, 0x85, 0x87, 0x89, 0x8b, 0x8d, 0x8f, 0x91, 0x93, 0x95, 0x96, 0x98, 0x99, 0x9b, 0x9c,
	  0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa7, 0xa8, 0xa9, 0xa9, 0xaa, 0xab},
	{ 0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8c, 0x8e, 0x90, 0x92, 0x94, 0x95, 0x97, 0x98, 0x9a, 0x9b,
	  0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa6, 0xa7, 0xa8, 0xa8, 0xa9, 0xaa},
	{ 0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8c, 0x8e, 0x90, 0x91, 0x93, 0x94, 0x96, 0x97, 0x99, 0x9a,
	  0x9b, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa5, 0xa5, 0xa6, 0xa7, 0xa7, 0xa8, 0xa9},
	{ 0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8b, 0x8d, 0x8f, 0x90, 0x92, 0x94, 0x95, 0x97, 0x98, 0x99,
	  0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa4, 0xa5, 0xa6, 0xa6, 0xa7, 0xa8},
	{ 0x80, 0x82, 0x84, 0x86, 0x87, 0x89, 0x8b, 0x8d, 0x8e, 0x90, 0x91, 0x93, 0x94, 0x96, 0x97, 0x98,
	  0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa3, 0xa4, 0xa5, 0xa6, 0xa6, 0xa7},
	{ 0x80, 0x82, 0x84, 0x85, 0x87, 0x89, 0x8a, 0x8c, 0x8e, 0x8f, 0x91, 0x92, 0x94, 0x95, 0x96, 0x98,
	  0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa2, 0xa3, 0xa4, 0xa5, 0xa5, 0xa6},
	{ 0x80, 0x82, 0x83, 0x85, 0x87, 0x88, 0x8a, 0x8c, 0x8d, 0x8f, 0x90, 0x92, 0x93, 0x94, 0x96, 0x97,
	  0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa2, 0xa3, 0xa4, 0xa5, 0xa5},
	{ 0x80, 0x82, 0x83, 0x85, 0x86, 0x88, 0x8a, 0x8b, 0x8d, 0x8e, 0x90, 0x91, 0x92, 0x94, 0x95, 0x96,
	  0x97, 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa2, 0xa3, 0xa4, 0xa4},
	{ 0x80, 0x82, 0x83, 0x85, 0x86, 0x88, 0x89, 0x8b, 0x8c, 0x8e, 0x8f, 0x90, 0x92, 0x93, 0x94, 0x95,
	  0x96, 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa2, 0xa3, 0xa4},
	{ 0x80, 0x82, 0x83, 0x85, 0x86, 0x87, 0x89, 0x8a, 0x8c, 0x8d, 0x8e, 0x90, 0x91, 0x92, 0x93, 0x95,
	  0x96, 0x97, 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9e, 0x9f, 0xa0, 0xa1, 0xa1, 0xa2, 0xa3},
	{ 0x80, 0x81, 0x83, 0x84, 0x86, 0x87, 0x89, 0x8a, 0x8b, 0x8d, 0x8e, 0x8f, 0x90, 0x92, 0x93, 0x94,
	  0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9e, 0x9f, 0xa0, 0xa1, 0xa1, 0xa2},
	{ 0x80, 0x81, 0x83, 0x84, 0x86, 0x87, 0x88, 0x8a, 0x8b, 0x8c, 0x8e, 0x8f, 0x90, 0x91, 0x92, 0x93,
	  0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0x9f, 0xa0, 0xa1, 0xa1},
	{ 0x80, 0x81, 0x83, 0x84, 0x85, 0x87, 0x88, 0x89, 0x8b, 0x8c, 0x8d, 0x8e, 0x90, 0x91, 0x92, 0x93,
	  0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0x9b, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0x9f, 0xa0, 0xa1},
	{ 0x80, 0x81, 0x83, 0x84, 0x85, 0x87, 0x88, 0x89, 0x8a, 0x8c, 0x8d, 0x8e, 0x8f, 0x90, 0x91, 0x92,
	  0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9c, 0x9d, 0x9e, 0x9f, 0x9f, 0xa0}};

short ST010_Sin(short Theta)
{
  return ST010_SinTable[(Theta >> 8) & 0xff];
}

short ST010_Cos(short Theta)
{
  return ST010_SinTable[((Theta + 0x4000) >> 8) & 0xff];
}

void ST010_OP01(short x0, short y0, short *x1, short *y1, short *Quadrant, short *Theta)
{
  if ((x0 < 0) && (y0 < 0))
  {
    *x1 = -x0;
    *y1 = -y0;
    *Quadrant = -0x8000;
  }
  else if (x0 < 0)
  {
    *x1 = y0;
    *y1 = -x0;
    *Quadrant = -0x4000;
  }
  else if (y0 < 0)
  {
    *x1 = -y0;
    *y1 = x0;
    *Quadrant = 0x4000;
  }
  else
  {
    *x1 = x0;
    *y1 = y0;	
    *Quadrant = 0x0000;
  }

  while ((*x1 > 0x1f) || (*y1 > 0x1f))
  {
    if (*x1 > 1) *x1 >>= 1;
    if (*y1 > 1) *y1 >>= 1;
  }

  if (*y1 == 0) *Quadrant += 0x4000;

  *Theta = (ST010_ArcTan[*y1][*x1] << 8) ^ *Quadrant;
}

void ST010_Scale(short Multiplier, short X0, short Y0, int *X1, int *Y1)
{
  *X1 = X0 * Multiplier << 1;
  *Y1 = Y0 * Multiplier << 1;
}

void ST010_Multiply(short Multiplicand, short Multiplier, int *Product)
{
  *Product = Multiplicand * Multiplier << 1;
}

void ST010_Rotate(short Theta, short X0, short Y0, short *X1, short *Y1)
{
  *X1 = (Y0 * ST010_Sin(Theta) >> 15) + (X0 * ST010_Cos(Theta) >> 15);
  *Y1 = (Y0 * ST010_Cos(Theta) >> 15) - (X0 * ST010_Sin(Theta) >> 15);
}

void ST010_SortDrivers(uint16 Positions, uint16 Places[32], uint16 Drivers[32]) 
{
  bool Sorted;
  uint16 Temp;

  if (Positions > 1)
  {
    do
    {
      Sorted = true;
      int i;
      for (i = 0; i < Positions - 1; i++)
      {
        if (Places[i] < Places[i + 1]) 
        {
          Temp = Places[i + 1];
          Places[i + 1] = Places[i];
          Places[i] = Temp;
        
          Temp = Drivers[i + 1];
          Drivers[i + 1] = Drivers[i];
          Drivers[i] = Temp;

          Sorted = false;
        }
      }
      Positions--;
    } while (!Sorted);
  }
}

#define ST010_WORD(offset) (*((short *)(SRAM+offset)))
//#define ST010_WORD(offset) (SRAM[offset + 1] << 8) | SRAM[offset]

void ST010DoCommand(void)
{
  switch(SRAM[0x20])
  {
    /*
    Calculate track data based on direction coords
        
    Input
      0x0000-0x0001 : DX (signed)
      0x0002-0x0003 : DY (signed)
    Output
      0x0010-0x0011 : Angle (signed)
    */  

    case 0x01:
    {
      SRAM[0x0006] = SRAM[0x0002];
      SRAM[0x0007] = SRAM[0x0003];
      ST010_OP01(*(short*)&SRAM[0x0000], *(short*)&SRAM[0x0002], (short *)SRAM, (short *)&SRAM[2], (short *)&SRAM[4], (short *)&SRAM[0x10]);
    }
    break;

    //Sorts a bunch of values by weight
    
    case 0x02:
    {
      ST010_SortDrivers(*(short*)&SRAM[0x0024], (uint16*)&SRAM[0x0040], (uint16*)&SRAM[0x0080]);
    }
    break;
    
    /*
    Two Dimensional Coordinate Scale

    Input
      0x0000-0x0001 : X0 (signed)
      0x0002-0x0003 : Y0 (signed)
      0x0004-0x0005 : Multiplier (signed)
    Output
      0x0010-0x0013 : X1 (signed)
      0x0014-0x0017 : Y1 (signed)
    */  
    
    case 0x03:
    {
      ST010_Scale(*(short*)&SRAM[0x0004], *(short*)&SRAM[0x0000], *(short*)&SRAM[0x0002], (int *)&SRAM[0x10], (int *)&SRAM[0x14]);
    }
    break;

    //Calculate the vector length of (x,y)

    case 0x04:
    {
      int16 square, x,y;
      x=*((int16*)SRAM);
      y=*((int16*)&SRAM[2]);
      square=(int16)sqrt((double)(y*y+x*x));
      *((int16*)&SRAM[0x10])=square;
      break;
    }

    //Calculate AI orientation based on specific guidelines
    case 0x05:
    {
      int dx,dy;
      int16 a1,b1,c1;
      uint16 o1;

      bool wrap=false;

      //Target (x,y) coordinates
      int16 ypos_max = ST010_WORD(0x00C0);
      int16 xpos_max = ST010_WORD(0x00C2);
    
      //Current coordinates and direction
      int32 ypos = SRAM[0xC4]|(SRAM[0xC5]<<8)|(SRAM[0xC6]<<16)|(SRAM[0xC7]<<24);
      int32 xpos = SRAM[0xC8]|(SRAM[0xC9]<<8)|(SRAM[0xCA]<<16)|(SRAM[0xCB]<<24);
      uint16 rot = SRAM[0xCC]|(SRAM[0xCD]<<8);

      //Physics
      uint16 speed = ST010_WORD(0x00D4);
      uint16 accel = ST010_WORD(0x00D6);
      uint16 speed_max = ST010_WORD(0x00D8);

      //Special condition acknowledgment
      int16 system = ST010_WORD(0x00DA);
      int16 flags = ST010_WORD(0x00DC);

      //New target coordinates
      int16 ypos_new = ST010_WORD(0x00DE);
      int16 xpos_new = ST010_WORD(0x00E0);

      //Mask upper bit
      xpos_new &= 0x7FFF;

      //Get the current distance
      dx = xpos_max-(xpos>>16);
      dy = ypos_max-(ypos>>16);

      //Quirk: clear and move in9
      SRAM[0xD2]=0xFF;
      SRAM[0xD3]=0xFF;
      SRAM[0xDA]=0;
      SRAM[0xDB]=0;

      //Grab the target angle
      ST010_OP01(dy,dx,&a1,&b1,&c1,&o1);

      //Check for wrapping
      if (abs(o1-rot)>0x8000)
      {
        o1+=0x8000;
        rot+=0x8000;
        wrap=true;
      }

      uint16 old_speed;
      old_speed = speed;

      //Special case
      if (abs(o1-rot)==0x8000)
      {
        speed = 0x100;
      }
      
      //Slow down for sharp curves
      else if (abs(o1-rot)>=0x1000)
      {
        uint32 slow = abs(o1-rot);
        slow >>= 4; //Scaling
        speed -= slow;
      }
      //Otherwise accelerate
      else
      {
        speed += accel;
        if (speed > speed_max)
        {
          //Clip speed
          speed = speed_max;
        }
      }

      //Prevent negative/positive overflow
      if( abs(old_speed-speed)>0x8000)
      {
        if (old_speed<speed) { speed=0; }
        else { speed=0xff00; }
      }

      //Adjust direction by so many degrees
      //Be careful of negative adjustments
      if ((o1>rot && (o1-rot)>0x80) || (o1<rot && (rot-o1)>=0x80))
      {
        if (o1<rot) { rot-=0x280; }
        else if (o1>rot) { rot+=0x280; }
      }

      //Turn off wrapping
      if (wrap) { rot-=0x8000; }

      //Now check the distances (store for later)
      dx = (xpos_max<<16)-xpos;
      dy = (ypos_max<<16)-ypos;
      dx>>=16;
      dy>>=16;

      //If we're in so many units of the target, signal it
      if ((system && (dy<=6 && dy>=-8) && (dx<=126 && dx>=-128)) ||
          (!system && (dx<=6 && dx>=-8) && (dy<=126 && dy>=-128)))
      {
        //Announce our new destination and flag it
        xpos_max = xpos_new&0x7FFF;
        ypos_max = ypos_new;
        flags |= 0x08;
      }

      //Update position
      xpos -= (ST010_Cos(rot) * 0x400 >> 15) * (speed >> 8) << 1;
      ypos -= (ST010_Sin(rot) * 0x400 >> 15) * (speed >> 8) << 1;

      //Quirk: mask upper byte
      xpos &= 0x1FFFFFFF;
      ypos &= 0x1FFFFFFF;

      SRAM[0x00C0]=(uint8)(ypos_max);
      SRAM[0x00C1]=(uint8)(ypos_max >> 8);
      SRAM[0x00C2]=(uint8)(xpos_max);
      SRAM[0x00C3]=(uint8)(xpos_max >> 8);
      SRAM[0x00C4]=(uint8)(ypos);
      SRAM[0x00C5]=(uint8)(ypos >> 8);
      SRAM[0x00C6]=(uint8)(ypos >> 16);
      SRAM[0x00C7]=(uint8)(ypos >> 24);
      SRAM[0x00C8]=(uint8)(xpos);
      SRAM[0x00C9]=(uint8)(xpos >> 8);
      SRAM[0x00CA]=(uint8)(xpos >> 16);
      SRAM[0x00CB]=(uint8)(xpos >> 24);
      SRAM[0x00CC]=(uint8)(rot);
      SRAM[0x00CD]=(uint8)(rot >> 8);
      SRAM[0x00D4]=(uint8)(speed);
      SRAM[0x00D5]=(uint8)(speed >> 8);
      SRAM[0x00DC]=(uint8)(flags);
      SRAM[0x00DD]=(uint8)(flags >> 8);
    }
    break;
    
    /*
    16-bit Multiplication

    Input
      0x0000-0x0001 : Multiplcand (signed)
      0x0002-0x0003 : Multiplier (signed)
    Output
      0x0010-0x0013 : Product (signed)
    */

    case 0x06:
    {
      ST010_Multiply(*(short*)&SRAM[0x0000], *(short*)&SRAM[0x0002], (int *)&SRAM[0x10]);
    }
    break;

    /*    
    Mode 7 Raster Data Calculation

    Input
      0x0000-0x0001 : Angle (signed)
    Output
      0x00f0-0x024f : Mode 7 Matrix A
      0x0250-0x03af : Mode 7 Matrix B
      0x03b0-0x050f : Mode 7 Matrix C
      0x0510-0x066f : Mode 7 Matrix D
    */   

    case 0x07:
    {
      int16 data;
      int32 offset = 0;
      int16 Theta = ST010_WORD(0x0000);

      int32 line;
      for (line = 0; line < 176; line++)
      {
        //Calculate Mode 7 Matrix A/D data
        data = ST010_M7Scale[line] * ST010_Cos(Theta) >> 15;
        SRAM[0x00f0 + offset]=(uint8)(data);
        SRAM[0x00f1 + offset]=(uint8)(data >> 8);
        SRAM[0x0510 + offset]=(uint8)(data);
        SRAM[0x0511 + offset]=(uint8)(data >> 8); 

        //Calculate Mode 7 Matrix B/C data
        data = ST010_M7Scale[line] * ST010_Sin(Theta) >> 15;
        SRAM[0x0250 + offset]=(uint8)(data);
        SRAM[0x0251 + offset]=(uint8)(data >> 8);

        if (data) { data = ~data; }
        
        SRAM[0x03b0 + offset]=(uint8)(data);
        SRAM[0x03b1 + offset]=(uint8)(data >> 8);
        
        offset += 2;
      }

      //Shift Angle for use with Lookup table
      SRAM[0x00] = SRAM[0x01];
      SRAM[0x01] = 0x00;
    }
    break;

    /*
    Two dimensional Coordinate Rotation

    Input
      0x0000-0x0001 : X0 (signed)
      0x0002-0x0003 : Y0 (signed)
      0x0004-0x0005 : Angle (signed)
    Output
      0x0010-0x0011 : X1 (signed)
      0x0012-0x0013 : Y1 (signed)
    */
    
    case 0x08:
    {
      ST010_Rotate(*(short*)&SRAM[0x0004], *(short*)&SRAM[0x0000], *(short*)&SRAM[0x0002], (short *)&SRAM[0x10], (short *)&SRAM[0x12]);
    }
    break;

    default: break;
  }

  //Lower signal: op processed
  SRAM[0x20]=0;
  SRAM[0x21]=0;
}