/*
 * mui_65c02.c
 *
 * Copyright (C) 2023 Michel Pollet <buserror@gmail.com>
 *
 * SPDX-License-Identifier: MIT
 */

#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include "minipt.h"
#include "mii_65c02.h"
#define MII_CPU_65C02_IMPL
#include "mii_65c02_ops.h"

#define likely(x)		__builtin_expect(!!(x), 1)
#define unlikely(x)		__builtin_expect(!!(x), 0)

mii_cpu_state_t
mii_cpu_init(
		mii_cpu_t *cpu )
{
	mii_cpu_state_t s = {
		.addr = 0,
		.reset = 1,
	};
	cpu->state = NULL;
	return s;
}

#define _FETCH(_val) { \
		s.addr = _val; s.w = 0; cpu->cycle++; \
		pt_yield(cpu->state); \
	}
#define _STORE(_addr, _val) { \
		s.addr = _addr; s.data = _val; s.w = 1; cpu->cycle++; \
		pt_yield(cpu->state); \
	}


#define _NZC(_val) { \
		uint16_t v = (_val); \
		cpu->P.N = !!(v & 0x80); \
		cpu->P.Z = (v & 0xff) == 0; \
		cpu->P.C = !!(v & 0xff00); \
	}
#define _NZ(_val) { \
		uint16_t v = (_val); \
		cpu->P.N = !!(v & 0x80); \
		cpu->P.Z = (v & 0xff) == 0; \
	}
#define _C(_val) { \
		cpu->P.C = !!(_val); \
	}

mii_cpu_state_t
mii_cpu_run(
		mii_cpu_t *cpu,
		mii_cpu_state_t s)
{
	mii_op_desc_t d = mii_cpu_op[cpu->IR].desc;
	pt_start(cpu->state);
next_instruction:
	if (unlikely(s.reset)) {
		s.reset = 0;
		_FETCH(0xfffc); cpu->_P = s.data;
		_FETCH(0xfffd);	cpu->_P |= s.data << 8;
		cpu->PC = cpu->_P;
	  	cpu->S = 0xFF;
		MII_SET_P(cpu, 0);
	}
	if (unlikely(s.irq) && cpu->P.I == 0) {
		if (!cpu->IRQ)
			cpu->IRQ = 1;
	}
	if (unlikely(cpu->IRQ)) {
		s.irq = 0;
		cpu->P.B = cpu->IRQ == 2;
		cpu->_D = cpu->PC;
		_STORE(0x0100 | cpu->S--, cpu->_D >> 8);
		_STORE(0x0100 | cpu->S--, cpu->_D & 0xff);
		uint8_t p = 0;
		MII_GET_P(cpu, p);
		_STORE(0x0100 | cpu->S--, p);
		cpu->P.I = 1;
		if (cpu->IRQ == 2)
			cpu->P.D = 0;
		cpu->IRQ = 0;
		_FETCH(0xfffe); cpu->_P = s.data;
		_FETCH(0xffff);	cpu->_P |= s.data << 8;
		cpu->PC = cpu->_P;
	}
	s.sync = 1;
	// we dont' reset the cycle here, that way calling code has a way of knowing
	// how many cycles were used by the previous instruction
	_FETCH(cpu->PC);
	s.sync = 0;
	cpu->cycle = 0;
	cpu->PC++;
	cpu->IR = s.data;
	d = mii_cpu_op[cpu->IR].desc;
	cpu->ir_log = (cpu->ir_log << 8) | cpu->IR;
	s.trap = cpu->trap && (cpu->ir_log & 0xffff) == cpu->trap;
	if (unlikely(s.trap))
		cpu->ir_log = 0;
	switch (d.mode) {
		case IMM:
			_FETCH(cpu->PC++);		cpu->_D = s.data;
			break;
		case BRANCH: // BEQ/BNE etc
		case ZP_REL: // $(xx)
			_FETCH(cpu->PC++);		cpu->_P = s.data;
			break;
		case ZP_X: // $xx,X
			_FETCH(cpu->PC++);		cpu->_P = (s.data + cpu->X) & 0xff;
			break;
		case ZP_Y:	// $xx,Y
			_FETCH(cpu->PC++);		cpu->_P = (s.data + cpu->Y) & 0xff;
			break;
		case ABS: {	// $xxxx
			_FETCH(cpu->PC++);		cpu->_P = s.data;
			_FETCH(cpu->PC++);		cpu->_P |= s.data << 8;
		}	break;
		case ABS_X: { // $xxxx,X
			_FETCH(cpu->PC++);		cpu->_P = s.data;
			_FETCH(cpu->PC++);		cpu->_P |= s.data << 8;
			cpu->_P += cpu->X;
			if ((cpu->_P & 0xff00) != (s.data << 8))
				cpu->cycle++;
		}	break;
		case ABS_Y: { // $xxxx,Y
			_FETCH(cpu->PC++);		cpu->_P = s.data;
			_FETCH(cpu->PC++);		cpu->_P |= s.data << 8;
			cpu->_P += cpu->Y;
			if ((cpu->_P & 0xff00) != (s.data << 8))
				cpu->cycle++;
		}	break;
		case IND_X: { // ($xx,X)
			_FETCH(cpu->PC++);		cpu->_D = s.data;
			cpu->_D += cpu->X;
			_FETCH(cpu->_D & 0xff);	cpu->_P = s.data;
			cpu->_D++;
			_FETCH(cpu->_D & 0xff);	cpu->_P |= s.data << 8;
		}	break;
		case IND_Y: { // ($xx),Y
			_FETCH(cpu->PC++);		cpu->_D = s.data;
			_FETCH(cpu->_D);		cpu->_P = s.data;
			_FETCH((cpu->_D + 1) & 0xff);
			cpu->_P |= s.data << 8;
			cpu->_P += cpu->Y;
		}	break;
		case IND: {	// ($xxxx)
			_FETCH(cpu->PC++); 		cpu->_D = s.data;
			_FETCH(cpu->PC++); 		cpu->_D |= s.data << 8;
			_FETCH(cpu->_D); 		cpu->_P = s.data;
			_FETCH(cpu->_D + 1); 	cpu->_P |= s.data << 8;
		}	break;
		case IND_Z: {	// ($xx)
			_FETCH(cpu->PC++); 		cpu->_D = s.data;
			_FETCH(cpu->_D); 		cpu->_P = s.data;
			_FETCH(cpu->_D + 1); 	cpu->_P |= s.data << 8;
		}	break;
		case IND_AX: { // ($xxxx,X)
			_FETCH(cpu->PC++);		cpu->_D = s.data;
			_FETCH(cpu->PC++);		cpu->_D |= s.data << 8;
			cpu->_D += cpu->X;
			if ((cpu->_D & 0xff00) != (s.data << 8))
				cpu->cycle++;
			_FETCH(cpu->_D);		cpu->_P = s.data;
			_FETCH(cpu->_D + 1);	cpu->_P |= s.data << 8;
		}	break;
	}
	if (d.r) {
		_FETCH(cpu->_P);
		cpu->_D = s.data;
	}
	switch (cpu->IR) {
		case 0x69: case 0x65: case 0x75: case 0x6D: case 0x7D:
		case 0x79: case 0x61: case 0x71: case 0x72:
		{ // ADC
			// Handle adding in BCD with bit D
			if (unlikely(cpu->P.D)) {
				uint8_t D = cpu->_D;
				uint8_t lo = (cpu->A & 0x0f) + (D & 0x0f) + !!cpu->P.C;
				if (lo > 9) lo += 6;
				uint8_t hi = (cpu->A >> 4) + (D >> 4) + (lo > 0x0f);
				cpu->P.Z = ((uint8_t)(cpu->A + D + cpu->P.C)) == 0;
				// that is 6502 behaviour
//				cpu->P.N = !!(hi & 0xf8);
				cpu->P.V = !!((!((cpu->A ^ D) & 0x80) &&
								((cpu->A ^ (hi << 4))) & 0x80));
				if (hi > 9) hi += 6;
				cpu->P.C = hi > 15;
			//	printf("ADC %02x %02x C:%d %x%x\n",
			//			cpu->A, D, !!cpu->P.C, hi & 0xf, lo & 0xf);
				cpu->A = (hi << 4) | (lo & 0x0f);
				// THAT is 65c02 behaviour
				cpu->P.N = !!(cpu->A & 0x80);
			} else {
				uint16_t sum = cpu->A + cpu->_D + !!cpu->P.C;
				cpu->P.V = cpu->P.C = 0;
				if (~(cpu->A ^ cpu->_D) & (cpu->A ^ sum) & 0x80)
					cpu->P.V = 1;
				_NZC(sum);
				cpu->A = sum;
			}
		}	break;
		case 0x29: case 0x25: case 0x35: case 0x2D: case 0x3D:
		case 0x39: case 0x21: case 0x31: case 0x32:
		{ // AND
			cpu->A &= cpu->_D;
			_NZ(cpu->A);
		}	break;
		case 0x0A:
		{ // ASL
			cpu->P.C = !!(cpu->A & 0x80);
			cpu->A <<= 1;
			_NZ(cpu->A);
		}	break;
		case 0x06: case 0x16: case 0x0E: case 0x1E:
		{ // ASL
			cpu->P.C = !!(cpu->_D & 0x80);
			cpu->_D <<= 1;
			_NZ(cpu->_D);
		}	break;
		case 0x0f: case 0x1f: case 0x2f: case 0x3f: case 0x4f:
		case 0x5f: case 0x6f: case 0x7f: case 0x8f: case 0x9f:
		case 0xaf: case 0xbf: case 0xcf: case 0xdf: case 0xef:
		case 0xff:
		{ // BBR/BBS
//			printf(" BB%c%d vs %02x\n", d.s_bit_value ? 'S' : 'R',
//					d.s_bit, cpu->_D);
			_FETCH(cpu->PC++);	// relative branch
			if (((cpu->_D >> d.s_bit) & 1) == d.s_bit_value) {
				cpu->_P = cpu->PC + (int8_t)s.data;
				cpu->cycle++;
				if ((cpu->_P & 0xff00) != (cpu->PC & 0xff00))
					cpu->cycle++;
				cpu->PC = cpu->_P;
			}
		}	break;
		case 0x90: case 0xB0: case 0xF0: case 0x30:
		case 0xD0: case 0x10: case 0x50: case 0x70:
		{ // BCC, BCS, BEQ, BMI, BNE, BPL, BVC, BVS
			if (d.s_bit_value == MII_GET_P_BIT(cpu, d.s_bit)) {
				cpu->_P = cpu->PC + (int8_t)cpu->_P;
				cpu->cycle++;
				if ((cpu->_P & 0xff00) != (cpu->PC & 0xff00))
					cpu->cycle++;
				cpu->PC = cpu->_P;
			}
		}	break;
		case 0x80:
		{	// BRA
			cpu->_P = cpu->PC + (int8_t)cpu->_P;
			_FETCH(cpu->PC);// cpu->cycle++;
			if ((cpu->_P & 0xff00) != (cpu->PC & 0xff00))
				cpu->cycle++;
			cpu->PC = cpu->_P;
		}	break;
		case 0x89:
		{	// BIT immediate -- does not change N & V!
			cpu->P.Z = !(cpu->A & cpu->_D);
		}	break;
		case 0x24: case 0x2C: case 0x34: case 0x3C:
		{ // BIT
			cpu->P.Z = !(cpu->A & cpu->_D);
			cpu->P.N = !!(cpu->_D & 0x80);
			cpu->P.V = !!(cpu->_D & 0x40);
		}	break;
		case 0x00:
		{ // BRK
			// Turns out BRK is a 2 byte opcode, who knew? well that guy did:
			// https://www.nesdev.org/the%20'B'%20flag%20&%20BRK%20instruction.txt#:~:text=A%20note%20on%20the%20BRK,opcode%2C%20and%20not%20just%201.
			_FETCH(cpu->PC++);	// cpu->cycle++;
			s.irq = 1;
			cpu->IRQ = 2;		// BRK sort of IRQ interrupt
		}	break;
		case 0x18: case 0xD8: case 0x58: case 0xB8:
		{ // CLC, CLD, CLI, CLV
			_FETCH(cpu->PC);
			MII_SET_P_BIT(cpu, d.s_bit, 0);
		}	break;
		case 0xC9: case 0xC5: case 0xD5: case 0xCD: case 0xDD:
		case 0xD9: case 0xC1: case 0xD1: case 0xD2:
		{ // CMP
			cpu->P.C = !!(cpu->A >= cpu->_D);
			uint8_t d = cpu->A - cpu->_D;
			_NZ(d);
		}	break;
		case 0xE0: case 0xE4: case 0xEC:
		{ // CPX
			cpu->P.C = !!(cpu->X >= cpu->_D);
			uint8_t d = cpu->X - cpu->_D;
			_NZ(d);
		}	break;
		case 0xC0: case 0xC4: case 0xCC:
		{ // CPY
			cpu->P.C = !!(cpu->Y >= cpu->_D);
			uint8_t d = cpu->Y - cpu->_D;
			_NZ(d);
		}	break;
		case 0x3A:
		{ // DEC
			_FETCH(cpu->PC);
			_NZ(--cpu->A);
		}	break;
		case 0xC6: case 0xD6: case 0xCE: case 0xDE:
		{ // DEC
			_FETCH(cpu->PC);
			_NZ(--cpu->_D);
		}	break;
		case 0xCA:
		{ // DEX
			_FETCH(cpu->PC);
			_NZ(--cpu->X);
		}	break;
		case 0x88:
		{ // DEY
			_FETCH(cpu->PC);
			_NZ(--cpu->Y);
		}	break;
		case 0x49: case 0x45: case 0x55: case 0x4D: case 0x5D:
		case 0x59: case 0x41: case 0x51: case 0x52:
		{ // EOR
			cpu->A ^= cpu->_D;
			_NZ(cpu->A);
		}	break;
		case 0x1A:
		{ // INC
			_FETCH(cpu->PC);
			_NZ(++cpu->A);
		}	break;
		case 0xE6: case 0xF6: case 0xEE: case 0xFE:
		{ // INC
			_FETCH(cpu->PC);
			_NZ(++cpu->_D);
		}	break;
		case 0xE8:
		{ // INX
			_FETCH(cpu->PC);
			_NZ(++cpu->X);
		}	break;
		case 0xC8:
		{ // INY
			_FETCH(cpu->PC);
			_NZ(++cpu->Y);
		}	break;
		case 0x4C: case 0x6C: case 0x7C:
		{ // JMP
			cpu->PC = cpu->_P;
		}	break;
		case 0x20:
		// https://github.com/AppleWin/AppleWin/issues/1257
		{ // JSR
			_FETCH(cpu->PC++);		cpu->_P = s.data;
			_FETCH(0x0100 | cpu->S);
			_STORE(0x0100 | cpu->S--, cpu->PC >> 8);
			_STORE(0x0100 | cpu->S--, cpu->PC & 0xff);
			_FETCH(cpu->PC++);		cpu->_P |= s.data << 8;
			cpu->PC = cpu->_P;
		}	break;
		case 0xA9: case 0xA5: case 0xB5: case 0xAD: case 0xBD:
		case 0xB9: case 0xA1: case 0xB1: case 0xB2:
		{ // LDA
			cpu->A = cpu->_D;
			_NZ(cpu->A);
		}	break;
		case 0xA2: case 0xA6: case 0xB6: case 0xAE: case 0xBE:
		{ // LDX
			cpu->X = cpu->_D;
			_NZ(cpu->X);
		}	break;
		case 0xA0: case 0xA4: case 0xB4: case 0xAC: case 0xBC:
		{ // LDY
			cpu->Y = cpu->_D;
			_NZ(cpu->Y);
		}	break;
		case 0x4A:
		{ // LSR
			_FETCH(cpu->PC);
			cpu->P.C = !!(cpu->A & 0x01);
			cpu->A >>= 1;
			_NZ(cpu->A);
		}	break;
		case 0x46: case 0x56: case 0x4E: case 0x5E:
		{ // LSR
			cpu->P.C = !!(cpu->_D & 0x01);
			cpu->_D >>= 1;
			_NZ(cpu->_D);
		}	break;
		case 0xEA:
		{ // NOP
			_FETCH(cpu->PC);
		}	break;
		case 0x09: case 0x05: case 0x15: case 0x0D: case 0x1D:
		case 0x19: case 0x01: case 0x11: case 0x12:
		{ // ORA
			cpu->A |= cpu->_D;
			_NZ(cpu->A);
		}	break;
		case 0x48:
		{ // PHA
			_STORE(0x0100 | cpu->S--, cpu->A); cpu->cycle++;
		}	break;
		case 0x08:
		{ // PHP
			uint8_t p = 0;
			MII_GET_P(cpu, p);
			p |= (1 << B_B) | (1 << B_X);
			_STORE(0x0100 | cpu->S--, p);	cpu->cycle++;
		}	break;
		case 0xDA:
		{ // PHX
			_STORE(0x0100 | cpu->S--, cpu->X);cpu->cycle++;
		}	break;
		case 0x5A:
		{ // PHY
			_STORE(0x0100 | cpu->S--, cpu->Y);cpu->cycle++;
		}	break;
		case 0x68:
		{ // PLA
			_FETCH(0x0100 | ++cpu->S);cpu->cycle++;
			cpu->A = s.data;cpu->cycle++;
			_NZ(cpu->A);
		}	break;
		case 0x28:
		{ // PLP
			_FETCH(0x0100 | ++cpu->S);cpu->cycle++;
			MII_SET_P(cpu, s.data);cpu->cycle++;
		}	break;
		case 0xFA:
		{ // PLX
			_FETCH(0x0100 | ++cpu->S);
			cpu->X = s.data;
			_NZ(cpu->X);
		}	break;
		case 0x7A:
		{ // PLY
			_FETCH(0x0100 | ++cpu->S);
			cpu->Y = s.data;
			_NZ(cpu->Y);
		}	break;
		case 0x2A:
		{ // ROL
			uint8_t c = cpu->P.C;
			cpu->P.C = !!(cpu->A & 0x80);
			cpu->A <<= 1;
			cpu->A |= c;
			_NZ(cpu->A);
		}	break;
		case 0x26: case 0x36: case 0x2E: case 0x3E:
		{ // ROL
			uint8_t c = cpu->P.C;
			cpu->P.C = !!(cpu->_D & 0x80);
			cpu->_D <<= 1;
			cpu->_D |= c;
			_NZ(cpu->_D);
		}	break;
		case 0x6A:
		{ // ROR
			uint8_t c = cpu->P.C;
			cpu->P.C = !!(cpu->A & 0x01);
			cpu->A >>= 1;
			cpu->A |= c << 7;
			_NZ(cpu->A);
		}	break;
		case 0x66: case 0x76: case 0x6E: case 0x7E:
		{ // ROR
			uint8_t c = cpu->P.C;
			cpu->P.C = !!(cpu->_D & 0x01);
			cpu->_D >>= 1;
			cpu->_D |= c << 7;
			_NZ(cpu->_D);
		}	break;
		case 0x40:
		{ // RTI
			_FETCH(cpu->PC);	// dummy write
			cpu->S++; _FETCH(0x0100 | cpu->S);
			for (int i = 0; i < 8; i++)
				MII_SET_P_BIT(cpu, i, i == B_B || (s.data & (1 << i)));
			cpu->P._R = 1;
			cpu->S++; _FETCH(0x0100 | cpu->S);
			cpu->_P = s.data;
			cpu->S++; _FETCH(0x0100 | cpu->S);
			cpu->_P |= s.data << 8;
			cpu->PC = cpu->_P;
		}	break;
		case 0x60:
		{ // RTS
			_FETCH(0x0100 | ((++cpu->S) & 0xff));cpu->cycle++;
			cpu->_P = s.data;
			_FETCH(0x0100 | ((++cpu->S) & 0xff));cpu->cycle++;
			cpu->_P |= s.data << 8;
			cpu->PC = cpu->_P + 1; cpu->cycle++;
		}	break;
		case 0xE9: case 0xE5: case 0xF5: case 0xED: case 0xFD:
		case 0xF9: case 0xE1: case 0xF1: case 0xF2:
		{ // SBC
			// Handle subbing in BCD with bit D
			if (unlikely(cpu->P.D)) {
				uint8_t D = 0x99 - cpu->_D;
				// verbatim ADC code here
				uint8_t lo = (cpu->A & 0x0f) + (D & 0x0f) + !!cpu->P.C;
				if (lo > 9) lo += 6;
				uint8_t hi = (cpu->A >> 4) + (D >> 4) + (lo > 0x0f);
				cpu->P.Z = ((uint8_t)(cpu->A + D + cpu->P.C)) == 0;
				// that is 6502 behaviour
//				cpu->P.N = !!(hi & 0xf8);
				cpu->P.V = !!((!((cpu->A ^ D) & 0x80) &&
								((cpu->A ^ (hi << 4))) & 0x80));
				if (hi > 9) hi += 6;
				cpu->P.C = hi > 15;
			//	printf("SBC %02x %02x C:%d %x%x\n",
			//			cpu->A, D, !!cpu->P.C, hi & 0xf, lo & 0xf);
				cpu->A = (hi << 4) | (lo & 0x0f);
				// THAT is 65c02 behaviour
				cpu->P.N = !!(cpu->A & 0x80);
			} else {
				cpu->_D = (~cpu->_D) & 0xff;
				uint16_t sum = cpu->A + cpu->_D + !!cpu->P.C;
				cpu->P.V = cpu->P.C = 0;
				if (~(cpu->A ^ cpu->_D) & (cpu->A ^ sum) & 0x80)
					cpu->P.V = 1;
				_NZC(sum);
				cpu->A = sum;
			}
		}	break;
		case 0x38: case 0xF8: case 0x78:
		{ // SEC, SED, SEI
			MII_SET_P_BIT(cpu, d.s_bit, 1);
		}	break;
		case 0x85: case 0x95: case 0x8D: case 0x9D:
		case 0x99: case 0x81: case 0x91: case 0x92:
		{ // STA
			cpu->_D = cpu->A;cpu->cycle++;
		}	break;
		case 0x86: case 0x96: case 0x8E:
		{ // STX
			cpu->_D = cpu->X;
		}	break;
		case 0x84: case 0x94: case 0x8C:
		{ // STY
			cpu->_D = cpu->Y;
		}	break;
		case 0x64: case 0x74: case 0x9C: case 0x9E:
		{ // STZ
			cpu->_D = 0;
		}	break;
		case 0x14: case 0x1c:
		{	// TRB
			cpu->P.Z = !(cpu->A & cpu->_D);
			cpu->_D &= ~cpu->A;
		}	break;
		case 0x04: case 0x0c:
		{	// TSB
			cpu->P.Z = !(cpu->A & cpu->_D);
			cpu->_D |= cpu->A;
		}	break;
		case 0xAA:
		{ // TAX
			cpu->X = cpu->A;cpu->cycle++;
			_NZ(cpu->X);
		}	break;
		case 0xA8:
		{ // TAY
			cpu->Y = cpu->A;cpu->cycle++;
			_NZ(cpu->Y);
		}	break;
		case 0xBA:
		{ // TSX
			cpu->X = cpu->S;cpu->cycle++;
			_NZ(cpu->X);
		}	break;
		case 0x8A:
		{ // TXA
			cpu->A = cpu->X;cpu->cycle++;
			_NZ(cpu->A);
		}	break;
		case 0x9A:
		{ // TXS
			cpu->S = cpu->X;cpu->cycle++;
		}	break;
		case 0x98:
		{ // TYA
			cpu->A = cpu->Y;cpu->cycle++;
			_NZ(cpu->A);
		}	break;
		case 0x07: case 0x17: case 0x27: case 0x37: case 0x47:
		case 0x57: case 0x67: case 0x77: case 0x87: case 0x97:
		case 0xA7: case 0xB7: case 0xC7: case 0xD7: case 0xE7:
		case 0xF7:
		{ // RMB/SMB
			cpu->_D = (cpu->_D & ~(1 << d.s_bit)) | (d.s_bit_value << d.s_bit);
		}	break;
		/* Apparently these NOPs use 3 bytes, according to the tests */
		case 0x5c: case 0xdc: case 0xfc:
			_FETCH(cpu->PC++);
		/* Apparently these NOPs use 2 bytes, according to the tests */
		case 0x02: case 0x22: case 0x42: case 0x62: case 0x82:
		case 0xC2: case 0xE2: case 0x44: case 0x54: case 0xD4:
		case 0xF4:
			_FETCH(cpu->PC++);	// consume that byte
			break;
		case 0xdb: case 0xfb:
		// trap NOPs
			break;
		default:
			printf("%04x %02x UNKNOWN INSTRUCTION\n", cpu->PC, cpu->IR);
		//	exit(1);
			break;
	}
	if (d.w) {
		_STORE(cpu->_P, cpu->_D);
	}
	goto next_instruction;
	pt_end(cpu->state);
	return s;
}
