#include "opcode.h"
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#define C (1 << 0)
#define Z (1 << 1)
#define I (1 << 2)
#define S (1 << 3)
#define V (1 << 6)
#define N (1 << 7)

#define STK_STADDR 0x010000 /* Starting address of the stack. */
uint8_t *addr; /* Address Space. */
uint8_t update_pc = 1; /* Update the program counter. */
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:
			sum = (value < 64) ? a[thread] << value : 0;
			c = a[thread] >> 63;
			a[thread] = sum;
			break;
		/* Shift accumulator right. */
		case SRA:
			sum = (value < 64) ? a[thread] >> value : 0;
			c = a[thread] & 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 | (uint64_t)N << 8*thread) : (ps & ~((uint64_t)N << 8*thread));
	ps = (v) ? (ps | (uint64_t)V << 8*thread) : (ps & ~((uint64_t)V << 8*thread));
	ps = (z) ? (ps | (uint64_t)Z << 8*thread) : (ps & ~((uint64_t)Z << 8*thread));
	ps = (c) ? (ps | (uint64_t)C << 8*thread) : (ps & ~((uint64_t)C << 8*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) {
	uint64_t dif;
	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;
		/* Branch if positive. */
		case BPO:
			if (!(ps & ((uint64_t)N << 8*thread)))
				pc[thread] = value;
			else
				pc[thread]+=0;
			break;
		/* Branch if negative. */
		case BNG:
			if (ps & ((uint64_t)N << 8*thread))
				pc[thread] = value;
			else
				pc[thread]+=0;
			break;
		/* Branch if equal. */
		case BEQ:
			if (ps & ((uint64_t)Z << 8*thread))
				pc[thread] = value;
			else
				pc[thread]+=0;
			break;
		/* Branch if not equal. */
		case BNE:
			if (!(ps & ((uint64_t)Z << 8*thread)))
				pc[thread] = value;
			else
				pc[thread]+=0;
			break;
		/* Branch if carry set. */
		case BCS:
			if (ps & ((uint64_t)C << 8*thread))
				pc[thread] = value;
			else
				pc[thread]+=0;
			break;
		/* Branch if carry clear. */
		case BCC:
			if (!(ps & ((uint64_t)C << 8*thread)))
				pc[thread] = value;
			else
				pc[thread]+=0;
			break;
		/* Branch if overflow set. */
		case BVS:
			if (ps & ((uint64_t)V << 8*thread))
				pc[thread] = value;
			else
				pc[thread]+=0;
			break;
		/* Branch if overflow clear. */
		case BVC:
			if (!(ps & ((uint64_t)V << 8*thread)))
				pc[thread] = value;
			else
				pc[thread]+=0;
			break;
		/* Compare accumulator. */
		case CMP:
		case CAY:
		case CAX:
			if (opcode == CAY || opcode == CAX) {
				dif = (opcode == CAY) ? a[thread]-y[thread] : a[thread]-x[thread];
				value = a[thread];
			} else {
				dif = value-a[thread];
			}
			ps = (dif & 0x8000000000000000) ? (ps | (uint64_t)N << 8*thread) : (ps & ~((uint64_t)N << 8*thread));
			ps = (dif == 0) ? (ps | (uint64_t)Z << 8*thread) : (ps & ~((uint64_t)Z << 8*thread));
			ps = (dif > value) ? (ps | (uint64_t)C << 8*thread) : (ps & ~((uint64_t)C << 8*thread));
			break;
		/* Compare y register. */
		case CPY:
			dif = value-y[thread];
			ps = (dif & 0x8000000000000000) ? (ps | (uint64_t)N << 8*thread) : (ps & ~((uint64_t)N << 8*thread));
			ps = (dif == 0) ? (ps | (uint64_t)Z << 8*thread) : (ps & ~((uint64_t)Z << 8*thread));
			ps = (dif > value) ? (ps | (uint64_t)C << 8*thread) : (ps & ~((uint64_t)C << 8*thread));
			break;
		/* Compare x register. */
		case CPX:
			dif = value-x[thread];
			ps = (dif & 0x8000000000000000) ? (ps | (uint64_t)N << 8*thread) : (ps & ~((uint64_t)N << 8*thread));
			ps = (dif == 0) ? (ps | (uint64_t)Z << 8*thread) : (ps & ~((uint64_t)Z << 8*thread));
			ps = (dif > value) ? (ps | (uint64_t)C << 8*thread) : (ps & ~((uint64_t)C << 8*thread));
			break;
	}
	return 1;
}

int setflags(uint8_t opcode, uint8_t thread) {
	switch (opcode) {
		/* Clear processor status. */
		case CPS:
			ps &= 0;
			break;
		/* Set interupt flag. */
		case SEI:
			ps |= ((uint64_t)I << 8*thread);
			break;
		/* Clear interupt flag. */
		case CLI:
			ps &= ~((uint64_t)I << 8*thread);
			break;
		/* Set carry flag. */
		case SEC:
			ps |= ((uint64_t)C << 8*thread);
			break;
		/* Clear carry flag. */
		case CLC:
			ps &= ~((uint64_t)C << 8*thread);
			break;
		/* Set stack protection flag. */
		case SSP:
			ps |= ((uint64_t)S << 8*thread);
			break;
		/* Clear stack protection flag. */
		case CSP:
			ps &= ~((uint64_t)S << 8*thread);
			break;
		/* Set overflow flag. */
		case SEV:
			ps |= ((uint64_t)V << 8*thread);
			break;
		/* Clear overflow flag. */
		case CLV:
			ps &= ~((uint64_t)V << 8*thread);
			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[thread], value);
			if (opcode == PAY)
				push(y[thread], value);
			if (opcode == PAX)
				push(x[thread], value);
			break;
		case PHY:
			push(y[thread], value);
			break;
		case PHX:
			push(x[thread], value);
			break;
		/* Pull processor status. */
		case PLP:
			ps = pull(value);
			break;
		/* Pull accumulator. */
		case PLA:
		case PYA:
		case PXA:
			a[thread] = pull(value);
			if (opcode == PAY)
				y[thread] = pull(value);
			if (opcode == PAX)
				x[thread] = pull(value);
			break;
		/* Pull y register. */
		case PLY:
			y[thread] = pull(value);
			break;
		/* Pull x register. */
		case PLX:
			x[thread] = pull(value);
			break;
	}
	return 1;
}

int misc(uint8_t opcode, uint8_t thread) {
	switch (opcode) {
		/* No operation. */
		case NOP:
			break;
		/* TODO: Implement interupts. */
		case BRK:
			break;
	}
	return 1;
}

int main(int argc, char **argv) {
	a[0] = 0;
	x[0] = 0;
	y[0] = 0;
	addr = malloc(8*0x04000000);
	int v = 0;
	addr[0x8000] = 0x00;	/* CPS */
	addr[0x8001] = 0xE1;	/* LDA #$E1 */
	addr[0x8002] = 0x01;
	addr[0x8003] = 0x51;	/* SLA #$01 */
	addr[0x8004] = 0x01;
	addr[0x8005] = 0xF1;	/* STA $4000*/
	addr[0x8006] = 0x00;
	addr[0x8007] = 0x40;
	addr[0x8008] = 0x50;	/* BCS $8001 */
	addr[0x8009] = 0x01;
	addr[0x800A] = 0x80;
	addr[0x800B] = 0x10;	/* JMP $1003 */
	addr[0x800C] = 0x03;
	addr[0x800D] = 0x80;
	addr[0x8010] = 0xFF;	/* STP */
	addr[0xFFFC] = 0x00;
	addr[0xFFFD] = 0x80;
	pc[0] = addr[0xFFFC] | (addr[0xFFFD] << 8);
	uint64_t value = 0;
	uint8_t end = 0;
	uint8_t opcode = 0;
	uint8_t size;
	printf("\033[2J");
	while (!end) {
	printf("pc: 0x%04llx, a: 0x%016llx, carry: %u, inst: %s, value: 0x%04llx, $%04llx: %02x\n", pc[0], a[0], ps & 1, opname[opcode], value, value, addr[value]);
	opcode = addr[pc[0]];
	if (opcode == STA || opcode == JMP || opcode == BCS)
		size = 2;
	else if (opcode == CPS || opcode == NOP)
		size = 0;
	else if (opcode == STP)
		end = 1, size = 0;
	else
		size = 1;
	if(update_pc)
		pc[0]++;
	else
		update_pc = 1;
	if (size) {
		int i = 0;
		while (i < size) {
			if (!i)
				value = (addr[pc[0]]);
			else
				value |= (addr[pc[0]] << (8*i));
			printf("pc: 0x%04llx, a: 0x%016llx, carry: %u, inst: %s, value: 0x%04llx, $%04llx: %02x\n", pc[0], a[0], ps & 1, opname[opcode], value, value, addr[value]);
			if (update_pc)
				pc[0]++;
			else
				update_pc = 1;
			i++;
		}
	}
	if (opcode == SLA || opcode == ADC)
		alu(opcode, value, 0);
	if (opcode == JMP || opcode == BCS)
		branch(opcode, value, 0);
	memory(opcode, value, 0);
	printf("pc: 0x%04llx, a: 0x%016llx, carry: %u, inst: %s, value: 0x%04llx, $%04llx: %02x\n", pc[0], a[0], ps & 1, opname[opcode], value, value, addr[value]);
	}
	free(addr);
	return 0;

}