#include "stdafx.h"
#include "SuperGameboy.h"
#include "Console.h"
#include "MemoryManager.h"
#include "BaseCartridge.h"
#include "Gameboy.h"
#include "GbApu.h"
#include "GbPpu.h"
#include "MessageManager.h"
#include "../Utilities/HexUtilities.h"
#include "../Utilities/HermiteResampler.h"

SuperGameboy::SuperGameboy(Console* console) : BaseCoprocessor(SnesMemoryType::Register)
{
	_console = console;
	_memoryManager = console->GetMemoryManager().get();
	_cart = _console->GetCartridge().get();
	
	_gameboy = _cart->GetGameboy();
	_gameboy->PowerOn(this);
	_ppu = _gameboy->GetPpu();
	_mixBuffer = new int16_t[0x10000];

	MemoryMappings* cpuMappings = _memoryManager->GetMemoryMappings();
	for(int i = 0; i <= 0x3F; i++) {
		cpuMappings->RegisterHandler(i, i, 0x6000, 0x7FFF, this);
		cpuMappings->RegisterHandler(i + 0x80, i + 0x80, 0x6000, 0x7FFF, this);
	}
}

SuperGameboy::~SuperGameboy()
{
	delete[] _mixBuffer;
}

void SuperGameboy::Reset()
{
	_control = 0;
	_resetClock = 0;

	memset(_input, 0, sizeof(_input));
	_inputIndex = 0;

	_listeningForPacket = false;
	_waitForHigh = true;
	_packetReady = false;
	_inputWriteClock = 0;
	_inputValue = 0;
	memset(_packetData, 0, sizeof(_packetData));
	_packetByte = 0;
	_packetBit = 0;

	_lcdRowSelect = 0;
	_readPosition = 0;
	memset(_lcdBuffer, 0, sizeof(_lcdBuffer));
}

uint8_t SuperGameboy::Read(uint32_t addr)
{
	addr &= 0xF80F;
	
	if(addr >= 0x7000 && addr <= 0x700F) {
		_packetReady = false;
		return _packetData[addr & 0x0F];
	} else if(addr >= 0x7800 && addr <= 0x780F) {
		if(_readPosition >= 320) {
			//Return 0xFF for 320..511 and then wrap to 0
			_readPosition = (_readPosition + 1) & 0x1FF;
			return 0xFF;
		}

		uint8_t* start = _lcdBuffer[_lcdRowSelect];
		start += ((_readPosition >> 1) & 0x07) * 160;
		start += (_readPosition >> 4) * 8;

		uint8_t data = 0;
		uint8_t shift = _readPosition & 0x01;
		for(int i = 0; i < 8; i++) {
			data |= ((start[i] >> shift) & 0x01) << (7 - i);
		}
		_readPosition++;
		return data;
	} else {
		switch(addr & 0xFFFF) {
			case 0x6000: return (GetLcdRow() << 3) | GetLcdBufferRow();
			case 0x6002: return _packetReady;
			case 0x600F: return 0x21; //or 0x61
		}
	}

	return 0;
}

void SuperGameboy::Write(uint32_t addr, uint8_t value)
{
	addr &= 0xF80F;

	switch(addr & 0xFFFF) {
		case 0x6001:
			_lcdRowSelect = value & 0x03; 
			_readPosition = 0;
			break;

		case 0x6003: {
			if(!(_control & 0x80) && (value & 0x80)) {
				_resetClock = _memoryManager->GetMasterClock();
				_gameboy->PowerOn(this);
				_ppu = _gameboy->GetPpu();
			}
			_control = value;
			_inputIndex %= GetPlayerCount();
			break;
		}

		case 0x6004: _input[0] = value; break;
		case 0x6005: _input[1] = value; break;
		case 0x6006: _input[2] = value; break;
		case 0x6007: _input[3] = value; break;
	}
}

void SuperGameboy::Run()
{
	_gameboy->Run(_memoryManager->GetMasterClock());
}

void SuperGameboy::ProcessInputPortWrite(uint8_t value)
{
	if(_inputValue == value) {
		return;
	}

	if(value == 0x00) {
		//Reset pulse
		_waitForHigh = true;
		_packetByte = 0;
		_packetBit = 0;
	} else if(_waitForHigh) {
		if(value == 0x10 || value == 0x20) {
			//Invalid sequence (should be 0x00 -> 0x30 -> 0x10/0x20 -> 0x30 -> 0x10/0x20, etc.)
			_waitForHigh = false;
			_listeningForPacket = false;
		} else if(value == 0x30) {
			_waitForHigh = false;
			_listeningForPacket = true;
		}
	} else if(_listeningForPacket) {
		if(value == 0x20) {
			//0 bit
			if(_packetByte >= 16 && _packetBit == 0) {
				_packetReady = true;
				_listeningForPacket = false;

				/*string log = HexUtilities::ToHex(_packetData[0] >> 3);
				log += " Size: " + std::to_string(_packetData[0] & 0x07) + " - ";
				for(int i = 0; i < 16; i++) {
					log += HexUtilities::ToHex(_packetData[i]) + " ";
				}
				MessageManager::Log(log);*/
			} else {
				_packetData[_packetByte] &= ~(1 << _packetBit);
			}
			_packetBit++;
			if(_packetBit == 8) {
				_packetBit = 0;
				_packetByte++;
			}
		} else if(value == 0x10) {
			//1 bit
			if(_packetByte >= 16) {
				//Invalid bit
				_listeningForPacket = false;
			} else {
				_packetData[_packetByte] |= (1 << _packetBit);
				_packetBit++;
				if(_packetBit == 8) {
					_packetBit = 0;
					_packetByte++;
				}
			}
		}
		_waitForHigh = _listeningForPacket;
	} else if(!(_inputValue & 0x20) && (value & 0x20)) {
		_inputIndex = (_inputIndex + 1) % GetPlayerCount();
	}

	_inputValue = value;
	_inputWriteClock = _memoryManager->GetMasterClock();
}

void SuperGameboy::WriteLcdColor(uint8_t scanline, uint8_t pixel, uint8_t color)
{
	_lcdBuffer[GetLcdBufferRow()][(scanline & 0x07) * 160 + pixel] = color;
}

uint8_t SuperGameboy::GetLcdRow()
{
	uint8_t scanline = _ppu->GetScanline();
	uint8_t row = scanline / 8;
	if(row >= 18) {
		row = 0;
	}
	return row;
}

uint8_t SuperGameboy::GetLcdBufferRow()
{
	return (_ppu->GetFrameCount() * 18 + GetLcdRow()) & 0x03;
}

uint8_t SuperGameboy::GetPlayerCount()
{
	uint8_t playerCount = ((_control >> 4) & 0x03) + 1;
	if(playerCount >= 3) {
		//Unknown: 2 and 3 both mean 4 players?
		return 4;
	}
	return playerCount;
}

void SuperGameboy::MixAudio(uint32_t targetRate, int16_t* soundSamples, uint32_t sampleCount)
{
	int16_t* gbSamples = nullptr;
	uint32_t gbSampleCount = 0;
	_gameboy->GetSoundSamples(gbSamples, gbSampleCount);
	_resampler.SetSampleRates(GbApu::SampleRate, targetRate);
	
	int32_t outCount = (int32_t)_resampler.Resample(gbSamples, gbSampleCount, _mixBuffer + _mixSampleCount) * 2;
	_mixSampleCount += outCount;

	int32_t copyCount = (int32_t)std::min(_mixSampleCount, sampleCount*2);
	for(int32_t i = 0; i < copyCount; i++) {
		soundSamples[i] += _mixBuffer[i];
	}

	int32_t remainingSamples = (int32_t)_mixSampleCount - copyCount;
	if(remainingSamples > 0) {
		memmove(_mixBuffer, _mixBuffer + copyCount, remainingSamples*sizeof(int16_t));
		_mixSampleCount = remainingSamples;
	} else {
		_mixSampleCount = 0;
	}
}

uint8_t SuperGameboy::GetControl()
{
	return _control;
}

uint64_t SuperGameboy::GetResetClock()
{
	return _resetClock;
}

uint8_t SuperGameboy::GetInputIndex()
{
	return 0xF - _inputIndex;
}

uint8_t SuperGameboy::GetInput()
{
	return _input[_inputIndex];
}

uint8_t SuperGameboy::Peek(uint32_t addr)
{
	return 0;
}

void SuperGameboy::PeekBlock(uint32_t addr, uint8_t* output)
{
	memset(output, 0, 0x1000);
}

AddressInfo SuperGameboy::GetAbsoluteAddress(uint32_t address)
{
	return { -1, SnesMemoryType::Register };
}

void SuperGameboy::Serialize(Serializer& s)
{
	s.Stream(
		_control, _resetClock, _input[0], _input[1], _input[2], _input[3], _inputIndex, _listeningForPacket, _packetReady,
		_inputWriteClock, _inputValue, _packetByte, _packetBit, _lcdRowSelect, _readPosition, _waitForHigh
	);

	s.StreamArray(_packetData, 16);
	s.StreamArray(_lcdBuffer[0], 1280);
	s.StreamArray(_lcdBuffer[1], 1280);
	s.StreamArray(_lcdBuffer[2], 1280);
	s.StreamArray(_lcdBuffer[3], 1280);
}