seta010.cpp

/*****************************************************************************\
     Snes9x - Portable Super Nintendo Entertainment System (TM) emulator.
                This file is licensed under the Snes9x License.
   For further information, consult the LICENSE file in the root directory.
\*****************************************************************************/

#include <math.h>
#include "snes9x.h"
#include "memmap.h"
#include "seta.h"

static 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
};

static const uint8 ST010_ArcTan[32][32] =
{
	{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
	{ 0x40, 0x20, 0x13, 0x0D, 0x0A, 0x08, 0x07, 0x06, 0x05, 0x05, 0x04, 0x04, 0x03, 0x03, 0x03, 0x03,
	  0x03, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x01, 0x01, 0x01, 0x01 },
	{ 0x40, 0x2D, 0x20, 0x18, 0x13, 0x10, 0x0D, 0x0B, 0x0A, 0x09, 0x08, 0x07, 0x07, 0x06, 0x06, 0x05,
	  0x05, 0x05, 0x05, 0x04, 0x04, 0x04, 0x04, 0x04, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 },
	{ 0x40, 0x33, 0x28, 0x20, 0x1A, 0x16, 0x13, 0x10, 0x0F, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x09, 0x08,
	  0x08, 0x07, 0x07, 0x06, 0x06, 0x06, 0x06, 0x05, 0x05, 0x05, 0x05, 0x05, 0x04, 0x04, 0x04, 0x04 },
	{ 0x40, 0x36, 0x2D, 0x26, 0x20, 0x1B, 0x18, 0x15, 0x13, 0x11, 0x10, 0x0E, 0x0D, 0x0C, 0x0B, 0x0B,
	  0x0A, 0x09, 0x09, 0x08, 0x08, 0x08, 0x07, 0x07, 0x07, 0x06, 0x06, 0x06, 0x06, 0x06, 0x05, 0x05 },
	{ 0x40, 0x38, 0x30, 0x2A, 0x25, 0x20, 0x1C, 0x19, 0x17, 0x15, 0x13, 0x11, 0x10, 0x0F, 0x0E, 0x0D,
	  0x0C, 0x0C, 0x0B, 0x0A, 0x0A, 0x0A, 0x09, 0x09, 0x08, 0x08, 0x08, 0x07, 0x07, 0x07, 0x07, 0x07 },
	{ 0x40, 0x39, 0x33, 0x2D, 0x28, 0x24, 0x20, 0x1D, 0x1A, 0x18, 0x16, 0x14, 0x13, 0x12, 0x10, 0x10,
	  0x0F, 0x0E, 0x0D, 0x0C, 0x0C, 0x0B, 0x0B, 0x0A, 0x0A, 0x0A, 0x09, 0x09, 0x09, 0x08, 0x08, 0x08 },
	{ 0x40, 0x3A, 0x35, 0x30, 0x2B, 0x27, 0x23, 0x20, 0x1D, 0x1B, 0x19, 0x17, 0x16, 0x14, 0x13, 0x12,
	  0x11, 0x10, 0x0F, 0x0E, 0x0E, 0x0D, 0x0D, 0x0C, 0x0C, 0x0B, 0x0B, 0x0A, 0x0A, 0x0A, 0x09, 0x09 },
	{ 0x40, 0x3B, 0x36, 0x31, 0x2D, 0x29, 0x26, 0x23, 0x20, 0x1E, 0x1B, 0x1A, 0x18, 0x16, 0x15, 0x14,
	  0x13, 0x12, 0x11, 0x10, 0x10, 0x0F, 0x0E, 0x0E, 0x0D, 0x0D, 0x0C, 0x0C, 0x0B, 0x0B, 0x0B, 0x0A },
	{ 0x40, 0x3B, 0x37, 0x33, 0x2F, 0x2B, 0x28, 0x25, 0x22, 0x20, 0x1E, 0x1C, 0x1A, 0x19, 0x17, 0x16,
	  0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x10, 0x0F, 0x0F, 0x0E, 0x0E, 0x0D, 0x0D, 0x0C, 0x0C, 0x0C },
	{ 0x40, 0x3C, 0x38, 0x34, 0x30, 0x2D, 0x2A, 0x27, 0x25, 0x20, 0x20, 0x1E, 0x1C, 0x1B, 0x19, 0x18,
	  0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11, 0x11, 0x10, 0x10, 0x0F, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D },
	{ 0x40, 0x3C, 0x39, 0x35, 0x32, 0x2F, 0x2C, 0x29, 0x26, 0x24, 0x22, 0x20, 0x1E, 0x1D, 0x1B, 0x1A,
	  0x19, 0x17, 0x16, 0x15, 0x14, 0x14, 0x13, 0x12, 0x12, 0x11, 0x10, 0x10, 0x0F, 0x0F, 0x0E, 0x0E },
	{ 0x40, 0x3D, 0x39, 0x36, 0x33, 0x30, 0x2D, 0x2A, 0x28, 0x26, 0x24, 0x22, 0x20, 0x1E, 0x1D, 0x1B,
	  0x1A, 0x19, 0x18, 0x17, 0x16, 0x15, 0x14, 0x14, 0x13, 0x12, 0x12, 0x11, 0x10, 0x10, 0x10, 0x0F },
	{ 0x40, 0x3D, 0x3A, 0x37, 0x34, 0x31, 0x2E, 0x2C, 0x2A, 0x27, 0x25, 0x23, 0x22, 0x20, 0x1E, 0x1D,
	  0x1C, 0x1B, 0x19, 0x18, 0x17, 0x17, 0x16, 0x15, 0x14, 0x14, 0x13, 0x12, 0x12, 0x11, 0x11, 0x10 },
	{ 0x40, 0x3D, 0x3A, 0x37, 0x35, 0x32, 0x30, 0x2D, 0x2B, 0x29, 0x27, 0x25, 0x23, 0x22, 0x20, 0x1F,
	  0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x17, 0x16, 0x16, 0x15, 0x14, 0x13, 0x13, 0x12, 0x12, 0x11 },
	{ 0x40, 0x3D, 0x3B, 0x38, 0x35, 0x33, 0x30, 0x2E, 0x2C, 0x2A, 0x28, 0x26, 0x25, 0x23, 0x21, 0x20,
	  0x1F, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x18, 0x17, 0x16, 0x15, 0x15, 0x14, 0x13, 0x13, 0x12 },
	{ 0x40, 0x3D, 0x3B, 0x38, 0x36, 0x34, 0x31, 0x2F, 0x2D, 0x2B, 0x29, 0x27, 0x26, 0x24, 0x23, 0x21,
	  0x20, 0x1F, 0x1E, 0x1D, 0x1B, 0x1B, 0x1A, 0x19, 0x18, 0x17, 0x16, 0x16, 0x15, 0x15, 0x14, 0x13 },
	{ 0x40, 0x3E, 0x3B, 0x39, 0x37, 0x34, 0x32, 0x30, 0x2E, 0x2C, 0x2A, 0x29, 0x27, 0x25, 0x24, 0x23,
	  0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x18, 0x17, 0x16, 0x16, 0x15, 0x14 },
	{ 0x40, 0x3E, 0x3B, 0x39, 0x37, 0x35, 0x33, 0x31, 0x2F, 0x2D, 0x2B, 0x2A, 0x28, 0x27, 0x25, 0x24,
	  0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x19, 0x18, 0x17, 0x17, 0x16, 0x15 },
	{ 0x40, 0x3E, 0x3C, 0x3A, 0x38, 0x36, 0x34, 0x32, 0x30, 0x2E, 0x2C, 0x2B, 0x29, 0x28, 0x26, 0x25,
	  0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x1A, 0x19, 0x18, 0x18, 0x17, 0x16 },
	{ 0x40, 0x3E, 0x3C, 0x3A, 0x38, 0x36, 0x34, 0x32, 0x30, 0x2F, 0x2D, 0x2C, 0x2A, 0x29, 0x27, 0x26,
	  0x25, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1B, 0x1A, 0x19, 0x19, 0x18, 0x17 },
	{ 0x40, 0x3E, 0x3C, 0x3A, 0x38, 0x36, 0x35, 0x33, 0x31, 0x30, 0x2E, 0x2C, 0x2B, 0x29, 0x28, 0x27,
	  0x25, 0x24, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1C, 0x1C, 0x1B, 0x1A, 0x1A, 0x19, 0x18 },
	{ 0x40, 0x3E, 0x3C, 0x3A, 0x39, 0x37, 0x35, 0x33, 0x32, 0x30, 0x2F, 0x2D, 0x2C, 0x2A, 0x29, 0x28,
	  0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1D, 0x1C, 0x1B, 0x1A, 0x1A, 0x19 },
	{ 0x40, 0x3E, 0x3C, 0x3B, 0x39, 0x37, 0x36, 0x34, 0x32, 0x31, 0x2F, 0x2E, 0x2C, 0x2B, 0x2A, 0x28,
	  0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1E, 0x1D, 0x1C, 0x1B, 0x1B, 0x1A },
	{ 0x40, 0x3E, 0x3D, 0x3B, 0x39, 0x38, 0x36, 0x34, 0x33, 0x31, 0x30, 0x2E, 0x2D, 0x2C, 0x2A, 0x29,
	  0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1E, 0x1D, 0x1C, 0x1B, 0x1B },
	{ 0x40, 0x3E, 0x3D, 0x3B, 0x3A, 0x38, 0x36, 0x35, 0x33, 0x32, 0x30, 0x2F, 0x2E, 0x2C, 0x2B, 0x2A,
	  0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1E, 0x1D, 0x1C, 0x1C },
	{ 0x40, 0x3E, 0x3D, 0x3B, 0x3A, 0x38, 0x37, 0x35, 0x34, 0x32, 0x31, 0x30, 0x2E, 0x2D, 0x2C, 0x2B,
	  0x2A, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1E, 0x1D, 0x1C },
	{ 0x40, 0x3E, 0x3D, 0x3B, 0x3A, 0x39, 0x37, 0x36, 0x34, 0x33, 0x32, 0x30, 0x2F, 0x2E, 0x2D, 0x2B,
	  0x2A, 0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x22, 0x21, 0x20, 0x1F, 0x1F, 0x1E, 0x1D },
	{ 0x40, 0x3F, 0x3D, 0x3C, 0x3A, 0x39, 0x37, 0x36, 0x35, 0x33, 0x32, 0x31, 0x30, 0x2E, 0x2D, 0x2C,
	  0x2B, 0x2A, 0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x22, 0x21, 0x20, 0x1F, 0x1F, 0x1E },
	{ 0x40, 0x3F, 0x3D, 0x3C, 0x3A, 0x39, 0x38, 0x36, 0x35, 0x34, 0x32, 0x31, 0x30, 0x2F, 0x2E, 0x2D,
	  0x2B, 0x2A, 0x29, 0x28, 0x27, 0x26, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x21, 0x20, 0x1F, 0x1F },
	{ 0x40, 0x3F, 0x3D, 0x3C, 0x3B, 0x39, 0x38, 0x37, 0x35, 0x34, 0x33, 0x32, 0x30, 0x2F, 0x2E, 0x2D,
	  0x2C, 0x2B, 0x2A, 0x29, 0x28, 0x27, 0x26, 0x25, 0x25, 0x24, 0x23, 0x22, 0x21, 0x21, 0x20, 0x1F },
	{ 0x40, 0x3F, 0x3D, 0x3C, 0x3B, 0x39, 0x38, 0x37, 0x35, 0x34, 0x33, 0x32, 0x31, 0x30, 0x2F, 0x2E,
	  0x2D, 0x2C, 0x2B, 0x2A, 0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x24, 0x23, 0x22, 0x21, 0x21, 0x20 }
};

// Mode 7 scaling constants for all raster lines
static 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
};

#ifndef PI
#define PI	3.1415926535897932384626433832795
#endif

#define ST010_WORD(offset)	(Memory.SRAM[offset + 1] << 8) | Memory.SRAM[offset]
#define ST010_DWORD(offset)	(Memory.SRAM[offset + 3] << 24) | (Memory.SRAM[offset + 2] << 16) | (Memory.SRAM[offset + 1] << 8) | Memory.SRAM[offset]


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

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

void ST010_Compass(int16 x0, int16 y0, int16 &x1, int16 &y1, int16 &Quadrant, int16 &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;
	}

	Theta = ST010_ArcTan[x1 & 0x1f][y1 & 0x1f] << 8;
	Theta = (Theta | Quadrant) ^ 0x8000;

	if ((x0 == 0) && (y0 < 0)) Quadrant = 0x4000;
}

static void ST010_Scale (int16 Multiplier, int16 X0, int16 Y0, int32 &X1, int32 &Y1)
{
	X1 = X0 * Multiplier << 1;
	Y1 = Y0 * Multiplier << 1;
}

static void ST010_Multiply (int16 Multiplicand, int16 Multiplier, int32 &Product)
{
	Product = Multiplicand * Multiplier << 1;
}

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

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

	if (Positions > 1)
	{
		do
		{
			Sorted = true;

			for (int 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);
	}
}

static void ST010_Raster(int16 Theta)
{
	int16 data;
	int offset = 0;

	for (int i = 0; i < 176; i++)
	{
		data = ST010_M7Scale[i] * ST010_Cos(Theta) >> 15;

		Memory.SRAM[0x00f0 + offset] = data; 
		Memory.SRAM[0x00f1 + offset] = data >> 8;

		Memory.SRAM[0x0510 + offset] = data; 
		Memory.SRAM[0x0511 + offset] = data >> 8; 
		
		data = ST010_M7Scale[i] * ST010_Sin(Theta) >> 15;  
		
		Memory.SRAM[0x0250 + offset] = data;		
		Memory.SRAM[0x0251 + offset] = data >> 8;
		
		if (data) data = ~data;
				 
		Memory.SRAM[0x03b0 + offset] = data;		
		Memory.SRAM[0x03b1 + offset] = data >> 8;
		
		offset += 2;
	}
}

static void ST010_Distance(int16 x0, int16 y0, int16 &Distance)
{
	int32 Product;
	
	x0 = (x0 < 0) ? -x0 : x0;
	y0 = (y0 < 0) ? -y0 : y0;

	if((uint16) x0 >= ((uint16) y0))
		Product = ((x0 * 0x3d78 << 1) + (y0 * 0x1976 << 1)) << 1;
	else
		Product = ((y0 * 0x3d78 << 1) + (x0 * 0x1976 << 1)) << 1;

	Distance = (Product + 0x8000) >> 16;
}

static void ST010_Navigation(int16 &MaxX, int16 &MaxY, int32 &x0, int32 &y0, int16 &Theta0, int16 &Theta1, int16 &x1, int16 &y1, uint16 &Radius, uint16 Increment, uint16 MaxRadius, int16 &Compass, int16 &Flags, int16 NewMaxX, int16 NewMaxY)
{
	int16	dummy1,dummy2,dummy3;
	uint16 utemp16;
	int32 temp32;


	x1 = MaxX - (x0 >> 16);
	y1 = MaxY - (y0 >> 16);

	ST010_Compass(x1, y1, dummy1, dummy2, dummy3, Theta1);
	Theta1 -= Theta0;

	if (Theta1 & 0xff00)
		Theta0 += (Theta1 & 0x8000) ? 0xfd80 : 0x0280;

	// compiler notice: -0x8000 ==> +0x8000
	utemp16 = ((Theta1 < 0) ? (int16) -Theta1 : Theta1) >> 4;

	if (utemp16 < 0x0100)
	{
		temp32 = Radius + Increment;
		Radius = (temp32 >= MaxRadius) ? MaxRadius : (uint16) temp32;
	}

	else
	{
		temp32 = Radius - utemp16;
		Radius = (temp32 <= 0) ? 0 : (uint16) temp32;
	}

	x0 -= ((ST010_Sin(Theta0) >> 5) * (Radius >> 8)) << 1;
	y0 -= ((ST010_Cos(Theta0) >> 5) * (Radius >> 8)) << 1;

	x0 &= 0x1fffffff;
	y0 &= 0x1fffffff;


	int16 MaxRadiusX, MaxRadiusY;
	if (Compass & 0x8000)
	{
		MaxRadiusX = 0x0008;
		MaxRadiusY = 0x0080;
	}
	else
	{
		MaxRadiusX = 0x0080;
		MaxRadiusY = 0x0008;
	}

	if ((abs(x1) < MaxRadiusX) && (abs(y1) < MaxRadiusY))
	{
		MaxX = NewMaxX;
		MaxY = NewMaxY & 0x0fff;
		Compass = (NewMaxY & 0x8000) ? 0xffff : 0x0000;
		Flags |= 0x0008;
	}
}

uint8 S9xGetST010 (uint32 Address)
{
	if (!(Address & 0x80000))
		return (0x80);

	if ((Address & 0xFFF) == 0x20)
		return (ST010.op_reg);

	if ((Address & 0xFFF) == 0x21)
		return (ST010.execute);

	return (Memory.SRAM[Address & Memory.SRAMMask]);
}

void S9xSetST010 (uint32 Address, uint8 Byte)
{
	if (!(Address & 0x80000))
	{
		ST010.control_enable = TRUE;
		return;
	}

#ifdef DEBUGGER
	printf("Write %06X:%02X\n", Address, Byte);
#endif

	if ((Address & 0xFFF) == 0x20 && ST010.control_enable)
		ST010.op_reg = Byte;

	if ((Address & 0xFFF) == 0x21 && ST010.control_enable)
		ST010.execute = Byte;
	else
		Memory.SRAM[Address & Memory.SRAMMask] = Byte;

	if (ST010.execute & 0x80)
	{
		switch (ST010.op_reg)
		{
			// Heading
			case 0x01:
			{
				Memory.SRAM[0x0006] = Memory.SRAM[0x0002];
				Memory.SRAM[0x0007] = Memory.SRAM[0x0003];

			#ifdef FAST_LSB_WORD_ACCESS
				ST010_Compass(*(int16 *) &Memory.SRAM[0x0000], *(int16 *) &Memory.SRAM[0x0002], (int16 &) Memory.SRAM[0x0000], (int16 &) Memory.SRAM[0x0002], (int16 &) Memory.SRAM[0x0004], (int16 &) Memory.SRAM[0x0010]);
			#else
				int16	x1, y1, Quadrant, Theta;

				ST010_Compass(ST010_WORD(0x0000), ST010_WORD(0x0002), x1, y1, Quadrant, Theta);

				Memory.SRAM[0x0000] = (uint8) (x1);
				Memory.SRAM[0x0001] = (uint8) (x1 >> 8);
				Memory.SRAM[0x0002] = (uint8) (y1);
				Memory.SRAM[0x0003] = (uint8) (y1 >> 8);
				Memory.SRAM[0x0004] = (uint8) (Quadrant);
				Memory.SRAM[0x0005] = (uint8) (Quadrant >> 8);
				Memory.SRAM[0x0010] = (uint8) (Theta);
				Memory.SRAM[0x0011] = (uint8) (Theta >> 8);
			#endif
				break;
			}

			// Sorts Driver Placements
			case 0x02:
			{
			#ifdef FAST_LSB_WORD_ACCESS
				ST010_SortDrivers(*(uint16 *) (Memory.SRAM + 0x0024), (uint16 *) (Memory.SRAM + 0x0040), (uint16 *) (Memory.SRAM + 0x0080));
			#else
				uint16	Places[32];
				uint16	Positions = ST010_WORD(0x0024);
				int		Pos, Offset;

				Offset = 0;

				for (Pos = 0; Pos < Positions; Pos++)
				{
					Places[Pos] = ST010_WORD(0x0040 + Offset);
					Offset += 2;
				}

				ST010_SortDrivers(Positions, Places, (uint16 *) (Memory.SRAM + 0x0080));

				Offset = 0;

				for (Pos = 0; Pos < Positions; Pos++)
				{
					Memory.SRAM[0x0040 + Offset] = (uint8) (Places[Pos]);
					Memory.SRAM[0x0041 + Offset] = (uint8) (Places[Pos] >> 8);
					Offset += 2;
				}
			#endif
				break;
			}

			// Two Dimensional Coordinate Scale
			case 0x03:
			{
			#ifdef FAST_LSB_WORD_ACCESS
				ST010_Scale(*(int16 *) &Memory.SRAM[0x0004], *(int16 *) &Memory.SRAM[0x0000], *(int16 *) &Memory.SRAM[0x0002], (int32 &) Memory.SRAM[0x0010], (int32 &) Memory.SRAM[0x0014]);
			#else
				int32	x1, y1;

				ST010_Scale(ST010_WORD(0x0004), ST010_WORD(0x0000), ST010_WORD(0x0002), x1, y1);

				Memory.SRAM[0x0010] = (uint8) (x1);
				Memory.SRAM[0x0011] = (uint8) (x1 >> 8);
				Memory.SRAM[0x0012] = (uint8) (x1 >> 16);
				Memory.SRAM[0x0013] = (uint8) (x1 >> 24);
				Memory.SRAM[0x0014] = (uint8) (y1);
				Memory.SRAM[0x0015] = (uint8) (y1 >> 8);
				Memory.SRAM[0x0016] = (uint8) (y1 >> 16);
				Memory.SRAM[0x0017] = (uint8) (y1 >> 24);
			#endif
				break;
			}

			// calculate the vector length of (x, y)
			case 0x04:
			{
			#ifdef FAST_LSB_WORD_ACCESS
				ST010_Distance(*(int16 *) &Memory.SRAM[0x0000], *(int16 *) &Memory.SRAM[0x0002], (int16 &) Memory.SRAM[0x0010]);
			#else
				int16	square;

				ST010_Distance(ST010_WORD(0x0000), ST010_WORD(0x0002), square);

				Memory.SRAM[0x10] = (uint8) (square);
				Memory.SRAM[0x11] = (uint8) (square >> 8);
			#endif
				break;
			}

			// calculate AI orientation based on specific guidelines
			case 0x05:
			{
			#ifdef FAST_LSB_WORD_ACCESS
				ST010_Navigation((int16 &) Memory.SRAM[0x00c0], (int16 &) Memory.SRAM[0x00c2], (int32 &) Memory.SRAM[0x00c4], (int32 &) Memory.SRAM[0x00c8], (int16 &) Memory.SRAM[0x00cc], (int16 &) Memory.SRAM[0x00ce], (int16 &) Memory.SRAM[0x00d0], (int16 &) Memory.SRAM[0x00d2], (uint16 &) Memory.SRAM[0x00d4], *(uint16 *) &Memory.SRAM[0x00d6], *(uint16 *) &Memory.SRAM[0x00d8], (int16 &) Memory.SRAM[0x00da], (int16 &) Memory.SRAM[0x00dc], *(int16 *) &Memory.SRAM[0x00de], *(int16 *) &Memory.SRAM[0x00e0]);
			#else
				int32 x0,y0;
				int16 MaxX,MaxY,Theta0,Theta1,x1,y1,Compass,Flags;
				uint16 Radius;

				MaxX = ST010_WORD(0x00c0);
				MaxY = ST010_WORD(0x00c2);
				x0 = ST010_DWORD(0x00c4);
				y0 = ST010_DWORD(0x00c8);
				Theta0 = ST010_WORD(0x00cc);
				Radius = ST010_WORD(0x00d4);
				Compass = ST010_WORD(0x00da);
				Flags = ST010_WORD(0x00dc);

				ST010_Navigation(MaxX, MaxY, x0, y0, Theta0, Theta1, x1, y1, Radius, ST010_WORD(0x00d6), ST010_WORD(0x00d8), Compass, Flags, ST010_WORD(0x00de), ST010_WORD(0x00e0));

				Memory.SRAM[0x00c0] = (uint8) (MaxX);
				Memory.SRAM[0x00c1] = (uint8) (MaxX >> 8);
				Memory.SRAM[0x00c2] = (uint8) (MaxY);
				Memory.SRAM[0x00c3] = (uint8) (MaxY >> 8);
				Memory.SRAM[0x00c4] = (uint8) (x0);
				Memory.SRAM[0x00c5] = (uint8) (x0 >> 8);
				Memory.SRAM[0x00c6] = (uint8) (x0 >> 16);
				Memory.SRAM[0x00c7] = (uint8) (x0 >> 24);
				Memory.SRAM[0x00c8] = (uint8) (y0);
				Memory.SRAM[0x00c9] = (uint8) (y0 >> 8);
				Memory.SRAM[0x00ca] = (uint8) (y0 >> 16);
				Memory.SRAM[0x00cb] = (uint8) (y0 >> 24);
				Memory.SRAM[0x00cc] = (uint8) (Theta0);
				Memory.SRAM[0x00cd] = (uint8) (Theta0 >> 8);
				Memory.SRAM[0x00ce] = (uint8) (Theta1);
				Memory.SRAM[0x00cf] = (uint8) (Theta1 >> 8);
				Memory.SRAM[0x00d0] = (uint8) (x1);
				Memory.SRAM[0x00d1] = (uint8) (x1 >> 8);
				Memory.SRAM[0x00d2] = (uint8) (y1);
				Memory.SRAM[0x00d3] = (uint8) (y1 >> 8);
				Memory.SRAM[0x00d4] = (uint8) (Radius);
				Memory.SRAM[0x00d5] = (uint8) (Radius >> 8);
				Memory.SRAM[0x00da] = (uint8) (Compass);
				Memory.SRAM[0x00db] = (uint8) (Compass >> 8);
				Memory.SRAM[0x00dc] = (uint8) (Flags);
				Memory.SRAM[0x00dd] = (uint8) (Flags >> 8);
			#endif
				break;
			}

			// 16-bit Multiplication
			case 0x06:
			{
			#ifdef FAST_LSB_WORD_ACCESS
				ST010_Multiply(*(int16 *) &Memory.SRAM[0x0000], *(int16 *) &Memory.SRAM[0x0002], (int32 &) Memory.SRAM[0x0010]);
			#else
				int32	Product;

				ST010_Multiply(ST010_WORD(0x0000), ST010_WORD(0x0002), Product);

				Memory.SRAM[0x0010] = (uint8) (Product);
				Memory.SRAM[0x0011] = (uint8) (Product >> 8);
				Memory.SRAM[0x0012] = (uint8) (Product >> 16);
				Memory.SRAM[0x0013] = (uint8) (Product >> 24);
			#endif
				break;
			}

			// Mode 7 Raster Data Calculation
			case 0x07:
			{
			#ifdef FAST_LSB_WORD_ACCESS
				ST010_Raster(*(int16 *) &Memory.SRAM[0x0000]);
			#else
				ST010_Raster(ST010_WORD(0x0000));
			#endif

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

				break;
			}

			// Two dimensional Coordinate Rotation
			case 0x08:
			{
			#ifdef FAST_LSB_WORD_ACCESS
				ST010_Rotate(*(int16 *) &Memory.SRAM[0x0004], *(int16 *) &Memory.SRAM[0x0000], *(int16 *) &Memory.SRAM[0x0002], (int16 &) Memory.SRAM[0x0010], (int16 &) Memory.SRAM[0x0012]);
			#else
				int16	x1, y1;

				ST010_Rotate(ST010_WORD(0x0004), ST010_WORD(0x0000), ST010_WORD(0x0002), x1, y1);

				Memory.SRAM[0x0010] = (uint8) (x1);
				Memory.SRAM[0x0011] = (uint8) (x1 >> 8);
				Memory.SRAM[0x0012] = (uint8) (y1);
				Memory.SRAM[0x0013] = (uint8) (y1 >> 8);
			#endif
				break;
			}

			default:
			#ifdef DEBUGGER
				printf("Unknown Op\n");
			#endif
				break;
		}

		// lower signal: op processed
		ST010.op_reg  = 0;
		ST010.execute = 0;
	}
}