#include "opcode.h"
#include <pthread.h>
#include <ctype.h>
#include <string.h>
#include <limits.h>

#if bench
#include <sys/time.h>
#include <signal.h>
#include <math.h>
#endif
#include <curses.h>

#define THREADS	1
#define BENCH_INST 100000000 << (THREADS-1)
#define CTRL_ADDR 0x100
#define TX_ADDR 0x101
#define RX_ADDR 0x102
#define STEP_ADDR 0x110
#define CURSES_BACKSPACE 0x7F

#define copy64 1

extern uint8_t kbd_rdy;

extern WINDOW *scr;

#if debug
extern uint8_t subdbg;
#endif

static const uint64_t mem_size = 0x04000000; /* Size of address space. */

extern uint8_t step;
extern uint8_t end;

#define setflag(flag, bit) ((flag)) ? (cpu->ps.u8[thread] |= bit) : (cpu->ps.u8[thread] &= ~bit)
#define getflag(bit) (cpu->ps.u8[thread] & bit)

extern pthread_mutex_t mutex;
extern pthread_mutex_t main_mutex;
extern pthread_cond_t cond;
extern pthread_cond_t main_cond;

#if debug
extern void disasm(struct sux *cpu, uint8_t lines, uint8_t opcode, uint8_t prefix, uint8_t thread);
#endif

/*#define KEYBUF_SIZE 0x40
char key[KEYBUF_SIZE];*/

extern int get_key(WINDOW *scr);
extern void io(uint64_t address, uint8_t rw);
extern void init_scr();


static void *memcopy(void *restrict dst, const void *restrict src, unsigned int n) {
	#if copy64
	uint64_t *d = dst;
	const uint64_t *s = src;
	unsigned int r = n % 8;
	n /= 8;
	#else
	uint8_t *d = dst;
	const uint8_t *s = src;
	#endif
	for (; n; *d++ = *s++, n--);
	#if copy64
    	if (r) {
		uint64_t mask = (-(uint64_t)1 >> ((8 - r) * 8));
		*d = (*d & ~mask) | (*s & mask);
	}
	#endif
	return dst;
}


static inline uint64_t read_value(struct sux *cpu, uint64_t reg, uint64_t address, uint8_t size, uint8_t inc_clk, uint8_t check_io) {
	#if (IO || debug) && !branch
	#if keypoll
	pthread_mutex_lock(&mutex);
	#endif
	if (check_io) {
		io(address, 1);
	}
	#if keypoll
	pthread_mutex_unlock(&mutex);
	#endif
	#endif
	#if getclk
	cpu->clk += inc_clk;
	#endif
	size = (size > 7) ? 7 : size;
	#if 1
	if (size < 7) {
		uint64_t mask = (-(uint64_t)1 >> ((7 - size) * 8));
		return (reg & ~mask) | (*(uint64_t *)(addr+address) & mask);
	} else {
		return *(uint64_t *)(addr+address);
	}
	#else
	return *(uint64_t *)memcopy(&reg, addr+address, size+1);
	#endif
}

static inline void write_value(struct sux *cpu, uint64_t value, uint64_t address, uint8_t size, uint8_t inc_clk, uint8_t check_io) {
	size = (size > 7) ? 7 : size;
	#if 1
	if (size < 7) {
		uint64_t mask = (-(uint64_t)1 >> ((7 - size) * 8));
		*(uint64_t *)(addr+address) = (*(uint64_t *)(addr+address) & ~mask) | (value & mask);
	} else {
		*(uint64_t *)(addr+address) = value;
	}
	#else
	memcopy(addr+address, &value, size+1);
	#endif
	#if (IO || debug) && !branch
	#if keypoll
	pthread_mutex_lock(&mutex);
	#endif
	if (check_io) {
		io(address, 0);
	}
	#if keypoll
	pthread_mutex_unlock(&mutex);
	#endif
	#endif
	#if getclk
	cpu->clk += inc_clk;
	#endif
}

static inline uint64_t offset_addr(struct sux *cpu, uint64_t offset, uint8_t size, uint8_t inc_clk, uint8_t prefix) {
	uint64_t of;
	switch (prefix >> 6) {
		case 1: of = ((cpu->stk_st << 16) | cpu->sp); break;
		case 2: of =                        cpu->pc ; break;
	}
	#if getclk
	cpu->clk += inc_clk;
	#endif
	switch (size) {
		case 0: return of + (int8_t )offset;
		case 1: return of + (int16_t)offset;
		case 2:
		case 3: return of + (int32_t)offset;
		case 4:
		case 5:
		case 6:
		case 7: return of + (int64_t)offset;
	}
}

static inline uint64_t imm_addr(struct sux *cpu) {
	return cpu->pc;
}

static inline uint64_t read_addr(struct sux *cpu, uint8_t prefix, uint8_t inc_clk, uint8_t type, uint8_t inc_pc) {
	uint64_t address;
	uint8_t size = get_addrsize(prefix, type);
	if (prefix >> 6) {
		address = offset_addr(cpu, read_value(cpu, 0, cpu->pc, size, inc_clk, 0), size, inc_clk, prefix);
	} else {
		address = read_value(cpu, 0, cpu->pc, size, inc_clk, 0);
	}
	if (inc_pc) {
		cpu->pc += size+1;
	}
	return address;
}

static inline uint64_t zmx_addr(struct sux *cpu, uint8_t prefix, uint8_t inc_clk, uint8_t inc_pc) {
	#if getclk
	cpu->clk += inc_clk;
	#endif
	return read_addr(cpu, prefix, inc_clk, ZM, inc_pc) + cpu->x;
}

static inline uint64_t zmy_addr(struct sux *cpu, uint8_t prefix, uint8_t inc_clk, uint8_t inc_pc) {
	#if getclk
	cpu->clk += inc_clk;
	#endif
	return read_addr(cpu, prefix, inc_clk, ZM, inc_pc) + cpu->y;
}


static inline uint64_t ind_addr(struct sux *cpu, uint8_t prefix, uint8_t inc_clk, uint8_t inc_pc) {
	return read_value(cpu, 0, read_addr(cpu, prefix, inc_clk, ZM, inc_pc), 7, inc_clk, 0);
}

static inline uint64_t indx_addr(struct sux *cpu, uint8_t prefix, uint8_t inc_clk, uint8_t inc_pc) {
	#if getclk
	cpu->clk += inc_clk;
	#endif
	return read_value(cpu, 0, read_addr(cpu, prefix, inc_clk, ZM, inc_pc)+cpu->x, 7, inc_clk, 0);
}

static inline uint64_t indy_addr(struct sux *cpu, uint8_t prefix, uint8_t inc_clk, uint8_t inc_pc) {
	#if getclk
	cpu->clk += inc_clk;
	#endif
	return ind_addr(cpu, prefix, inc_clk, inc_pc) + cpu->y;
}

static inline uint64_t rel_addr(struct sux *cpu, uint8_t prefix, uint8_t inc_clk, uint8_t inc_pc) {
	uint8_t rs   = (prefix >> 4) & 3;
	uint8_t size = (1 << rs) - 1;
	uint64_t offset = read_value(cpu, 0, cpu->pc, size, inc_clk, 0);
	uint64_t address;
	if (inc_pc) {
		cpu->pc += (size + 1);
	}
	#if getclk
	cpu->clk += inc_clk;
	#endif
	switch (rs) {
		default: return cpu->pc + (int8_t )offset;
		case 1 : return cpu->pc + (int16_t)offset;
		case 2 : return cpu->pc + (int32_t)offset;
		case 3 : return cpu->pc + (int64_t)offset;
	}
}

static inline uint64_t get_addr(struct sux *cpu, uint8_t opcode, uint8_t prefix, uint8_t inc_pc, uint8_t inc_clk, uint8_t thread) {
	uint64_t address = 0;
	switch (optype[opcode]) {
		case BREG:
		case IMPL:
			break;
		case IMM:
			address = imm_addr(cpu);
			switch (opcode) {
				case LSL_IMM:
				case LSR_IMM:
				case ROL_IMM:
				case ROR_IMM:
				case ASR_IMM:
					if (inc_pc) {
						++cpu->pc;
					}
					break;
				default:
					if (inc_pc) {
						cpu->pc+=(1 << ((prefix >> 4) & 3));
					}
					/* Falls Through. */
				case TXS_IMM: break;
			}
			break;
		case ZM  : return read_addr(cpu, prefix, inc_clk,  ZM, inc_pc);
		case ABS : return read_addr(cpu, prefix, inc_clk, ABS, inc_pc);
		case ZMX : return  zmx_addr(cpu, prefix, inc_clk, /**/ inc_pc);
		case ZMY : return  zmy_addr(cpu, prefix, inc_clk, /**/ inc_pc);
		case IND : return  ind_addr(cpu, prefix, inc_clk, /**/ inc_pc);
		case INDX: return indx_addr(cpu, prefix, inc_clk, /**/ inc_pc);
		case INDY: return indy_addr(cpu, prefix, inc_clk, /**/ inc_pc);
		case REL : return  rel_addr(cpu, prefix, inc_clk, /**/ inc_pc);

	}
	return address;
}


static inline uint64_t adc(struct sux *cpu, uint64_t reg, uint64_t value, uint8_t thread) {
	uint64_t sum = reg+value+getflag(C);
	setflag(sum == 0, Z);
	setflag((sum >> 63), N);
	setflag(((reg^value) >> 63) && ((reg^sum) >> 63), V);
	setflag((sum < value), C);
	return sum;
}
static inline uint64_t sbc(struct sux *cpu, uint64_t reg, uint64_t value, uint8_t thread) {
	uint64_t sum = reg-value-!getflag(C);
	setflag(sum == 0, Z);
	setflag(sum >> 63, N);
	setflag(((reg^value) >> 63) && ((reg^sum) >> 63), V);
	setflag((sum > value), C);
	return sum;
}

static inline uint64_t transfer(struct sux *cpu, uint64_t src, uint64_t value, uint8_t opcode, uint8_t prefix, uint8_t thread) {
	switch (opcode) {
                case TXS_IMM:
			if (prefix == 0x13 && (value == thread+1 || value > 8)) {
				cpu->stk_st = value & 0xFF;
				cpu->stk_st += value << 16;
				cpu->pc+=2;
			}
		default: break;
	}
	setflag(src == 0, Z);
	setflag(src >> 63, N);
	return src;
}

static inline void push(struct sux *cpu, uint64_t value, uint8_t size, uint8_t thread) {
	uint64_t sbr = (cpu->stk_st << 16);
	write_value(cpu, value, (sbr+cpu->sp)-size, size, 1, 0);
	cpu->sp -= size+1;
}

static inline uint64_t pull(struct sux *cpu, uint8_t size, uint8_t thread) {
	uint64_t sbr = (cpu->stk_st << 16);
	uint64_t value = read_value(cpu, 0, sbr+cpu->sp+1, size, 1, 0);
	cpu->sp += size+1;
	return value;
}

static inline uint64_t and(struct sux *cpu, uint64_t reg, uint64_t value, uint8_t thread) {
	reg &= value;
	setflag(reg == 0, Z);
	setflag(reg >> 63, N);
	return reg;
}
static inline uint64_t or(struct sux *cpu, uint64_t reg, uint64_t value, uint8_t thread) {
	reg |= value;
	setflag(reg == 0, Z);
	setflag(reg >> 63, N);
	return reg;
}
static inline uint64_t xor(struct sux *cpu, uint64_t reg, uint64_t value, uint8_t thread) {
	reg ^= value;
	setflag(reg == 0, Z);
	setflag(reg >> 63, N);
	return reg;
}

static inline uint64_t lsl(struct sux *cpu, uint64_t reg, uint64_t value, uint8_t thread) {
	uint64_t sum = (value < 64) ? reg << value : 0;
	setflag(sum == 0, Z);
	setflag(sum >> 63, N);
	setflag(reg >> (64-value), C);
	return sum;
}

static inline uint64_t lsr(struct sux *cpu, uint64_t reg, uint64_t value, uint8_t thread) {
	uint64_t sum = (value < 64) ? reg >> value : 0;
	setflag(sum == 0, Z);
	setflag(sum >> 63, N);
	setflag(reg & 1, C);
	return sum;
}

static inline uint64_t asr(struct sux *cpu, uint64_t reg, uint64_t value, uint8_t thread) {
	uint8_t sign = reg >> 63;
	uint64_t sum = (value < 64) ? (reg >> value) | ((uint64_t)sign << 63) : 0;
	setflag(sum == 0, Z);
	setflag(sum >> 63, N);
	setflag(reg & 1, C);
	return sum;
}

static inline uint64_t rol(struct sux *cpu, uint64_t reg, uint64_t value, uint8_t thread) {
	uint64_t sum = reg << value;
	sum |= getflag(C);
	setflag(sum == 0, Z);
	setflag(sum >> 63, N);
	setflag(reg >> (uint64_t)(64-value), C);
	return sum;
}

static inline uint64_t ror(struct sux *cpu, uint64_t reg, uint64_t value, uint8_t thread) {
	uint64_t sum = reg >> value;
	sum |= (uint64_t)getflag(C) << (uint64_t)(64-value);
	setflag(sum == 0, Z);
	setflag(sum >> 63, N);
	setflag(reg & 1, C);
	return sum;
}

static inline uint64_t mul(struct sux *cpu, uint64_t reg, uint64_t value, uint8_t thread) {
	uint64_t sum = reg*value;
	setflag(sum == 0, Z);
	setflag(sum >> 63, N);
	setflag(!((reg^value) >> 63) && ((reg^sum) >> 63), V);
	return sum;
}

static inline uint64_t divd(struct sux *cpu, uint64_t reg, uint64_t value, uint64_t *rem, uint8_t thread) {
	uint64_t sum = reg/value;
	*rem = reg % value;
	setflag(sum == 0, Z);
	setflag((sum >> 63), N);
	return sum;
}
static inline void cmp(struct sux *cpu, uint64_t value, uint64_t reg, uint8_t thread) {
	uint64_t sum = reg-value;
	setflag(sum >> 63, N);
	setflag(((reg^value) >> 63) && ((reg^sum) >> 63), V);
	setflag(sum == 0, Z);
	setflag(reg >= value, C);
}

/* Increment, or Decrement register. */
static inline uint64_t idr(struct sux *cpu, uint64_t reg, uint8_t inc, uint8_t thread) {
	reg += (inc) ? 1 : -1;
	setflag(reg == 0, Z);
	setflag(reg >> 63, N);
	return reg;
}

/* Increment, or Decrement memory. */
static inline void idm(struct sux *cpu, uint64_t address, uint8_t prefix, uint8_t inc, uint8_t thread) {
	uint8_t size = (1 << ((prefix >> 4) & 3))-1;
	uint64_t value;
	value = read_value(cpu, 0, address, size, 1, 0);
	value += (inc) ? 1 : -1;
	uint8_t sign = 0;
	switch ((prefix >> 4) & 3) {
		default: sign =  7; break;
		case  1: sign = 15; break;
		case  2: sign = 31; break;
		case  3: sign = 63; break;
	}
	setflag(value == 0, Z);
	setflag(value >> sign, N);
	write_value(cpu, value, address, size, 1, 1);
}

static inline uint64_t load(struct sux *cpu, uint64_t reg, uint64_t address, uint8_t size, uint8_t thread) {
	uint64_t value = read_value(cpu, reg, address, size, 1, 1);
	setflag(value == 0, Z);
	setflag(value >> 63, N);
	return value;
}

static inline void store(struct sux *cpu, uint64_t address, uint64_t reg, uint8_t prefix, uint8_t thread) {
	uint8_t size = (1 << ((prefix >> 4) & 3))-1;
	write_value(cpu, reg, address, size, 1, 1);
}