#include "opcode.h"
#include <stdint.h>
#define STK_STADDR 0x010000 /* Starting address of the stack. */
uint8_t *addr; /* Address Space. */
uint16_t sp; /* Stack pointer. */
uint64_t a[8]; /* Accumulator. */
uint64_t y[8]; /* Y index. */
uint64_t x[8]; /* X index. */
uint8_t crt; /* Current Running Threads. */
uint64_t pc[8]; /* Program counter. */
uint64_t ps; /* Processor status. */

uint64_t rol(uint64_t a, uint64_t value) {
	const uint64_t mask = 8 * sizeof(a) - 1;
	value &= mask;
	return (a << value) | (a >> (-value & mask));
}

uint64_t ror(uint64_t a, uint64_t value) {
	const uint64_t mask = 8 * sizeof(a) - 1;
	value &= mask;
	return (a >> value) | (a << (-value & mask));
}

int push(uint64_t value, uint8_t size) {
	for (uint8_t i = 0; i < size; i++) {
		addr[STK_STADDR+sp] = value >> 8*i;
		sp--;
	}
	return 1;
}

uint64_t pull(uint8_t size) {
	uint64_t value;
	for (uint8_t i = 0; i < size; i++) {
		value = addr[STK_STADDR+sp] << 8*i;
		sp++;
	}
	return value;
}

int alu(uint8_t opcode, uint64_t value, uint8_t thread) {
	uint64_t sum;
	uint8_t c = 0; /* Carry flag. */
	uint8_t v = 0; /* Overflow flag. */
	uint8_t z = 0; /* Zero flag. */
	uint8_t n = 0; /* Negative flag. */
	switch(opcode) {
		/* Add with carry. */
		case ADC:
			c = (ps & (1 << thread)) >> thread;
			sum = a[thread]+value+c;
			v = !((a[thread]^value) & 0x8000000000000000) && ((a[thread]^sum) & 0x8000000000000000);
			c = (sum < value);
			a[thread] = sum;
			break;
		/* Subtract with carry. */
		case SBC:
			c = !(ps & (1 << thread));
			sum = a[thread]-value-c;
			v = ((a[thread]^value) & 0x8000000000000000) && ((a[thread]^sum) & 0x8000000000000000);
			c = (sum > value);
			a[thread] = sum;
			break;
		/* Multiply with accumulator. */
		case MUL:
			c = (ps & (1 << thread)) >> thread;
			sum = a[thread]*value+c;
			v = !((a[thread]^value) & 0x8000000000000000) && ((a[thread]^sum) & 0x8000000000000000);
			c = (!((a[thread]^sum) && (a[thread]^value)) || (a[thread] >= ((uint64_t)1 << 32) && value >= ((uint64_t)1 << 32)));
			a[thread] = sum;
			break;
		/* Divide with accumulator. */
		case DIV:
			sum = a[thread]/value;
			v = ((a[thread]^value) & 0x8000000000000000) && ((a[thread]^sum) & 0x8000000000000000);
			a[thread] = sum;
			break;
		/* Bitwise AND. */
		case AND:
		case AAY:
		case AAX:
			a[thread] &= value;
			if (opcode == AAY)
				y[thread] &= value;
			if (opcode == AAX)
				x[thread] &= value;
			break;
		case ANY:
			y[thread] &= value;
			break;
		case ANX:
			x[thread] &= value;
			break;
		/* Bitwise OR. */
		case ORA:
		case OAY:
		case OAX:
			a[thread] |= value;
			if (opcode == OAY)
				y[thread] |= value;
			if (opcode == OAX)
				x[thread] |= value;
			break;
		case ORY:
			y[thread] |= value;
			break;
		case ORX:
			x[thread] |= value;
			break;
		/* Bitwise Exclusive OR. */
		case XOR:
		case XAY:
		case XAX:
			a[thread] ^= value;
			if (opcode == XAY)
				y[thread] ^= value;
			if (opcode == XAX)
				x[thread] ^= value;
			break;
		case XRY:
			y[thread] ^= value;
			break;
		case XRX:
			x[thread] ^= value;
			break;
		/* Shift accumulator left. */
		case SLA:
			c = (ps & (1 << thread)) >> thread;
			if (value > 1)
				c = ((a << value-1) & 0x8000000000000000) >> 63;
			sum = (value < 64) ? a[thread] << value : 0;
			a[thread] = sum;
			break;
		/* Shift accumulator right. */
		case SRA:
			c = (ps & (1 << thread)) >> thread;
			if (value > 1)
				c = (a >> value-1) & 1;
			sum = (value < 64) ? a << value : 0;
			c = sum & 1;
			a[thread] = sum;
			break;
		/* Rotate accumulator left. */
		case ROL:
			a[thread] = rol(a[thread], value);
			break;
		/* Rotate accumulator right. */
		case ROR:
			a[thread] = ror(a[thread], value);
			break;
		/* Increment accumulator. */
		case INC:
		case IAY:
		case IAX:
			a[thread]++;
			if (opcode == IAY)
				y[thread]++;
			if[opcode == IAX]
				x[thread]++;
			break;
		/* Increment y register. */
		case INY:
			y[thread]++;
			break;
		/* Increment x register. */
		case INX:
			x[thread]++;
			break;
		/* Decrement accumulator. */
		case DEC:
		case DAY:
		case DAX:
			a[thread]--;
			if (opcode == DAY)
				y[thread]--;
			if[opcode == DAX]
				x[thread]--;
			break;
		/* Decrement y register. */
		case DEY:
			y[thread]--;
			break;
		/* Decrement y register. */
		case DEX:
			x[thread]--;
			break;
		default:
			printf("Cool, you inputed a non existent opcode, which means\n"
				"that you have now wasted clock cycles.\n"
				"Good job! *clap*\n");
			break;
	}
	z = (a[thread] == 0);
	n = (a[thread] >> 63);
	ps = (n) ? (ps | 1 << 7+thread) : (ps & ~(1 << 7+thread));
	ps = (v) ? (ps | 1 << 6+thread) : (ps & ~(1 << 6+thread));
	ps = (z) ? (ps | 1 << 1+thread) : (ps & ~(1 << 1+thread));
	ps = (c) ? (ps | 1 << thread) : (ps & ~(1 << thread));
	return 1;
}

int threads(uint8_t opcode, uint64_t value) {
	uint8_t t = value & 0xFF; /* This tells the CPU, which threads to start, or end. */
	switch(opcode) {
		/* Start Thread. */
		case STT:
			crt |= t;
			for (uint8_t i = 0; i < 7; i++)
				if ((t >> i) & 1)
					pc[i] = value >> 8;
			break;
		/* End Thread. */
		case ENT:
			crt &= ~t;
			for (uint8_t i = 0; i < 7; i++)
				if ((t >> i) & 1)
					pc[i] = 0;
			break;
	}
	return 1;
}

int branch(uint8_t opcode, uint64_t value, uint8_t thread) {
	switch (opcode) {
		/* Jump. */
		case JMP:
			pc[thread] = value;
			break;
		/* Jump to subroutine. */
		case JSR:
			push(pc[thread], 8);
			pc[thread] = value;
			break;
		/* Return from subroutine. */
		case RTS:
			pc[thread] = pull(8);
			break;
	}
	return 1;
}

int memory(uint8_t opcode, uint64_t value, uint8_t thread) {
	switch (opcode) {
		/* Load value. */
		case LDA:  /* LDA, imm */
		case LDY:  /* LDY, imm */
		case LAY:  /* LAY, imm */
		case LDX:  /* LDX, imm */
		case LAX:  /* LAX, imm */
		case 0x95: /* LDA, abs */
		case 0x25: /* LDY, abs */
		case 0x35: /* LAY, abs */
		case 0x45: /* LDX, abs */
		case 0x55: /* LAX, abs */
			if (opcode == LDA || opcode == LAY || opcode == LAX)
				a[thread] = value;
			else if (opcode == 0x95 || opcode == 0x35 || opcode == 0x55)
				a[thread] = addr[value];
			if (opcode == LAY || opcode == LDY)
				y[thread] = value;
			else if (opcode == 0x35)
				y[thread] = addr[value];
			if (opcode == LAX)
				x[thread] = value;
			else if (opcode == 0x55)
				x[thread] = addr[value];
		/* Store value. */
		case STA:
		case SAY:
		case SAX:
			addr[value] = a[thread];
			if (opcode == SAY)
				addr[value] = y[thread];
			if (opcode == SAX)
				addr[value] = x[thread];
			break;
		case STY:
			addr[value] = y[thread];
			break;
		case STX:
			addr[value] = x[thread];
			break;
		/* Push processor status. */
		case PHP:
			push(ps, value);
			break;
		/* Push accumulator. */
		case PHA:
		case PAY:
		case PAX:
			push(a, value);
			if (opcode == PAY)
				push(y, value);
			if (opcode == PAX)
				push(x, value);
			break;
		case PHY:
			push(y, value);
			break;
		case PHX:
			push(x, value);
			break;
		/* Pull processor status. */
		case PLP:
			ps = pull(value);
			break;
		/* Pull accumulator. */
		case PLA:
		case PYA:
		case PXA:
			a = pull(value);
			if (opcode == PAY)
				y = pull(value);
			if (opcode == PAX)
				x = pull(value);
			break;
		/* Pull y register. */
		case PLY:
			y = pull(value);
			break;
		/* Pull x register. */
		case PLX:
			x = pull(value);
			break;
	}
	return 1;
}