#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, bus_t *bus, bool initialize )
{
	cpu->bus = bus;
	cpu->debug_flags = 0;
	cpu->steps = 0;
	
	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 = 0; i < steps; i++ ) {
			cpu_6502_step( cpu );
		}
	} else {
		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 steps: %d\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, cpu->steps );
}

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++;

	if( cpu->debug_flags & DEBUG_STATUS ) {
		cpu_6502_print_state( cpu, opcode );
	}
	if( cpu->debug_flags & DEBUG_STACK ) {
		cpu_6502_print_stack( cpu );
	}
	if( cpu->debug_flags & DEBUG_ZERO_PAGE ) {
		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 STX_ABS:
			operand16 = cpu_6502_read_word( cpu, cpu->PC );
			cpu->PC += 2;
			cpu_6502_write_byte( cpu, operand16, cpu->X );
			break;
		
		case DEY_IMPL:
			cpu->Y--;
			update_negative_and_sign( cpu, cpu->Y );
			break;
			
		case ROL_ACC:
			tmp = ( cpu->A << 1 ) | 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 BNE_REL:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			if( !is_zero( cpu ) ) {
				cpu->PC += (int8_t)operand8;
			}
			break;
			
		case BCC_REL:
			operand8 = cpu_6502_read_byte( cpu, cpu->PC );
			cpu->PC++;
			if( !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;
			
		default:
			fprintf( stderr, "ERROR: Illegal opcode %02X at PC %04X\n", opcode, cpu->PC );
			exit( EXIT_FAILURE );
	}
	
	cpu->steps++;
}