#include "CPU/CPU.hpp"
#include "NES.hpp"


// --------------- ARITHMETIC --------------- //
void CPU::ADC()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	uint16_t tempSum = acc + fetchedByte;

	tempSum += status.carry;

	status.carry = (tempSum & 0xFF00u) ? 1u : 0u;

	bool accSign = IsNegative(acc);
	bool valSign = IsNegative(fetchedByte);
	bool sumSign = IsNegative(tempSum);

	acc = tempSum;

	// If sign of operands does not match sign of result -> overflow
	status.overflow = ((accSign == valSign) && (accSign != sumSign)) ? 1u : 0u;

	status.zero = acc ? 0u : 1u;
	status.negative = (IsNegative(acc)) ? 1u : 0u;

	cycles += (pageBoundaryCrossed) ? 1u : 0u;
}

void CPU::SBC()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	uint16_t tempDiff = acc - fetchedByte;

	tempDiff -= (1u - status.carry);

	status.carry = (tempDiff & 0xFF00) ? 0u : 1u;

	bool accSign = IsNegative(acc);
	bool valSign = IsNegative(fetchedByte);
	bool sumSign = IsNegative(tempDiff);

	// Set and truncate most-significant byte of temp sum
	acc = tempDiff;

	// If sign of operands does not match sign of result -> overflow
	status.overflow = ((accSign != valSign) && (valSign == sumSign)) ? 1u : 0u;

	status.zero = acc ? 0u : 1u;
	status.negative = (IsNegative(acc)) ? 1u : 0u;

	cycles += (pageBoundaryCrossed) ? 1u : 0u;
}

void CPU::CMP()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	status.carry = (acc >= fetchedByte) ? 1u : 0u;
	status.zero = (acc == fetchedByte) ? 1u : 0u;
	status.negative = (IsNegative(acc - fetchedByte)) ? 1u : 0u;

	cycles += (pageBoundaryCrossed) ? 1u : 0u;
}

void CPU::CPX()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	status.carry = (x >= fetchedByte) ? 1u : 0u;
	status.zero = (x == fetchedByte) ? 1u : 0u;
	status.negative = (IsNegative(x - fetchedByte)) ? 1u : 0u;
}

void CPU::CPY()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	status.carry = (y >= fetchedByte) ? 1u : 0u;
	status.zero = (y == fetchedByte) ? 1u : 0u;
	status.negative = (IsNegative(y - fetchedByte)) ? 1u : 0u;
}


// --------------- BRANCHES --------------- //
void CPU::BPL()
{
	if (!status.negative)
	{
		pc = fetchedAddress;

		cycles += (pageBoundaryCrossed) ? 2u : 1u;
	}
}

void CPU::BMI()
{
	if (status.negative)
	{
		pc = fetchedAddress;

		cycles += (pageBoundaryCrossed) ? 2u : 1u;
	}
}

void CPU::BVC()
{
	if (!status.overflow)
	{
		pc = fetchedAddress;

		cycles += (pageBoundaryCrossed) ? 2u : 1u;
	}
}

void CPU::BVS()
{
	if (status.overflow)
	{
		pc = fetchedAddress;

		cycles += (pageBoundaryCrossed) ? 2u : 1u;
	}
}

void CPU::BCC()
{
	if (!status.carry)
	{
		pc = fetchedAddress;

		cycles += (pageBoundaryCrossed) ? 2u : 1u;
	}
}

void CPU::BCS()
{
	if (status.carry)
	{
		pc = fetchedAddress;

		cycles += (pageBoundaryCrossed) ? 2u : 1u;
	}
}

void CPU::BNE()
{
	if (!status.zero)
	{
		pc = fetchedAddress;

		cycles += (pageBoundaryCrossed) ? 2u : 1u;
	}
}

void CPU::BEQ()
{
	if (status.zero)
	{
		pc = fetchedAddress;

		cycles += (pageBoundaryCrossed) ? 2u : 1u;
	}
}


// --------------- INCREMENT/DECREMENT --------------- //
void CPU::INC()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	++fetchedByte;

	nes->Write(BusSource::CPU, fetchedAddress, fetchedByte);

	status.zero = fetchedByte ? 0u : 1u;
	status.negative = (IsNegative(fetchedByte)) ? 1u : 0u;
}

void CPU::INX()
{
	++x;

	status.zero = x ? 0u : 1u;
	status.negative = (IsNegative(x)) ? 1u : 0u;
}

void CPU::INY()
{
	++y;

	status.zero = y ? 0u : 1u;
	status.negative = (IsNegative(y)) ? 1u : 0u;
}

void CPU::DEC()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	--fetchedByte;

	nes->Write(BusSource::CPU, fetchedAddress, fetchedByte);

	status.zero = fetchedByte ? 0u : 1u;
	status.negative = (IsNegative(fetchedByte)) ? 1u : 0u;
}

void CPU::DEX()
{
	--x;

	status.zero = x ? 0u : 1u;
	status.negative = (IsNegative(x)) ? 1u : 0u;
}

void CPU::DEY()
{
	--y;

	status.zero = y ? 0u : 1u;
	status.negative = (IsNegative(y)) ? 1u : 0u;
}


// --------------- JUMPS & CALLS --------------- //
void CPU::JMP()
{
	pc = fetchedAddress;
}

void CPU::JSR()
{
	uint8_t pcMsb = ((pc - 1u) & 0xFF00u) >> 8u;
	uint8_t pcLsb = (pc - 1u) & 0xFFu;

	StackPush(pcMsb);
	StackPush(pcLsb);

	pc = fetchedAddress;
}

void CPU::RTS()
{
	uint8_t pcLsb = StackPop();
	uint8_t pcMsb = StackPop();

	uint16_t address = ComposeAddress(pcMsb, pcLsb) + 1u;

	pc = address;
}


// --------------- LOAD/STORE --------------- //
void CPU::LDA()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	cycles += (pageBoundaryCrossed) ? 1u : 0u;

	acc = fetchedByte;

	status.zero = acc ? 0u : 1u;
	status.negative = (IsNegative(acc)) ? 1u : 0u;
}

void CPU::LDX()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	cycles += (pageBoundaryCrossed) ? 1u : 0u;

	x = fetchedByte;

	status.zero = x ? 0u : 1u;
	status.negative = (IsNegative(x)) ? 1u : 0u;
}

void CPU::LDY()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	cycles += (pageBoundaryCrossed) ? 1u : 0u;

	y = fetchedByte;

	status.zero = y ? 0u : 1u;
	status.negative = (IsNegative(y)) ? 1u : 0u;
}

void CPU::STA()
{
	nes->Write(BusSource::CPU, fetchedAddress, acc);
}

void CPU::STX()
{
	nes->Write(BusSource::CPU, fetchedAddress, x);
}

void CPU::STY()
{
	nes->Write(BusSource::CPU, fetchedAddress, y);
}


// --------------- LOGICAL --------------- //
void CPU::AND()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);

	acc &= fetchedByte;

	cycles += (pageBoundaryCrossed) ? 1u : 0u;
	status.zero = acc ? 0u : 1u;
	status.negative = (IsNegative(acc)) ? 1u : 0u;
}

void CPU::EOR()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	acc ^= fetchedByte;

	cycles += (pageBoundaryCrossed) ? 1u : 0u;
	status.zero = acc ? 0u : 1u;
	status.negative = (IsNegative(acc)) ? 1u : 0u;
}

void CPU::ORA()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	acc |= fetchedByte;

	cycles += (pageBoundaryCrossed) ? 1u : 0u;
	status.zero = acc ? 0u : 1u;
	status.negative = (IsNegative(acc)) ? 1u : 0u;
}

void CPU::BIT()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	status.zero = (acc & fetchedByte) ? 0u : 1u;
	status.overflow = (TestBits(fetchedByte, (1u << 6u))) ? 1u : 0u;
	status.negative = (IsNegative(fetchedByte)) ? 1u : 0u;
}


// --------------- SHIFTS --------------- //
void CPU::ASL()
{
	if (instructions[opcode].fetch == &CPU::Implicit)
	{
		fetchedByte = acc;
	}
	else
	{
		fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	}

	// Set carry to fetchedByte of MSB pre-shift
	if (TestBits(fetchedByte, 0x80))
	{
		status.carry = 1u;
	}
	else
	{
		status.carry = 0u;
	}

	fetchedByte <<= 1u;

	status.zero = fetchedByte ? 0u : 1u;
	status.negative = (IsNegative(fetchedByte)) ? 1u : 0u;

	if (instructions[opcode].fetch == &CPU::Implicit)
	{
		acc = fetchedByte;
	}
	else
	{
		nes->Write(BusSource::CPU, fetchedAddress, fetchedByte);
	}
}

void CPU::LSR()
{
	if (instructions[opcode].fetch == &CPU::Implicit)
	{
		fetchedByte = acc;
	}
	else
	{
		fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	}

	// Set carry to fetchedByte of LSB pre-shift
	if (TestBits(fetchedByte, 0x01))
	{
		status.carry = 1u;
	}
	else
	{
		status.carry = 0u;
	}

	fetchedByte >>= 1u;

	status.zero = fetchedByte ? 0u : 1u;
	status.negative = (IsNegative(fetchedByte)) ? 1u : 0u;

	if (instructions[opcode].fetch == &CPU::Implicit)
	{
		acc = fetchedByte;
	}
	else
	{
		nes->Write(BusSource::CPU, fetchedAddress, fetchedByte);
	}
}

void CPU::ROL()
{
	if (instructions[opcode].fetch == &CPU::Implicit)
	{
		fetchedByte = acc;
	}
	else
	{
		fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	}

	uint8_t oldCarry = status.carry;

	// Set carry to fetchedByte of MSB pre-shift
	if (TestBits(fetchedByte, 0x80))
	{
		status.carry = 1u;
	}
	else
	{
		status.carry = 0u;
	}

	fetchedByte <<= 1u;

	// Put old carry at LSB
	if (oldCarry)
	{
		fetchedByte |= 0x1u;
	}

	status.zero = fetchedByte ? 0u : 1u;
	status.negative = (IsNegative(fetchedByte)) ? 1u : 0u;

	if (instructions[opcode].fetch == &CPU::Implicit)
	{
		acc = fetchedByte;
	}
	else
	{
		nes->Write(BusSource::CPU, fetchedAddress, fetchedByte);
	}
}

void CPU::ROR()
{
	if (instructions[opcode].fetch == &CPU::Implicit)
	{
		fetchedByte = acc;
	}
	else
	{
		fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	}

	uint8_t oldCarry = status.carry;

	// Set carry to fetchedByte of LSB pre-shift
	if (TestBits(fetchedByte, 0x01))
	{
		status.carry = 1u;
	}
	else
	{
		status.carry = 0u;
	}

	fetchedByte >>= 1u;

	// Put old carry at MSB
	if (oldCarry)
	{
		fetchedByte |= 0x80u;
	}

	status.zero = fetchedByte ? 0u : 1u;
	status.negative = (IsNegative(fetchedByte)) ? 1u : 0u;

	if (instructions[opcode].fetch == &CPU::Implicit)
	{
		acc = fetchedByte;
	}
	else
	{
		nes->Write(BusSource::CPU, fetchedAddress, fetchedByte);
	}
}


// --------------- STACK --------------- //
void CPU::PHP()
{
	Status statusCopy = status;
	statusCopy.b1 = 1u;
	statusCopy.b0 = 1u;

	StackPush(statusCopy.GetByte());
}

void CPU::PLP()
{
	uint8_t stackCopy = StackPop();

	status.SetByte(stackCopy);
	status.b1 = 1u;
	status.b0 = 0u;
}

void CPU::PHA()
{
	StackPush(acc);
}

void CPU::PLA()
{
	acc = StackPop();

	status.zero = acc ? 0u : 1u;
	status.negative = (IsNegative(acc)) ? 1u : 0u;
}


// --------------- STATUS FLAG CHANGES --------------- //
void CPU::CLC()
{
	status.carry = 0u;
}

void CPU::SEC()
{
	status.carry = 1u;
}

void CPU::CLI()
{
	status.interruptDisable = 0u;
}

void CPU::SEI()
{
	status.interruptDisable = 1u;
}

void CPU::CLV()
{
	status.overflow = 0u;
}

void CPU::CLD()
{
	status.decimal = 0u;
}

void CPU::SED()
{
	status.decimal = 1u;
}


// --------------- SYSTEM --------------- //
void CPU::BRK()
{
	uint8_t pcMsb = (pc & 0xFF00u) >> 8u;
	uint8_t pcLsb = pc & 0xFFu;

	StackPush(pcMsb);
	StackPush(pcLsb);

	Status statusCopy = status;
	statusCopy.b1 = 1u;
	statusCopy.b0 = 0u;

	StackPush(statusCopy.GetByte());

	pcMsb = nes->Read(BusSource::CPU, IRQ_VECTOR_MSB);
	pcLsb = nes->Read(BusSource::CPU, IRQ_VECTOR_LSB);

	pc = ComposeAddress(pcMsb, pcLsb);
}

void CPU::NOP()
{
	// Do nothing
	cycles += (pageBoundaryCrossed) ? 1u : 0u;
}

void CPU::RTI()
{
	uint8_t statusCopy = StackPop();

	status.SetByte(statusCopy);
	status.b1 = 1u;

	uint8_t pcLsb = StackPop();
	uint8_t pcMsb = StackPop();

	pc = ComposeAddress(pcMsb, pcLsb);
}


// --------------- TRANSFERS --------------- //
void CPU::TXA()
{
	acc = x;

	status.zero = acc ? 0u : 1u;
	status.negative = (IsNegative(acc)) ? 1u : 0u;
}

void CPU::TYA()
{
	acc = y;

	status.zero = acc ? 0u : 1u;
	status.negative = (IsNegative(acc)) ? 1u : 0u;
}

void CPU::TXS()
{
	sp = x;
}

void CPU::TAY()
{
	y = acc;

	status.zero = y ? 0u : 1u;
	status.negative = (IsNegative(y)) ? 1u : 0u;
}

void CPU::TAX()
{
	x = acc;

	status.zero = x ? 0u : 1u;
	status.negative = (IsNegative(x)) ? 1u : 0u;
}

void CPU::TSX()
{
	x = sp;

	status.zero = x ? 0u : 1u;
	status.negative = (IsNegative(x)) ? 1u : 0u;
}


// --------------- UNOFFICIAL --------------- //
void CPU::LAX()
{
	LDA();
	TAX();
}

void CPU::SAX()
{
	uint8_t byte = acc & x;

	nes->Write(BusSource::CPU, fetchedAddress, byte);
}

void CPU::DCP()
{
	DEC();
	CMP();

	cycles -= (pageBoundaryCrossed) ? 1u : 0u;
}

void CPU::ISC()
{
	INC();
	SBC();

	cycles -= (pageBoundaryCrossed) ? 1u : 0u;
}

void CPU::SLO()
{
	ASL();
	ORA();

	cycles -= (pageBoundaryCrossed) ? 1u : 0u;
}

void CPU::RLA()
{
	ROL();
	AND();

	cycles -= (pageBoundaryCrossed) ? 1u : 0u;
}

void CPU::SRE()
{
	LSR();
	EOR();

	cycles -= (pageBoundaryCrossed) ? 1u : 0u;
}

void CPU::RRA()
{
	ROR();
	ADC();

	cycles -= (pageBoundaryCrossed) ? 1u : 0u;
}

void CPU::ANC()
{
	AND();

	status.carry = (fetchedByte & 0x80u) >> 7u;
}

void CPU::ALR()
{
	AND();
	LSR();
}

void CPU::ARR()
{
	AND();
	ROR();

	status.carry = (fetchedByte & 0x40u) >> 6u;
	status.overflow = ((fetchedByte & 0x40u) >> 6u) ^ ((fetchedByte & 0x20u) >> 5u);
}

void CPU::AXS()
{
	fetchedByte = nes->Read(BusSource::CPU, fetchedAddress);
	x = (acc & x) - fetchedByte;

	status.negative = (IsNegative(x)) ? 1u : 0u;
	status.zero = x ? 0u : 1u;
	status.carry = (x & 0xFF00u) ? 1u : 0u;
}