//
// Created by bruno on 2.2.2025.
//

#include "core.h"
#include "memory.h"
#include "string.h"

// Initialize CPU
void init_cpu(CPU *cpu) {
    memset(cpu, 0, sizeof(CPU));
    cpu->sp = MEM_SIZE - 1; // Stack grows downward
}

// Helper function for setting flags in the CPU (here we assume bit0 is the Zero flag,
// and bit1 is the Negative flag).
static inline void set_flags(CPU *cpu, int32_t result) {
    cpu->flags = 0;
    if (result == 0)
        cpu->flags |= 0x01;  // Zero flag
    if (result < 0)
        cpu->flags |= 0x02;  // Negative flag
}

// Execute a program (a byte array) on the given CPU.
void step(CPU *cpu) {
    if (cpu->mode < EverySecond) {
        return;
    }
    if (cpu->pc >= MEM_SIZE) {
        cpu->mode = Done; //terminate
    }
    uint8_t opcode = read_mem(cpu, cpu->pc++);
    uint8_t reg1, reg2, imm;
    uint32_t temp, newPC;
    int32_t cmpResult;
    switch (opcode) {
        case NOP:
            cpu->pc++;
            break;

        case BRK:
            cpu->pc++;
            cpu->mode = Paused;
            break;

        case INC_RN:
            cpu->pc++;
            reg1 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1]++;

        case INC_ADDR:
            cpu->pc++;
            imm = read_mem(cpu, cpu->pc++);
            write_mem(cpu, imm, read_mem(cpu, imm) + 1);

        case DEC_RN:
            cpu->pc++;
            reg1 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1]--;

        case DEC_ADDR:
            cpu->pc++;
            imm = read_mem(cpu, cpu->pc++);
            write_mem(cpu, imm, read_mem(cpu, imm) - 1);

        case MOV_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] = imm;
            break;

        case MOV_RN_RM:
            reg1 = read_mem(cpu, cpu->pc++);
            reg2 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] = cpu->regs[reg2];
            break;

        case MOV_RN_ADDR:
            // Load from memory into register.
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] = read_mem(cpu, imm);
            break;

        case MOV_ADDR_RN:
            // Store from register into memory.
            imm = read_mem(cpu, cpu->pc++);
            reg1 = read_mem(cpu, cpu->pc++);
            write_mem(cpu, imm, cpu->regs[reg1]);
            break;

        case SWAP: {
            // Swap contents of two registers.
            reg1 = read_mem(cpu, cpu->pc++);
            reg2 = read_mem(cpu, cpu->pc++);
            temp = cpu->regs[reg1];
            cpu->regs[reg1] = cpu->regs[reg2];
            cpu->regs[reg2] = temp;
            break;
        }

        case SWAPN: {
            // Swap the nibbles of a register.
            reg1 = read_mem(cpu, cpu->pc++);
            uint8_t val = (uint8_t) cpu->regs[reg1];
            cpu->regs[reg1] = ((val & 0x0F) << 4) | ((val & 0xF0) >> 4);
            break;
        }

        case ADD_RN_RM:
            reg1 = read_mem(cpu, cpu->pc++);
            reg2 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] += cpu->regs[reg2];
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case ADD_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] += imm;
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case SUB_RN_RM:
            reg1 = read_mem(cpu, cpu->pc++);
            reg2 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] -= cpu->regs[reg2];
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case SUB_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] -= imm;
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case MUL_RN_RM:
            reg1 = read_mem(cpu, cpu->pc++);
            reg2 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] *= cpu->regs[reg2];
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case MUL_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] *= imm;
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case DIV_RN_RM:
            reg1 = read_mem(cpu, cpu->pc++);
            reg2 = read_mem(cpu, cpu->pc++);
            if (cpu->regs[reg2] == 0) {
                printf("Error: Division by zero!\n");
                cpu->mode = Error;
                return;
            }
            cpu->regs[reg1] /= cpu->regs[reg2];
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case DIV_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            if (imm == 0) {
                printf("Error: Division by zero!\n");
                cpu->mode = Error;
                return;
            }
            cpu->regs[reg1] /= imm;
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case MOD_RN_RM:
            reg1 = read_mem(cpu, cpu->pc++);
            reg2 = read_mem(cpu, cpu->pc++);
            if (cpu->regs[reg2] == 0) {
                printf("Error: Modulo by zero!\n");
                cpu->mode = Error;
                return;
            }
            cpu->regs[reg1] %= cpu->regs[reg2];
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case MOD_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            if (imm == 0) {
                printf("Error: Modulo by zero!\n");
                cpu->mode = Error;
                return;
            }
            cpu->regs[reg1] %= imm;
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case NEG_RN:
            reg1 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] = -((int32_t) cpu->regs[reg1]);
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case AND_RN_RM:
            reg1 = read_mem(cpu, cpu->pc++);
            reg2 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] &= cpu->regs[reg2];
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case AND_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] &= imm;
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case OR_RN_RM:
            reg1 = read_mem(cpu, cpu->pc++);
            reg2 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] |= cpu->regs[reg2];
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case OR_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] |= imm;
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case XOR_RN_RM:
            reg1 = read_mem(cpu, cpu->pc++);
            reg2 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] ^= cpu->regs[reg2];
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case XOR_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] ^= imm;
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case NOT_RN:
            reg1 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] = ~cpu->regs[reg1];
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case SHL_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] <<= imm;
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case SHR_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] >>= imm;  // Logical right shift (assuming unsigned value)
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case SAR_RN_IMM:
            reg1 = read_mem(cpu, cpu->pc++);
            imm = read_mem(cpu, cpu->pc++);
            // Arithmetic right shift; cast to signed before shifting.
            cpu->regs[reg1] = ((int32_t) cpu->regs[reg1]) >> imm;
            set_flags(cpu, cpu->regs[reg1]);
            break;

        case JMP:
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            cpu->pc = newPC;
            break;

        case JMP_REL:
            imm = (int32_t) read_mem(cpu, cpu->pc++);
            cpu->pc += imm;
            break;

        case CMP: {
            // Compare two registers: set flags (Zero, Negative)
            reg1 = read_mem(cpu, cpu->pc++);
            reg2 = read_mem(cpu, cpu->pc++);
            cmpResult = (int32_t) cpu->regs[reg1] - (int32_t) cpu->regs[reg2];
            set_flags(cpu, cmpResult);
            break;
        }

        case JE_BIT_RN: {
            // Jump if bit in register set
            reg1 = read_mem(cpu, cpu->pc++);
            if (reg1 >= REG_COUNT) {
                reg1 = REG_COUNT - 1;
            }
            uint8_t bit = read_mem(cpu, cpu->pc++);
            if (bit > 7) {
                bit = 7;
            }
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            if (cpu->regs[reg1] & (1 << bit))
                cpu->pc = newPC;
            break;
        }

        case JE_BIT_ADDR: {
            // Jump if bit in register set
            temp = read_mem32(cpu, cpu->pc);
            if (temp >= MEM_SIZE) {
                temp = MEM_SIZE - 1;
            }
            uint8_t bit = read_mem(cpu, cpu->pc++);
            if (bit > 7) {
                bit = 7;
            }
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            if (cpu->memory[temp] & (1 << bit))
                cpu->pc = newPC;
            break;
        }


        case JE: {
            // Jump if equal (Zero flag set)
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            if (cpu->flags & 0x01)
                cpu->pc = newPC;
            break;
        }

        case JNE_BIT_RN: {
            // Jump if bit in register set
            reg1 = read_mem(cpu, cpu->pc++);
            if (reg1 >= REG_COUNT) {
                reg1 = REG_COUNT - 1;
            }
            uint8_t bit = read_mem(cpu, cpu->pc++);
            if (bit > 7) {
                bit = 7;
            }
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            if (!(cpu->regs[reg1] & (1 << bit)))
                cpu->pc = newPC;
            break;
        }

        case JNE_BIT_ADDR: {
            // Jump if bit in register set
            temp = read_mem32(cpu, cpu->pc);
            if (temp >= MEM_SIZE) {
                temp = MEM_SIZE - 1;
            }
            uint8_t bit = read_mem(cpu, cpu->pc++);
            if (bit > 7) {
                bit = 7;
            }
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            if (!(cpu->memory[temp] & (1 << bit)))
                cpu->pc = newPC;
            break;
        }

        case JNE: {
            // Jump if not equal (Zero flag clear)
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            if (!(cpu->flags & 0x01))
                cpu->pc = newPC;
            break;
        }

        case JG: {
            // Jump if greater: not negative and not zero.
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            if (((cpu->flags & 0x02) == 0) && ((cpu->flags & 0x01) == 0))
                cpu->pc = newPC;
            break;
        }

        case JL: {
            // Jump if less: Negative flag set.
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            if (cpu->flags & 0x02)
                cpu->pc = newPC;
            break;
        }

        case JGE: {
            // Jump if greater or equal: Zero flag set or negative clear.
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            if ((cpu->flags & 0x01) || ((cpu->flags & 0x02) == 0))
                cpu->pc = newPC;
            break;
        }

        case JLE: {
            // Jump if less or equal: Negative flag set or Zero flag set.
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            if ((cpu->flags & 0x02) || (cpu->flags & 0x01))
                cpu->pc = newPC;
            break;
        }

        case CALL: {
            // Push the current PC onto the stack, then jump to the address.
            newPC = read_mem32(cpu, cpu->pc);
            cpu->pc += 4;
            // Push return address (current PC) onto the stack.
            write_mem(cpu, cpu->sp--, (uint8_t) (cpu->pc & 0xFF));
            cpu->pc = newPC;
            break;
        }

        case RET:
            // For RET, assume that the return address was stored on the stack.
            cpu->pc = read_mem(cpu, ++cpu->sp);
            break;

        case PUSH: {
            // Push register value onto the stack.
            reg1 = read_mem(cpu, cpu->pc++);
            write_mem(cpu, cpu->sp--, (uint8_t) (cpu->regs[reg1] & 0xFF));
            break;
        }

        case POP: {
            // Pop a value from the stack into a register.
            reg1 = read_mem(cpu, cpu->pc++);
            cpu->regs[reg1] = read_mem(cpu, ++cpu->sp);
            break;
        }

        case PUSHF:
            // Push the flags register.
            write_mem(cpu, cpu->sp--, cpu->flags);
            break;

        case POPF:
            // Pop into the flags register.
            cpu->flags = read_mem(cpu, ++cpu->sp);
            break;

        default:
            printf("Unknown opcode: %d\n", opcode);
            cpu->mode = Error;
    }
}