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

#include <stdio.h>
#include <stdlib.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, struct memory_t *memory )
{
	cpu->memory = memory;
	cpu->debug = false;
}

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->memory->read( cpu->memory, addr );
}

void cpu_6502_write_byte( cpu_6502_t *cpu, uint16_t addr, uint8_t data )
{
	cpu->memory->write( cpu->memory, addr, data );
}

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

void cpu_6502_run( cpu_6502_t *cpu )
{
	while( true ) {
		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;
	}
}

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

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

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

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

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

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

static bool is_carry( cpu_6502_t *cpu )
{
	return cpu->PS & PS_C;
}
	
void cpu_6502_print_state( cpu_6502_t *cpu, uint8_t opcode )
{
	fprintf( stderr, "PC: %04X SP: 01%02X PS: %02X %c%c-%c%c%c%c%c A: %02X X: %02X Y: %02X OP: %02X\n", 
		cpu->PC, cpu->SP, cpu->PS,
		is_negative( cpu ) ? 'N' : 'n',
		is_overflow( cpu ) ? 'V' : 'v',
		is_brk( cpu ) ? 'B' : 'b',
		is_decimal( cpu ) ? 'D' : 'd',
		is_interrupt( cpu ) ? 'I' : 'i',
		is_zero( cpu ) ? 'Z' : 'z',
		is_carry( cpu ) ? 'C' : 'c',
		cpu->A, cpu->X, cpu->Y, opcode );
}

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;
	
	opcode = cpu_6502_read_byte( cpu, cpu->PC );
	cpu->PC++;

	if( cpu->debug ) {
		cpu_6502_print_state( cpu, opcode );
		cpu_6502_print_stack( cpu );
		cpu_6502_print_zerop_page( 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 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;
			
		default:
			fprintf( stderr, "ERROR: Illegal opcode %02X at PC %04X\n", opcode, cpu->PC );
			exit( EXIT_FAILURE );
	}
}