#include "6502.h"
#include "memory.h"

#include <stdio.h>

static const uint16_t reset_vector = 0xFFFC;
static const uint16_t ZP_base = 0x0;
static const uint16_t SP_base = 0x100;

void cpu_6502_init( cpu_6502_t *cpu, bus_t *bus, bool initialize )
{
	cpu->bus = bus;
	cpu->debug_flags = 0;
	cpu->steps = 0;
	cpu->error_state = ERROR_STATE_OK;
	
	if( initialize ) {
		cpu->PC = 0x0000;
		cpu->SP = 0x00;
		cpu->A = 0x00;
		cpu->X = 0x00;
		cpu->Y = 0x00;
	}
}

uint16_t cpu_6502_read_word( cpu_6502_t *cpu, uint16_t addr )
{
	return cpu_6502_read_byte( cpu, addr ) + ( cpu_6502_read_byte( cpu, addr + 1 ) << 8 );
}
	
void cpu_6502_push_byte( cpu_6502_t *cpu, uint8_t data )
{
	cpu_6502_write_byte( cpu, SP_base + cpu->SP, data );
	cpu->SP--;
}

void cpu_6502_push_word( cpu_6502_t *cpu, uint16_t data )
{
	cpu_6502_push_byte( cpu, ( data >> 8 ) & 0xFF );
	cpu_6502_push_byte( cpu, data & 0xFF );
}

uint8_t cpu_6502_pop_byte( cpu_6502_t *cpu )
{
	uint8_t data;
	
	cpu->SP++;
	data = cpu_6502_read_byte( cpu, SP_base + cpu->SP );
	
	return data;
}

uint16_t cpu_6502_pop_word( cpu_6502_t *cpu )
{
	uint8_t low;
	uint8_t high;
	uint16_t data;

	low = cpu_6502_pop_byte( cpu );
	high = cpu_6502_pop_byte( cpu );
	
	data = ( high << 8 ) + low;
	
	return data;
}

void cpu_6502_reset( cpu_6502_t *cpu )
{
	cpu->PC = cpu_6502_read_word( cpu, reset_vector );
}

uint8_t cpu_6502_read_byte( cpu_6502_t *cpu, uint16_t addr )
{
	return cpu->bus->base.vtable->read( cpu->bus, addr );
}

void cpu_6502_write_byte( cpu_6502_t *cpu, uint16_t addr, uint8_t data )
{
	cpu->bus->base.vtable->write( cpu->bus, addr, data );
}

void cpu_6502_write_word( cpu_6502_t *cpu, uint16_t addr, uint16_t data )
{
	cpu->bus->base.vtable->write( cpu->bus, addr, ( data && 0x00FF ) );
	cpu->bus->base.vtable->write( cpu->bus, addr + 1, ( data && 0x00FF ) );
}

void cpu_6502_run( cpu_6502_t *cpu, int steps )
{
	if( steps != -1 ) {
		for( int i = 1; i <= steps && cpu->error_state == ERROR_STATE_OK; i++ ) {
			cpu_6502_step( cpu );
		}
	} else {
		while( cpu->error_state == ERROR_STATE_OK ) {
			cpu_6502_step( cpu );
		}
	}
}

static void handle_zero( cpu_6502_t *cpu, uint8_t x )
{
	if( x == 0 ) {
		cpu->PS |= PS_Z;
	} else {
		cpu->PS &= ~PS_Z;
	}
}

static void handle_sign( cpu_6502_t *cpu, uint8_t x )
{
	if( x & 0x80 ) {
		cpu->PS |= PS_N;
	} else {
		cpu->PS &= ~ PS_N;
	}
}

bool cpu_6502_is_negative( cpu_6502_t *cpu )
{
	return cpu->PS & PS_N;
}

bool cpu_6502_is_overflow( cpu_6502_t *cpu )
{
	return cpu->PS & PS_V;
}

bool cpu_6502_is_brk( cpu_6502_t *cpu )
{
	return cpu->PS & PS_B;
}

bool cpu_6502_is_decimal( cpu_6502_t *cpu )
{
	return cpu->PS & PS_D;
}

bool cpu_6502_is_interrupt( cpu_6502_t *cpu )
{
	return cpu->PS & PS_I;
}

bool cpu_6502_is_zero( cpu_6502_t *cpu )
{
	return cpu->PS & PS_Z;
}

bool cpu_6502_is_carry( cpu_6502_t *cpu )
{
	return cpu->PS & PS_C;
}

static const char mnemonic[NOF_OPCODES][MAX_MENMONIC_LENGTH] = {
	/*         0     1      2      3      4      5      6      7      8      9      A      B      C      D      E      F */
	/* 0 */  "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???",
	/* 1 */  "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???",
	/* 2 */  "JSR", "???", "???", "???", "???", "???", "???", "???", "???", "???", "ROL", "???", "???", "???", "???", "???",
	/* 3 */  "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???",
	/* 4 */  "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "JMP", "???", "???", "???",
	/* 5 */  "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???",
	/* 6 */  "RTS", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???",
	/* 7 */  "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???",
	/* 8 */  "???", "???", "???", "???", "???", "???", "STX", "???", "DEY", "???", "???", "???", "???", "???", "STX", "???",
	/* 9 */  "BCC", "???", "???", "???", "???", "???", "???", "???", "???", "???", "TXS", "???", "???", "???", "???", "???",
	/* A */  "LDY", "???", "LDX", "???", "LDY", "LDA", "LDX", "???", "???", "LDA", "???", "???", "???", "???", "???", "???",
	/* B */  "BCS", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???",
	/* C */  "???", "???", "???", "???", "???", "???", "???", "???", "INY", "???", "DEX", "???", "???", "???", "???", "???",
	/* D */  "BNE", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???",
	/* E */  "CPX", "???", "???", "???", "???", "???", "INC", "???", "???", "SBC", "NOP", "???", "???", "???", "???", "???",
	/* F */  "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???", "???"

/*
             "BRK", "ORA", ""   , "", "TSB", "ORA", "ASL", "RMB0", "PHP", "ORA", "ASL", ""   , "TSB", "ORA", "ASL", "BBR0",
             "BPL", "ORA", "ORA", "", "TRB", "ORA", "ASL", "RMB1", "CLC", "ORA", "INC", ""   , "TRB", "ORA", "ASL", "BBR1",
             "JSR", "AND", ""   , "", "BIT", "AND", "ROL", "RMB2", "PLP", "AND", "ROL", ""   , "BIT", "AND", "ROL", "BBR2",
             "BMI", "AND", "AND", "", "BIT", "AND", "ROL", "RMB3", "SEC", "AND", "DEC", ""   , "BIT", "AND", "ROL", "BBR3",
             "RTI", "EOR", ""   , "", ""   , "EOR", "LSR", "RMB4", "PHA", "EOR", "LSR", ""   , "JMP", "EOR", "LSR", "BBR4",
             "BVC", "EOR", "EOR", "", ""   , "EOR", "LSR", "RMB5", "CLI", "EOR", "PHY", ""   , ""   , "EOR", "LSR", "BBR5",
             "RTS", "ADC", ""   , "", "STZ", "ADC", "ROR", "RMB6", "PLA", "ADC", "ROR", ""   , "JMP", "ADC", "ROR", "BBR6",
             "BVS", "ADC", "ADC", "", "STZ", "ADC", "ROR", "RMB7", "SEI", "ADC", "PLY", ""   , "JMP", "ADC", "ROR", "BBR7",
             "BRA", "STA", ""   , "", "STY", "STA", "STX", "SMB0", "DEY", "BIT", "TXA", ""   , "STY", "STA", "STX", "BBS0",
             "BCC", "STA", "STA", "", "STY", "STA", "STX", "SMB1", "TYA", "STA", "TXS", ""   , "STZ", "STA", "STZ", "BBS1",
             "LDY", "LDA", "LDX", "", "LDY", "LDA", "LDX", "SMB2", "TAY", "LDA", "TAX", ""   , "LDY", "LDA", "LDX", "BBS2",
             "BCS", "LDA", "LDA", "", "LDY", "LDA", "LDX", "SMB3", "CLV", "LDA", "TSX", ""   , "LDY", "LDA", "LDX", "BBS3",
             "CPY", "CMP", ""   , "", "CPY", "CMP", "DEC", "SMB4", "INY", "CMP", "DEX", "WAI", "CPY", "CMP", "DEC", "BBS4",
             "BNE", "CMP", "CMP", "", ""   , "CMP", "DEC", "SMB5", "CLD", "CMP", "PHX", "STP", ""   , "CMP", "DEC", "BBS5",
             "CPX", "SBC", ""   , "", "CPX", "SBC", "INC", "SMB6", "INX", "SBC", "NOP", ""   , "CPX", "SBC", "INC", "BBS6",
             "BEQ", "SBC", "SBC", "", ""   , "SBC", "INC", "SMB7", "SED", "SBC", "PLX", ""   , ""   , "SBC", "INC", "BBS7"	
*/
};

void cpu_6502_get_opcode_mnemonic( cpu_6502_t *cpu, char *buf, int buflen, uint16_t addr )
{
	uint8_t opcode;
	
	opcode = cpu_6502_read_byte( cpu, addr );
	
	snprintf( buf, buflen, "%s (%02X)", mnemonic[opcode], opcode );
}

void cpu_6502_print_debug( cpu_6502_t *cpu )
{
	if( cpu->debug_flags & DEBUG_STATUS ) {
		cpu_6502_print_state( cpu, cpu->PC-1 );
	}
	if( cpu->debug_flags & DEBUG_STACK ) {
		cpu_6502_print_stack( cpu );
	}
	if( cpu->debug_flags & DEBUG_ZERO_PAGE ) {
		cpu_6502_print_zerop_page( cpu );
	}
}
	
void cpu_6502_print_state( cpu_6502_t *cpu, uint16_t addr )
{
	char opcode_str[MAX_OPCODE_MNEMONIC_STRING_LENGTH];
	
	cpu_6502_get_opcode_mnemonic( cpu, opcode_str, MAX_OPCODE_MNEMONIC_STRING_LENGTH, addr );
	
	fprintf( stderr, "PC: %04X SP: 01%02X PS: %02X %c%c-%c%c%c%c%c A: %02X X: %02X Y: %02X steps: %d OP: %s\n", 
		cpu->PC, cpu->SP, cpu->PS,
		cpu_6502_is_negative( cpu ) ? 'N' : 'n',
		cpu_6502_is_overflow( cpu ) ? 'V' : 'v',
		cpu_6502_is_brk( cpu ) ? 'B' : 'b',
		cpu_6502_is_decimal( cpu ) ? 'D' : 'd',
		cpu_6502_is_interrupt( cpu ) ? 'I' : 'i',
		cpu_6502_is_zero( cpu ) ? 'Z' : 'z',
		cpu_6502_is_carry( cpu ) ? 'C' : 'c',
		cpu->A, cpu->X, cpu->Y, cpu->steps,
		opcode_str );
}

static void update_negative_and_sign( cpu_6502_t *cpu, uint8_t x )
{
	handle_zero( cpu, x );
	handle_sign( cpu, x );
}

void cpu_6502_print_memory( cpu_6502_t *cpu, uint16_t base, uint8_t size )
{
	for( uint16_t addr = base; addr <= base + size; addr++ ) {
		fprintf( stderr, " %02X", cpu_6502_read_byte( cpu, addr ) );
	}
	fputs( "\n", stderr );
}

void cpu_6502_print_stack( cpu_6502_t *cpu )
{
	cpu_6502_print_memory( cpu, SP_base, 255 );
}

void cpu_6502_print_zerop_page( cpu_6502_t *cpu )
{
	cpu_6502_print_memory( cpu, ZP_base, 255 );
}

void cpu_6502_step( cpu_6502_t *cpu )
{
	uint8_t opcode;
	uint8_t operand8;
	uint16_t operand16;
	uint16_t tmp;
	
	opcode = cpu_6502_read_byte( cpu, cpu->PC );
	cpu->PC++;
	
	cpu_6502_print_debug( cpu );
	
	switch( opcode ) {
		case LDX_IMM:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			cpu->X = operand8;
			update_negative_and_sign( cpu, cpu->X );
			break;

		case LDX_ZERO:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			cpu->X = cpu_6502_read_byte( cpu, operand8 );
			update_negative_and_sign( cpu, cpu->X );
			break;

		case LDY_IMM:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			cpu->Y = operand8;
			update_negative_and_sign( cpu, cpu->Y );
			break;

		case LDY_ZERO:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			cpu->Y = cpu_6502_read_byte( cpu, operand8 );
			update_negative_and_sign( cpu, cpu->Y );
			break;
			
		case LDA_IMM:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			cpu->A = operand8;
			update_negative_and_sign( cpu, cpu->A );
			break;
			
		case LDA_ZERO:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			cpu->A = cpu_6502_read_byte( cpu, operand8 );
			update_negative_and_sign( cpu, cpu->A );
			break;
		
		case STX_ZERO:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			cpu_6502_write_byte( cpu, operand8, cpu->X );
			break;
		
		case STX_ABS:
			operand16 = cpu_6502_read_word( cpu, cpu->PC );
			cpu->PC += 2;
			cpu_6502_write_byte( cpu, operand16, cpu->X );
			break;

		case DEX_IMPL:
			cpu->X--;
			update_negative_and_sign( cpu, cpu->X );
			break;
			
		case DEY_IMPL:
			cpu->Y--;
			update_negative_and_sign( cpu, cpu->Y );
			break;
		
		case INY_IMPL:
			cpu->Y++;
			update_negative_and_sign( cpu, cpu->Y );
			break;
		
		case INC_ZERO:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			tmp = cpu_6502_read_byte( cpu, operand8 );
			tmp++;
			cpu_6502_write_byte( cpu, operand8, tmp );
			update_negative_and_sign( cpu, tmp );
			break;
			
		case ROL_ACC:
			tmp = ( cpu->A << 1 ) | cpu_6502_is_carry( cpu );
			if( tmp & 0xFF00 ) {
				cpu->PS |= PS_C;
			} else {
				cpu->PS &= ~PS_C;
			}
			cpu->A = tmp & 0xFF;
			update_negative_and_sign( cpu, cpu->A );
			break;
		
		case CPX_IMM:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			tmp = ( cpu->X - operand8 ) & 0xFF;
			if( cpu->X >= operand8 ) {
				cpu->PS |= PS_C;
			} else {
				cpu->PS &= ~PS_C;
			}
			update_negative_and_sign( cpu, tmp );
			break;
		
		case SBC_IMM:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			tmp = cpu->A - operand8 - ( cpu_6502_is_carry( cpu ) ? 0 : 1 );
			update_negative_and_sign( cpu, tmp );
			if( tmp & 0xFF00 ) {
				cpu->PS |= PS_C;
			} else {
				cpu->PS &= ~PS_C;
			}
			if( ( ( cpu->A ^ tmp ) & 0x80 ) && ( ( cpu->A ^ operand8 ) & 0x80 ) ) {
				cpu->PS |= PS_V;
			} else {
				cpu->PS &= ~PS_V;
			}
			cpu->A = tmp & 0xFF;
			break;
		
		case BNE_REL:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			if( !cpu_6502_is_zero( cpu ) ) {
				cpu->PC += (int8_t)operand8;
			}
			break;
			
		case BCC_REL:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			if( !cpu_6502_is_carry( cpu ) ) {
				cpu->PC += (int8_t)operand8;
			}
			break;

		case BCS_REL:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			if( cpu_6502_is_carry( cpu ) ) {
				cpu->PC += (int8_t)operand8;
			}
			break;
			
		case JMP_ABS:
			cpu->PC = cpu_6502_read_word( cpu, cpu->PC );
			break;
		
		case JSR_ABS:
			cpu_6502_push_word( cpu, cpu->PC + 1 );
			cpu->PC = cpu_6502_read_word( cpu, cpu->PC );
			break;
		
		case RTS_IMPL:
			cpu->PC = cpu_6502_pop_word( cpu );
			cpu->PC++;
			break;

		case TXS_IMPL:
			cpu->SP = cpu->X;
			break;
			
		case NOP_IMPL:
			break;
			
		default:
			fprintf( stderr, "ERROR: Illegal opcode %02X at PC %04X\n", opcode, cpu->PC );
			cpu->error_state |= ERROR_STATE_ILLEGAL_OPCODE;
	}
	
	cpu->steps++;
}