/* Copyright (c) 2013-2015 Jeffrey Pfau
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
#include <mgba/internal/gba/hardware.h>

#include <mgba/internal/arm/macros.h>
#include <mgba/internal/gba/io.h>
#include <mgba/internal/gba/serialize.h>
#include <mgba-util/formatting.h>
#include <mgba-util/hash.h>
#include <mgba-util/memory.h>

#define EREADER_BLOCK_SIZE 40
#define EREADER_DOTCODE_STRIDE 1200
#define EREADER_DOTCODE_SIZE (EREADER_DOTCODE_STRIDE * 40 + 200)

mLOG_DEFINE_CATEGORY(GBA_HW, "GBA Pak Hardware", "gba.hardware");

MGBA_EXPORT const int GBA_LUX_LEVELS[10] = { 5, 11, 18, 27, 42, 62, 84, 109, 139, 183 };
const uint16_t EREADER_ADDRESS_CODES[] = {
	1023,
	1174,
	2628,
	3373,
	4233,
	6112,
	6450,
	7771,
	8826,
	9491,
	11201,
	11432,
	12556,
	13925,
	14519,
	16350,
	16629,
	18332,
	18766,
	20007,
	21379,
	21738,
	23096,
	23889,
	24944,
	26137,
	26827,
	28578,
	29190,
	30063,
	31677,
	31956,
	33410,
	34283,
	35641,
	35920,
	37364,
	38557,
	38991,
	40742,
	41735,
	42094,
	43708,
	44501,
	45169,
	46872,
	47562,
	48803,
	49544,
	50913,
	51251,
	53082,
	54014,
	54679
};

const int EREADER_NYBBLE_5BIT[16][5] = {
	{ 0, 0, 0, 0, 0 },
	{ 0, 0, 0, 0, 1 },
	{ 0, 0, 0, 1, 0 },
	{ 1, 0, 0, 1, 0 },
	{ 0, 0, 1, 0, 0 },
	{ 0, 0, 1, 0, 1 },
	{ 0, 0, 1, 1, 0 },
	{ 1, 0, 1, 1, 0 },
	{ 0, 1, 0, 0, 0 },
	{ 0, 1, 0, 0, 1 },
	{ 0, 1, 0, 1, 0 },
	{ 1, 0, 1, 0, 0 },
	{ 0, 1, 1, 0, 0 },
	{ 0, 1, 1, 0, 1 },
	{ 1, 0, 0, 0, 1 },
	{ 1, 0, 0, 0, 0 }
};

const uint8_t EREADER_CALIBRATION_TEMPLATE[] = {
	0x43, 0x61, 0x72, 0x64, 0x2d, 0x45, 0x20, 0x52, 0x65, 0x61, 0x64, 0x65, 0x72, 0x20, 0x32, 0x30,
	0x30, 0x31, 0x00, 0x00, 0xcf, 0x72, 0x2f, 0x37, 0x3a, 0x3a, 0x3a, 0x38, 0x33, 0x30, 0x30, 0x37,
	0x3a, 0x39, 0x37, 0x35, 0x33, 0x2f, 0x2f, 0x34, 0x36, 0x36, 0x37, 0x36, 0x34, 0x31, 0x2d, 0x30,
	0x32, 0x34, 0x35, 0x35, 0x34, 0x30, 0x2a, 0x2d, 0x2d, 0x2f, 0x31, 0x32, 0x31, 0x2f, 0x29, 0x2a,
	0x2c, 0x2b, 0x2c, 0x2e, 0x2e, 0x2d, 0x18, 0x2d, 0x8f, 0x03, 0x00, 0x00, 0xc0, 0xfd, 0x77, 0x00,
	0x00, 0x00, 0x01
};

static void _readPins(struct GBACartridgeHardware* hw);
static void _outputPins(struct GBACartridgeHardware* hw, unsigned pins);

static void _rtcReadPins(struct GBACartridgeHardware* hw);
static unsigned _rtcOutput(struct GBACartridgeHardware* hw);
static void _rtcProcessByte(struct GBACartridgeHardware* hw);
static void _rtcUpdateClock(struct GBACartridgeHardware* hw);
static unsigned _rtcBCD(unsigned value);

static time_t _rtcGenericCallback(struct mRTCSource* source);

static void _gyroReadPins(struct GBACartridgeHardware* hw);

static void _rumbleReadPins(struct GBACartridgeHardware* hw);

static void _lightReadPins(struct GBACartridgeHardware* hw);

static uint16_t _gbpRead(struct mKeyCallback*);
static uint16_t _gbpSioWriteRegister(struct GBASIODriver* driver, uint32_t address, uint16_t value);
static void _gbpSioProcessEvents(struct mTiming* timing, void* user, uint32_t cyclesLate);

static void _eReaderReset(struct GBACartridgeHardware* hw);
static void _eReaderWriteControl0(struct GBACartridgeHardware* hw, uint8_t value);
static void _eReaderWriteControl1(struct GBACartridgeHardware* hw, uint8_t value);
static void _eReaderReadData(struct GBACartridgeHardware* hw);

static const int RTC_BYTES[8] = {
	0, // Force reset
	0, // Empty
	7, // Date/Time
	0, // Force IRQ
	1, // Control register
	0, // Empty
	3, // Time
	0 // Empty
};

void GBAHardwareInit(struct GBACartridgeHardware* hw, uint16_t* base) {
	hw->gpioBase = base;
	hw->eReaderDots = NULL;
	GBAHardwareClear(hw);

	hw->gbpCallback.d.readKeys = _gbpRead;
	hw->gbpCallback.p = hw;
	hw->gbpDriver.d.init = 0;
	hw->gbpDriver.d.deinit = 0;
	hw->gbpDriver.d.load = 0;
	hw->gbpDriver.d.unload = 0;
	hw->gbpDriver.d.writeRegister = _gbpSioWriteRegister;
	hw->gbpDriver.p = hw;
	hw->gbpNextEvent.context = &hw->gbpDriver;
	hw->gbpNextEvent.name = "GBA SIO Game Boy Player";
	hw->gbpNextEvent.callback = _gbpSioProcessEvents;
	hw->gbpNextEvent.priority = 0x80;
}

void GBAHardwareClear(struct GBACartridgeHardware* hw) {
	hw->devices = HW_NONE | (hw->devices & HW_GB_PLAYER_DETECTION);
	hw->readWrite = GPIO_WRITE_ONLY;
	hw->pinState = 0;
	hw->direction = 0;

	if (hw->eReaderDots) {
		mappedMemoryFree(hw->eReaderDots, EREADER_DOTCODE_SIZE);
		hw->eReaderDots = NULL;
	}

	if (hw->p->sio.drivers.normal == &hw->gbpDriver.d) {
		GBASIOSetDriver(&hw->p->sio, 0, SIO_NORMAL_32);
	}
}

void GBAHardwareGPIOWrite(struct GBACartridgeHardware* hw, uint32_t address, uint16_t value) {
	if (!hw->gpioBase) {
		return;
	}
	switch (address) {
	case GPIO_REG_DATA:
		hw->pinState &= ~hw->direction;
		hw->pinState |= value & hw->direction;
		_readPins(hw);
		break;
	case GPIO_REG_DIRECTION:
		hw->direction = value;
		break;
	case GPIO_REG_CONTROL:
		hw->readWrite = value;
		break;
	default:
		mLOG(GBA_HW, WARN, "Invalid GPIO address");
	}
	if (hw->readWrite) {
		STORE_16(hw->pinState, 0, hw->gpioBase);
		STORE_16(hw->direction, 2, hw->gpioBase);
		STORE_16(hw->readWrite, 4, hw->gpioBase);
	} else {
		hw->gpioBase[0] = 0;
		hw->gpioBase[1] = 0;
		hw->gpioBase[2] = 0;
	}
}

void GBAHardwareInitRTC(struct GBACartridgeHardware* hw) {
	hw->devices |= HW_RTC;
	hw->rtc.bytesRemaining = 0;

	hw->rtc.transferStep = 0;

	hw->rtc.bitsRead = 0;
	hw->rtc.bits = 0;
	hw->rtc.commandActive = 0;
	hw->rtc.command = 0;
	hw->rtc.control = 0x40;
	memset(hw->rtc.time, 0, sizeof(hw->rtc.time));
}

void _readPins(struct GBACartridgeHardware* hw) {
	if (hw->devices & HW_RTC) {
		_rtcReadPins(hw);
	}

	if (hw->devices & HW_GYRO) {
		_gyroReadPins(hw);
	}

	if (hw->devices & HW_RUMBLE) {
		_rumbleReadPins(hw);
	}

	if (hw->devices & HW_LIGHT_SENSOR) {
		_lightReadPins(hw);
	}
}

void _outputPins(struct GBACartridgeHardware* hw, unsigned pins) {
	if (hw->readWrite) {
		uint16_t old;
		LOAD_16(old, 0, hw->gpioBase);
		old &= hw->direction;
		hw->pinState = old | (pins & ~hw->direction & 0xF);
		STORE_16(hw->pinState, 0, hw->gpioBase);
	}
}

// == RTC

void _rtcReadPins(struct GBACartridgeHardware* hw) {
	// Transfer sequence:
	// P: 0 | 1 |  2 | 3
	// == Initiate
	// > HI | - | LO | -
	// > HI | - | HI | -
	// == Transfer bit (x8)
	// > LO | x | HI | -
	// > HI | - | HI | -
	// < ?? | x | ?? | -
	// == Terminate
	// >  - | - | LO | -
	switch (hw->rtc.transferStep) {
	case 0:
		if ((hw->pinState & 5) == 1) {
			hw->rtc.transferStep = 1;
		}
		break;
	case 1:
		if ((hw->pinState & 5) == 5) {
			hw->rtc.transferStep = 2;
		} else if ((hw->pinState & 5) != 1) {
			hw->rtc.transferStep = 0;
		}
		break;
	case 2:
		if (!(hw->pinState & 1)) {
			hw->rtc.bits &= ~(1 << hw->rtc.bitsRead);
			hw->rtc.bits |= ((hw->pinState & 2) >> 1) << hw->rtc.bitsRead;
		} else {
			if (hw->pinState & 4) {
				if (!RTCCommandDataIsReading(hw->rtc.command)) {
					++hw->rtc.bitsRead;
					if (hw->rtc.bitsRead == 8) {
						_rtcProcessByte(hw);
					}
				} else {
					_outputPins(hw, 5 | (_rtcOutput(hw) << 1));
					++hw->rtc.bitsRead;
					if (hw->rtc.bitsRead == 8) {
						--hw->rtc.bytesRemaining;
						if (hw->rtc.bytesRemaining <= 0) {
							hw->rtc.commandActive = 0;
							hw->rtc.command = 0;
						}
						hw->rtc.bitsRead = 0;
					}
				}
			} else {
				hw->rtc.bitsRead = 0;
				hw->rtc.bytesRemaining = 0;
				hw->rtc.commandActive = 0;
				hw->rtc.command = 0;
				hw->rtc.transferStep = hw->pinState & 1;
				_outputPins(hw, 1);
			}
		}
		break;
	}
}

void _rtcProcessByte(struct GBACartridgeHardware* hw) {
	--hw->rtc.bytesRemaining;
	if (!hw->rtc.commandActive) {
		RTCCommandData command;
		command = hw->rtc.bits;
		if (RTCCommandDataGetMagic(command) == 0x06) {
			hw->rtc.command = command;

			hw->rtc.bytesRemaining = RTC_BYTES[RTCCommandDataGetCommand(command)];
			hw->rtc.commandActive = hw->rtc.bytesRemaining > 0;
			switch (RTCCommandDataGetCommand(command)) {
			case RTC_RESET:
				hw->rtc.control = 0;
				break;
			case RTC_DATETIME:
			case RTC_TIME:
				_rtcUpdateClock(hw);
				break;
			case RTC_FORCE_IRQ:
			case RTC_CONTROL:
				break;
			}
		} else {
			mLOG(GBA_HW, WARN, "Invalid RTC command byte: %02X", hw->rtc.bits);
		}
	} else {
		switch (RTCCommandDataGetCommand(hw->rtc.command)) {
		case RTC_CONTROL:
			hw->rtc.control = hw->rtc.bits;
			break;
		case RTC_FORCE_IRQ:
			mLOG(GBA_HW, STUB, "Unimplemented RTC command %u", RTCCommandDataGetCommand(hw->rtc.command));
			break;
		case RTC_RESET:
		case RTC_DATETIME:
		case RTC_TIME:
			break;
		}
	}

	hw->rtc.bits = 0;
	hw->rtc.bitsRead = 0;
	if (!hw->rtc.bytesRemaining) {
		hw->rtc.commandActive = 0;
		hw->rtc.command = 0;
	}
}

unsigned _rtcOutput(struct GBACartridgeHardware* hw) {
	uint8_t outputByte = 0;
	if (!hw->rtc.commandActive) {
		mLOG(GBA_HW, GAME_ERROR, "Attempting to use RTC without an active command");
		return 0;
	}
	switch (RTCCommandDataGetCommand(hw->rtc.command)) {
	case RTC_CONTROL:
		outputByte = hw->rtc.control;
		break;
	case RTC_DATETIME:
	case RTC_TIME:
		outputByte = hw->rtc.time[7 - hw->rtc.bytesRemaining];
		break;
	case RTC_FORCE_IRQ:
	case RTC_RESET:
		break;
	}
	unsigned output = (outputByte >> hw->rtc.bitsRead) & 1;
	return output;
}

void _rtcUpdateClock(struct GBACartridgeHardware* hw) {
	time_t t;
	struct mRTCSource* rtc = hw->p->rtcSource;
	if (rtc) {
		if (rtc->sample) {
			rtc->sample(rtc);
		}
		t = rtc->unixTime(rtc);
	} else {
		t = time(0);
	}
	struct tm date;
	localtime_r(&t, &date);
	hw->rtc.time[0] = _rtcBCD(date.tm_year - 100);
	hw->rtc.time[1] = _rtcBCD(date.tm_mon + 1);
	hw->rtc.time[2] = _rtcBCD(date.tm_mday);
	hw->rtc.time[3] = _rtcBCD(date.tm_wday);
	if (RTCControlIsHour24(hw->rtc.control)) {
		hw->rtc.time[4] = _rtcBCD(date.tm_hour);
	} else {
		hw->rtc.time[4] = _rtcBCD(date.tm_hour % 12);
	}
	hw->rtc.time[5] = _rtcBCD(date.tm_min);
	hw->rtc.time[6] = _rtcBCD(date.tm_sec);
}

unsigned _rtcBCD(unsigned value) {
	int counter = value % 10;
	value /= 10;
	counter += (value % 10) << 4;
	return counter;
}

time_t _rtcGenericCallback(struct mRTCSource* source) {
	struct GBARTCGenericSource* rtc = (struct GBARTCGenericSource*) source;
	switch (rtc->override) {
	case RTC_NO_OVERRIDE:
	default:
		return time(0);
	case RTC_FIXED:
		return rtc->value;
	case RTC_FAKE_EPOCH:
		return rtc->value + rtc->p->video.frameCounter * (int64_t) VIDEO_TOTAL_LENGTH / GBA_ARM7TDMI_FREQUENCY;
	}
}

void GBARTCGenericSourceInit(struct GBARTCGenericSource* rtc, struct GBA* gba) {
	rtc->p = gba;
	rtc->override = RTC_NO_OVERRIDE;
	rtc->value = 0;
	rtc->d.sample = 0;
	rtc->d.unixTime = _rtcGenericCallback;
}

// == Gyro

void GBAHardwareInitGyro(struct GBACartridgeHardware* hw) {
	hw->devices |= HW_GYRO;
	hw->gyroSample = 0;
	hw->gyroEdge = 0;
}

void _gyroReadPins(struct GBACartridgeHardware* hw) {
	struct mRotationSource* gyro = hw->p->rotationSource;
	if (!gyro || !gyro->readGyroZ) {
		return;
	}

	if (hw->pinState & 1) {
		if (gyro->sample) {
			gyro->sample(gyro);
		}
		int32_t sample = gyro->readGyroZ(gyro);

		// Normalize to ~12 bits, focused on 0x6C0
		hw->gyroSample = (sample >> 21) + 0x6C0; // Crop off an extra bit so that we can't go negative
	}

	if (hw->gyroEdge && !(hw->pinState & 2)) {
		// Write bit on falling edge
		unsigned bit = hw->gyroSample >> 15;
		hw->gyroSample <<= 1;
		_outputPins(hw, bit << 2);
	}

	hw->gyroEdge = !!(hw->pinState & 2);
}

// == Rumble

void GBAHardwareInitRumble(struct GBACartridgeHardware* hw) {
	hw->devices |= HW_RUMBLE;
}

void _rumbleReadPins(struct GBACartridgeHardware* hw) {
	struct mRumble* rumble = hw->p->rumble;
	if (!rumble) {
		return;
	}

	rumble->setRumble(rumble, !!(hw->pinState & 8));
}

// == Light sensor

void GBAHardwareInitLight(struct GBACartridgeHardware* hw) {
	hw->devices |= HW_LIGHT_SENSOR;
	hw->lightCounter = 0;
	hw->lightEdge = false;
	hw->lightSample = 0xFF;
}

void _lightReadPins(struct GBACartridgeHardware* hw) {
	if (hw->pinState & 4) {
		// Boktai chip select
		return;
	}
	if (hw->pinState & 2) {
		struct GBALuminanceSource* lux = hw->p->luminanceSource;
		mLOG(GBA_HW, DEBUG, "[SOLAR] Got reset");
		hw->lightCounter = 0;
		if (lux) {
			lux->sample(lux);
			hw->lightSample = lux->readLuminance(lux);
		} else {
			hw->lightSample = 0xFF;
		}
	}
	if ((hw->pinState & 1) && hw->lightEdge) {
		++hw->lightCounter;
	}
	hw->lightEdge = !(hw->pinState & 1);

	bool sendBit = hw->lightCounter >= hw->lightSample;
	_outputPins(hw, sendBit << 3);
	mLOG(GBA_HW, DEBUG, "[SOLAR] Output %u with pins %u", hw->lightCounter, hw->pinState);
}

// == Tilt

void GBAHardwareInitTilt(struct GBACartridgeHardware* hw) {
	hw->devices |= HW_TILT;
	hw->tiltX = 0xFFF;
	hw->tiltY = 0xFFF;
	hw->tiltState = 0;
}

void GBAHardwareTiltWrite(struct GBACartridgeHardware* hw, uint32_t address, uint8_t value) {
	switch (address) {
	case 0x8000:
		if (value == 0x55) {
			hw->tiltState = 1;
		} else {
			mLOG(GBA_HW, GAME_ERROR, "Tilt sensor wrote wrong byte to %04x: %02x", address, value);
		}
		break;
	case 0x8100:
		if (value == 0xAA && hw->tiltState == 1) {
			hw->tiltState = 0;
			struct mRotationSource* rotationSource = hw->p->rotationSource;
			if (!rotationSource || !rotationSource->readTiltX || !rotationSource->readTiltY) {
				return;
			}
			if (rotationSource->sample) {
				rotationSource->sample(rotationSource);
			}
			int32_t x = rotationSource->readTiltX(rotationSource);
			int32_t y = rotationSource->readTiltY(rotationSource);
			// Normalize to ~12 bits, focused on 0x3A0
			hw->tiltX = (x >> 21) + 0x3A0; // Crop off an extra bit so that we can't go negative
			hw->tiltY = (y >> 21) + 0x3A0;
		} else {
			mLOG(GBA_HW, GAME_ERROR, "Tilt sensor wrote wrong byte to %04x: %02x", address, value);
		}
		break;
	default:
		mLOG(GBA_HW, GAME_ERROR, "Invalid tilt sensor write to %04x: %02x", address, value);
		break;
	}
}

uint8_t GBAHardwareTiltRead(struct GBACartridgeHardware* hw, uint32_t address) {
	switch (address) {
	case 0x8200:
		return hw->tiltX & 0xFF;
	case 0x8300:
		return ((hw->tiltX >> 8) & 0xF) | 0x80;
	case 0x8400:
		return hw->tiltY & 0xFF;
	case 0x8500:
		return (hw->tiltY >> 8) & 0xF;
	default:
		mLOG(GBA_HW, GAME_ERROR, "Invalid tilt sensor read from %04x", address);
		break;
	}
	return 0xFF;
}

// == Game Boy Player

static const uint16_t _logoPalette[] = {
	0xFFDF, 0x640C, 0xE40C, 0xE42D, 0x644E, 0xE44E, 0xE46E, 0x68AF,
	0xE8B0, 0x68D0, 0x68F0, 0x6911, 0xE911, 0x6D32, 0xED32, 0xED73,
	0x6D93, 0xED94, 0x6DB4, 0xF1D5, 0x71F5, 0xF1F6, 0x7216, 0x7257,
	0xF657, 0x7678, 0xF678, 0xF699, 0xF6B9, 0x76D9, 0xF6DA, 0x7B1B,
	0xFB1B, 0xFB3C, 0x7B5C, 0x7B7D, 0xFF7D, 0x7F9D, 0x7FBE, 0x7FFF,
	0x642D, 0x648E, 0xE88F, 0xE8F1, 0x6D52, 0x6D73, 0xF1B4, 0xF216,
	0x7237, 0x7698, 0x7AFA, 0xFAFA, 0xFB5C, 0xFFBE, 0x7FDE, 0xFFFF,
	0xFFFF, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000
};

static const uint32_t _logoHash = 0xEEDA6963;

static const uint32_t _gbpTxData[] = {
	0x0000494E, 0x0000494E,
	0xB6B1494E, 0xB6B1544E,
	0xABB1544E, 0xABB14E45,
	0xB1BA4E45, 0xB1BA4F44,
	0xB0BB4F44, 0xB0BB8002,
	0x10000010, 0x20000013,
	0x30000003
};

bool GBAHardwarePlayerCheckScreen(const struct GBAVideo* video) {
	if (memcmp(video->palette, _logoPalette, sizeof(_logoPalette)) != 0) {
		return false;
	}
	uint32_t hash = hash32(&video->renderer->vram[0x4000], 0x4000, 0);
	return hash == _logoHash;
}

void GBAHardwarePlayerUpdate(struct GBA* gba) {
	if (gba->memory.hw.devices & HW_GB_PLAYER) {
		if (GBAHardwarePlayerCheckScreen(&gba->video)) {
			++gba->memory.hw.gbpInputsPosted;
			gba->memory.hw.gbpInputsPosted %= 3;
			gba->keyCallback = &gba->memory.hw.gbpCallback.d;
		} else {
			// TODO: Save and restore
			gba->keyCallback = 0;
		}
		gba->memory.hw.gbpTxPosition = 0;
		return;
	}
	if (gba->keyCallback || gba->sio.drivers.normal) {
		return;
	}
	if (GBAHardwarePlayerCheckScreen(&gba->video)) {
		gba->memory.hw.devices |= HW_GB_PLAYER;
		gba->memory.hw.gbpInputsPosted = 0;
		gba->keyCallback = &gba->memory.hw.gbpCallback.d;
		GBASIOSetDriver(&gba->sio, &gba->memory.hw.gbpDriver.d, SIO_NORMAL_32);
	}
}

uint16_t _gbpRead(struct mKeyCallback* callback) {
	struct GBAGBPKeyCallback* gbpCallback = (struct GBAGBPKeyCallback*) callback;
	if (gbpCallback->p->gbpInputsPosted == 2) {
		return 0xF0;
	}
	return 0;
}

uint16_t _gbpSioWriteRegister(struct GBASIODriver* driver, uint32_t address, uint16_t value) {
	struct GBAGBPSIODriver* gbp = (struct GBAGBPSIODriver*) driver;
	if (address == REG_SIOCNT) {
		if (value & 0x0080) {
			uint32_t rx = gbp->p->p->memory.io[REG_SIODATA32_LO >> 1] | (gbp->p->p->memory.io[REG_SIODATA32_HI >> 1] << 16);
			if (gbp->p->gbpTxPosition < 12 && gbp->p->gbpTxPosition > 0) {
				// TODO: Check expected
			} else if (gbp->p->gbpTxPosition >= 12) {
				uint32_t mask = 0x33;
				// 0x00 = Stop
				// 0x11 = Hard Stop
				// 0x22 = Start
				if (gbp->p->p->rumble) {
					gbp->p->p->rumble->setRumble(gbp->p->p->rumble, (rx & mask) == 0x22);
				}
			}
			mTimingDeschedule(&gbp->p->p->timing, &gbp->p->gbpNextEvent);
			mTimingSchedule(&gbp->p->p->timing, &gbp->p->gbpNextEvent, 2048);
		}
		value &= 0x78FB;
	}
	return value;
}

void _gbpSioProcessEvents(struct mTiming* timing, void* user, uint32_t cyclesLate) {
	UNUSED(timing);
	UNUSED(cyclesLate);
	struct GBAGBPSIODriver* gbp = user;
	uint32_t tx = 0;
	int txPosition = gbp->p->gbpTxPosition;
	if (txPosition > 16) {
		gbp->p->gbpTxPosition = 0;
		txPosition = 0;
	} else if (txPosition > 12) {
		txPosition = 12;
	}
	tx = _gbpTxData[txPosition];
	++gbp->p->gbpTxPosition;
	gbp->p->p->memory.io[REG_SIODATA32_LO >> 1] = tx;
	gbp->p->p->memory.io[REG_SIODATA32_HI >> 1] = tx >> 16;
	if (GBASIONormalIsIrq(gbp->d.p->siocnt)) {
		GBARaiseIRQ(gbp->p->p, IRQ_SIO, cyclesLate);
	}
	gbp->d.p->siocnt = GBASIONormalClearStart(gbp->d.p->siocnt);
	gbp->p->p->memory.io[REG_SIOCNT >> 1] = gbp->d.p->siocnt & ~0x0080;
}

// == e-Reader

void GBAHardwareInitEReader(struct GBACartridgeHardware* hw) {
	hw->devices |= HW_EREADER;
	_eReaderReset(hw);

	if (hw->p->memory.savedata.data[0xD000] == 0xFF) {
		memset(&hw->p->memory.savedata.data[0xD000], 0, 0x1000);
		memcpy(&hw->p->memory.savedata.data[0xD000], EREADER_CALIBRATION_TEMPLATE, sizeof(EREADER_CALIBRATION_TEMPLATE));
	}
	if (hw->p->memory.savedata.data[0xE000] == 0xFF) {
		memset(&hw->p->memory.savedata.data[0xE000], 0, 0x1000);
		memcpy(&hw->p->memory.savedata.data[0xE000], EREADER_CALIBRATION_TEMPLATE, sizeof(EREADER_CALIBRATION_TEMPLATE));
	}
}

void GBAHardwareEReaderWrite(struct GBACartridgeHardware* hw, uint32_t address, uint16_t value) {
	address &= 0x700FF;
	switch (address >> 17) {
	case 0:
		hw->eReaderRegisterUnk = value & 0xF;
		break;
	case 1:
		hw->eReaderRegisterReset = (value & 0x8A) | 4;
		if (value & 2) {
			_eReaderReset(hw);
		}
		break;
	case 2:
		mLOG(GBA_HW, GAME_ERROR, "e-Reader write to read-only registers: %05X:%04X", address, value);
		break;
	default:
		mLOG(GBA_HW, STUB, "Unimplemented e-Reader write: %05X:%04X", address, value);
	}
}

void GBAHardwareEReaderWriteFlash(struct GBACartridgeHardware* hw, uint32_t address, uint8_t value) {
	address &= 0xFFFF;
	switch (address) {
	case 0xFFB0:
		_eReaderWriteControl0(hw, value);
		break;
	case 0xFFB1:
		_eReaderWriteControl1(hw, value);
		break;
	case 0xFFB2:
		hw->eReaderRegisterLed &= 0xFF00;
		hw->eReaderRegisterLed |= value;
		break;
	case 0xFFB3:
		hw->eReaderRegisterLed &= 0x00FF;
		hw->eReaderRegisterLed |= value << 8;
		break;
	default:
		mLOG(GBA_HW, STUB, "Unimplemented e-Reader write to flash: %04X:%02X", address, value);
	}
}

uint16_t GBAHardwareEReaderRead(struct GBACartridgeHardware* hw, uint32_t address) {
	address &= 0x700FF;
	uint16_t value;
	switch (address >> 17) {
	case 0:
		return hw->eReaderRegisterUnk;
	case 1:
		return hw->eReaderRegisterReset;
	case 2:
		if (address > 0x40088) {
			return 0;
		}
		LOAD_16(value, address & 0xFE, hw->eReaderData);
		return value;
	}
	mLOG(GBA_HW, STUB, "Unimplemented e-Reader read: %05X", address);
	return 0;
}

uint8_t GBAHardwareEReaderReadFlash(struct GBACartridgeHardware* hw, uint32_t address) {
	address &= 0xFFFF;
	switch (address) {
	case 0xFFB0:
		return hw->eReaderRegisterControl0;
	case 0xFFB1:
		return hw->eReaderRegisterControl1;
	default:
		mLOG(GBA_HW, STUB, "Unimplemented e-Reader read from flash: %04X", address);
		return 0;
	}
}

static void _eReaderAnchor(uint8_t* origin) {
	origin[EREADER_DOTCODE_STRIDE * 0 + 1] = 1;
	origin[EREADER_DOTCODE_STRIDE * 0 + 2] = 1;
	origin[EREADER_DOTCODE_STRIDE * 0 + 3] = 1;
	origin[EREADER_DOTCODE_STRIDE * 1 + 0] = 1;
	origin[EREADER_DOTCODE_STRIDE * 1 + 1] = 1;
	origin[EREADER_DOTCODE_STRIDE * 1 + 2] = 1;
	origin[EREADER_DOTCODE_STRIDE * 1 + 3] = 1;
	origin[EREADER_DOTCODE_STRIDE * 1 + 4] = 1;
	origin[EREADER_DOTCODE_STRIDE * 2 + 0] = 1;
	origin[EREADER_DOTCODE_STRIDE * 2 + 1] = 1;
	origin[EREADER_DOTCODE_STRIDE * 2 + 2] = 1;
	origin[EREADER_DOTCODE_STRIDE * 2 + 3] = 1;
	origin[EREADER_DOTCODE_STRIDE * 2 + 4] = 1;
	origin[EREADER_DOTCODE_STRIDE * 3 + 0] = 1;
	origin[EREADER_DOTCODE_STRIDE * 3 + 1] = 1;
	origin[EREADER_DOTCODE_STRIDE * 3 + 2] = 1;
	origin[EREADER_DOTCODE_STRIDE * 3 + 3] = 1;
	origin[EREADER_DOTCODE_STRIDE * 3 + 4] = 1;
	origin[EREADER_DOTCODE_STRIDE * 4 + 1] = 1;
	origin[EREADER_DOTCODE_STRIDE * 4 + 2] = 1;
	origin[EREADER_DOTCODE_STRIDE * 4 + 3] = 1;
}

static void _eReaderAlignment(uint8_t* origin) {
	origin[8] = 1;
	origin[10] = 1;
	origin[12] = 1;
	origin[14] = 1;
	origin[16] = 1;
	origin[18] = 1;
	origin[21] = 1;
	origin[23] = 1;
	origin[25] = 1;
	origin[27] = 1;
	origin[29] = 1;
	origin[31] = 1;
}

static void _eReaderAddress(uint8_t* origin, int a) {
	origin[EREADER_DOTCODE_STRIDE * 7 + 2] = 1;
	uint16_t addr = EREADER_ADDRESS_CODES[a];
	int i;
	for (i = 0; i < 16; ++i) {
		origin[EREADER_DOTCODE_STRIDE * (16 + i) + 2] = (addr >> (15 - i)) & 1;
	}
}

void GBAHardwareEReaderScan(struct GBACartridgeHardware* hw, const void* data, size_t size) {
	if (!hw->eReaderDots) {
		hw->eReaderDots = anonymousMemoryMap(EREADER_DOTCODE_SIZE);
	}
	memset(hw->eReaderDots, 0, EREADER_DOTCODE_SIZE);

	int base;
	switch (size) {
	case 2912:
		base = 25;
		break;
	case 1872:
		base = 1;
		break;
	default:
		return;
	}

	size_t i;
	for (i = 0; i < (size / 104) + 1; ++i) {
		uint8_t* origin = &hw->eReaderDots[35 * i + 200];
		_eReaderAnchor(&origin[EREADER_DOTCODE_STRIDE * 0]);
		_eReaderAnchor(&origin[EREADER_DOTCODE_STRIDE * 35]);
		_eReaderAddress(origin, base + i);
	}
	for (i = 0; i < size / 104; ++i) {
		uint8_t block[1040];
		uint8_t* origin = &hw->eReaderDots[35 * i + 200];
		_eReaderAlignment(&origin[EREADER_DOTCODE_STRIDE * 2]);
		_eReaderAlignment(&origin[EREADER_DOTCODE_STRIDE * 37]);

		int b;
		for (b = 0; b < 104; ++b) {
			const int* nybble5;
			nybble5 = EREADER_NYBBLE_5BIT[((const uint8_t*) data)[i * 104 + b] >> 4];
			block[b * 10 + 0] = nybble5[0];
			block[b * 10 + 1] = nybble5[1];
			block[b * 10 + 2] = nybble5[2];
			block[b * 10 + 3] = nybble5[3];
			block[b * 10 + 4] = nybble5[4];
			nybble5 = EREADER_NYBBLE_5BIT[((const uint8_t*) data)[i * 104 + b] & 0xF];
			block[b * 10 + 5] = nybble5[0];
			block[b * 10 + 6] = nybble5[1];
			block[b * 10 + 7] = nybble5[2];
			block[b * 10 + 8] = nybble5[3];
			block[b * 10 + 9] = nybble5[4];
		}

		b = 0;
		int y;
		for (y = 0; y < 3; ++y) {
			memcpy(&origin[EREADER_DOTCODE_STRIDE * (4 + y) + 7], &block[b], 26);
			b += 26;
		}
		for (y = 0; y < 26; ++y) {
			memcpy(&origin[EREADER_DOTCODE_STRIDE * (7 + y) + 3], &block[b], 34);
			b += 34;
		}
		for (y = 0; y < 3; ++y) {
			memcpy(&origin[EREADER_DOTCODE_STRIDE * (33 + y) + 7], &block[b], 26);
			b += 26;
		}
	}
	hw->eReaderX = -24;
}

void _eReaderReset(struct GBACartridgeHardware* hw) {
	memset(hw->eReaderData, 0, sizeof(hw->eReaderData));
	hw->eReaderRegisterUnk = 0;
	hw->eReaderRegisterReset = 4;
	hw->eReaderRegisterControl0 = 0;
	hw->eReaderRegisterControl1 = 0x80;
	hw->eReaderRegisterLed = 0;
	hw->eReaderState = 0;
	hw->eReaderActiveRegister = 0;
}

void _eReaderWriteControl0(struct GBACartridgeHardware* hw, uint8_t value) {
	EReaderControl0 control = value & 0x7F;
	EReaderControl0 oldControl = hw->eReaderRegisterControl0;
	if (hw->eReaderState == EREADER_SERIAL_INACTIVE) {
		if (EReaderControl0IsClock(oldControl) && EReaderControl0IsData(oldControl) && !EReaderControl0IsData(control)) {
			hw->eReaderState = EREADER_SERIAL_STARTING;
		}
	} else if (EReaderControl0IsClock(oldControl) && !EReaderControl0IsData(oldControl) && EReaderControl0IsData(control)) {
		hw->eReaderState = EREADER_SERIAL_INACTIVE;

	} else if (hw->eReaderState == EREADER_SERIAL_STARTING) {
		if (EReaderControl0IsClock(oldControl) && !EReaderControl0IsData(oldControl) && !EReaderControl0IsClock(control)) {
			hw->eReaderState = EREADER_SERIAL_BIT_0;
			hw->eReaderCommand = EREADER_COMMAND_IDLE;
		}
	} else if (EReaderControl0IsClock(oldControl) && !EReaderControl0IsClock(control)) {
		mLOG(GBA_HW, DEBUG, "[e-Reader] Serial falling edge: %c %i", EReaderControl0IsDirection(control) ? '>' : '<', EReaderControl0GetData(control));
		// TODO: Improve direction control
		if (EReaderControl0IsDirection(control)) {
			hw->eReaderByte |= EReaderControl0GetData(control) << (7 - (hw->eReaderState - EREADER_SERIAL_BIT_0));
			++hw->eReaderState;
			if (hw->eReaderState == EREADER_SERIAL_END_BIT) {
				mLOG(GBA_HW, DEBUG, "[e-Reader] Wrote serial byte: %02x", hw->eReaderByte);
				switch (hw->eReaderCommand) {
				case EREADER_COMMAND_IDLE:
					hw->eReaderCommand = hw->eReaderByte;
					break;
				case EREADER_COMMAND_SET_INDEX:
					hw->eReaderActiveRegister = hw->eReaderByte;
					hw->eReaderCommand = EREADER_COMMAND_WRITE_DATA;
					break;
				case EREADER_COMMAND_WRITE_DATA:
					switch (hw->eReaderActiveRegister & 0x7F) {
					case 0:
					case 0x57:
					case 0x58:
					case 0x59:
					case 0x5A:
						// Read-only
						mLOG(GBA_HW, GAME_ERROR, "Writing to read-only e-Reader serial register: %02X", hw->eReaderActiveRegister);
						break;
					default:
						if ((hw->eReaderActiveRegister & 0x7F) > 0x5A) {
							mLOG(GBA_HW, GAME_ERROR, "Writing to non-existent e-Reader serial register: %02X", hw->eReaderActiveRegister);
							break;
						}
						hw->eReaderSerial[hw->eReaderActiveRegister & 0x7F] = hw->eReaderByte;
						break;
					}
					++hw->eReaderActiveRegister;
					break;
				default:
					mLOG(GBA_HW, ERROR, "Hit undefined state %02X in e-Reader state machine", hw->eReaderCommand);
					break;
				}
				hw->eReaderState = EREADER_SERIAL_BIT_0;
				hw->eReaderByte = 0;
			}
		} else if (hw->eReaderCommand == EREADER_COMMAND_READ_DATA) {
			int bit = hw->eReaderSerial[hw->eReaderActiveRegister & 0x7F] >> (7 - (hw->eReaderState - EREADER_SERIAL_BIT_0));
			control = EReaderControl0SetData(control, bit);
			++hw->eReaderState;
			if (hw->eReaderState == EREADER_SERIAL_END_BIT) {
				++hw->eReaderActiveRegister;
				mLOG(GBA_HW, DEBUG, "[e-Reader] Read serial byte: %02x", hw->eReaderSerial[hw->eReaderActiveRegister & 0x7F]);
			}
		}
	} else if (!EReaderControl0IsDirection(control)) {
		// Clear the error bit
		control = EReaderControl0ClearData(control);
	}
	hw->eReaderRegisterControl0 = control;
	if (!EReaderControl0IsScan(oldControl) && EReaderControl0IsScan(control)) {
		hw->eReaderX = 0;
		hw->eReaderY = 0;
	} else if (EReaderControl0IsLedEnable(control) && EReaderControl0IsScan(control) && !EReaderControl1IsScanline(hw->eReaderRegisterControl1)) {
		_eReaderReadData(hw);
	}
	mLOG(GBA_HW, STUB, "Unimplemented e-Reader Control0 write: %02X", value);
}

void _eReaderWriteControl1(struct GBACartridgeHardware* hw, uint8_t value) {
	EReaderControl1 control = (value & 0x32) | 0x80;
	hw->eReaderRegisterControl1 = control;
	if (EReaderControl0IsScan(hw->eReaderRegisterControl0) && !EReaderControl1IsScanline(control)) {
		++hw->eReaderY;
		if (hw->eReaderY == (hw->eReaderSerial[0x15] | (hw->eReaderSerial[0x14] << 8))) {
			hw->eReaderY = 0;
			if (hw->eReaderX < 3400) {
				hw->eReaderX += 220;
			}
		}
		_eReaderReadData(hw);
	}
	mLOG(GBA_HW, STUB, "Unimplemented e-Reader Control1 write: %02X", value);
}

void _eReaderReadData(struct GBACartridgeHardware* hw) {
	memset(hw->eReaderData, 0, EREADER_BLOCK_SIZE);
	if (hw->eReaderDots) {
		int y = hw->eReaderY - 10;
		if (y < 0 || y >= 120) {
			memset(hw->eReaderData, 0, EREADER_BLOCK_SIZE);
		} else {
			int i;
			uint8_t* origin = &hw->eReaderDots[EREADER_DOTCODE_STRIDE * (y / 3) + 16];
			for (i = 0; i < 20; ++i) {
				uint16_t word = 0;
				int x = hw->eReaderX + i * 16;
				word |= origin[(x +  0) / 3] << 8;
				word |= origin[(x +  1) / 3] << 9;
				word |= origin[(x +  2) / 3] << 10;
				word |= origin[(x +  3) / 3] << 11;
				word |= origin[(x +  4) / 3] << 12;
				word |= origin[(x +  5) / 3] << 13;
				word |= origin[(x +  6) / 3] << 14;
				word |= origin[(x +  7) / 3] << 15;
				word |= origin[(x +  8) / 3];
				word |= origin[(x +  9) / 3] << 1;
				word |= origin[(x + 10) / 3] << 2;
				word |= origin[(x + 11) / 3] << 3;
				word |= origin[(x + 12) / 3] << 4;
				word |= origin[(x + 13) / 3] << 5;
				word |= origin[(x + 14) / 3] << 6;
				word |= origin[(x + 15) / 3] << 7;
				STORE_16(word, (19 - i) << 1, hw->eReaderData);
			}
		}
	}
	hw->eReaderRegisterControl1 = EReaderControl1FillScanline(hw->eReaderRegisterControl1);
	if (EReaderControl0IsLedEnable(hw->eReaderRegisterControl0)) {
		uint16_t led = 2754; // TODO: Figure out why this breaks if using the LED register
		GBARaiseIRQ(hw->p, IRQ_GAMEPAK, -led);
	}
}

// == Serialization

void GBAHardwareSerialize(const struct GBACartridgeHardware* hw, struct GBASerializedState* state) {
	GBASerializedHWFlags1 flags1 = 0;
	GBASerializedHWFlags2 flags2 = 0;
	flags1 = GBASerializedHWFlags1SetReadWrite(flags1, hw->readWrite);
	STORE_16(hw->pinState, 0, &state->hw.pinState);
	STORE_16(hw->direction, 0, &state->hw.pinDirection);
	state->hw.devices = hw->devices;

	STORE_32(hw->rtc.bytesRemaining, 0, &state->hw.rtc.bytesRemaining);
	STORE_32(hw->rtc.transferStep, 0, &state->hw.rtc.transferStep);
	STORE_32(hw->rtc.bitsRead, 0, &state->hw.rtc.bitsRead);
	STORE_32(hw->rtc.bits, 0, &state->hw.rtc.bits);
	STORE_32(hw->rtc.commandActive, 0, &state->hw.rtc.commandActive);
	STORE_32(hw->rtc.command, 0, &state->hw.rtc.command);
	STORE_32(hw->rtc.control, 0, &state->hw.rtc.control);
	memcpy(state->hw.rtc.time, hw->rtc.time, sizeof(state->hw.rtc.time));

	STORE_16(hw->gyroSample, 0, &state->hw.gyroSample);
	flags1 = GBASerializedHWFlags1SetGyroEdge(flags1, hw->gyroEdge);
	STORE_16(hw->tiltX, 0, &state->hw.tiltSampleX);
	STORE_16(hw->tiltY, 0, &state->hw.tiltSampleY);
	flags2 = GBASerializedHWFlags2SetTiltState(flags2, hw->tiltState);
	flags2 = GBASerializedHWFlags1SetLightCounter(flags2, hw->lightCounter);
	state->hw.lightSample = hw->lightSample;
	flags1 = GBASerializedHWFlags1SetLightEdge(flags1, hw->lightEdge);
	flags2 = GBASerializedHWFlags2SetGbpInputsPosted(flags2, hw->gbpInputsPosted);
	flags2 = GBASerializedHWFlags2SetGbpTxPosition(flags2, hw->gbpTxPosition);
	STORE_32(hw->gbpNextEvent.when - mTimingCurrentTime(&hw->p->timing), 0, &state->hw.gbpNextEvent);
	STORE_16(flags1, 0, &state->hw.flags1);
	state->hw.flags2 = flags2;
}

void GBAHardwareDeserialize(struct GBACartridgeHardware* hw, const struct GBASerializedState* state) {
	GBASerializedHWFlags1 flags1;
	LOAD_16(flags1, 0, &state->hw.flags1);
	hw->readWrite = GBASerializedHWFlags1GetReadWrite(flags1);
	LOAD_16(hw->pinState, 0, &state->hw.pinState);
	LOAD_16(hw->direction, 0, &state->hw.pinDirection);
	hw->devices = state->hw.devices;

	LOAD_32(hw->rtc.bytesRemaining, 0, &state->hw.rtc.bytesRemaining);
	LOAD_32(hw->rtc.transferStep, 0, &state->hw.rtc.transferStep);
	LOAD_32(hw->rtc.bitsRead, 0, &state->hw.rtc.bitsRead);
	LOAD_32(hw->rtc.bits, 0, &state->hw.rtc.bits);
	LOAD_32(hw->rtc.commandActive, 0, &state->hw.rtc.commandActive);
	LOAD_32(hw->rtc.command, 0, &state->hw.rtc.command);
	LOAD_32(hw->rtc.control, 0, &state->hw.rtc.control);
	memcpy(hw->rtc.time, state->hw.rtc.time, sizeof(hw->rtc.time));

	LOAD_16(hw->gyroSample, 0, &state->hw.gyroSample);
	hw->gyroEdge = GBASerializedHWFlags1GetGyroEdge(flags1);
	LOAD_16(hw->tiltX, 0, &state->hw.tiltSampleX);
	LOAD_16(hw->tiltY, 0, &state->hw.tiltSampleY);
	hw->tiltState = GBASerializedHWFlags2GetTiltState(state->hw.flags2);
	hw->lightCounter = GBASerializedHWFlags1GetLightCounter(flags1);
	hw->lightSample = state->hw.lightSample;
	hw->lightEdge = GBASerializedHWFlags1GetLightEdge(flags1);
	hw->gbpInputsPosted = GBASerializedHWFlags2GetGbpInputsPosted(state->hw.flags2);
	hw->gbpTxPosition = GBASerializedHWFlags2GetGbpTxPosition(state->hw.flags2);

	uint32_t when;
	LOAD_32(when, 0, &state->hw.gbpNextEvent);
	if (hw->devices & HW_GB_PLAYER) {
		GBASIOSetDriver(&hw->p->sio, &hw->gbpDriver.d, SIO_NORMAL_32);
		if (hw->p->memory.io[REG_SIOCNT >> 1] & 0x0080) {
			mTimingSchedule(&hw->p->timing, &hw->gbpNextEvent, when);
		}
	}
}