#include "emu.h"

static inline Instruction fetchInst(Emulator* emu) {
    return decodeInstruction(emu->mem, &emu->pc);
}

static inline void updateFlagsRegister(Emulator* emu, int32_t value) {
    if (value == 0) {
        emu->regs[REG_F] = FLAG_ZERO;
    } else if (value < 0) {
        emu->regs[REG_F] = FLAG_NEGATIVE;
    } else if (value > UINT16_MAX) {
        emu->regs[REG_F] = FLAG_OVERFLOW;
    }
}

static inline void updateDevices(Emulator* emu) {
    for (size_t i = 0; i < MAX_DEVICES; i++) {
        Device* device = emu->devices[i];
        if (!device) continue;

        if (device->update) {
            device->update(device);
        }
    }
}

void startEmulator(Emulator* emu) {
    static void* dispatchTable[] = {
        &&MOV,
        &&MVI,
        &&ADD,
        &&SUB,
        &&LOAD,
        &&STORE,
        &&AND,
        &&OR,
        &&XOR,
        &&SHF,
        &&JMP,
        &&JIZ,
        &&CALL,
        &&RET,
        &&PORT,
        &&HLT
    };

    emu->halted = false;
    Instruction inst;

    #define DISPATCH() updateDevices(emu); \
                       inst = fetchInst(emu); \
                       goto *dispatchTable[inst.opcode]

    DISPATCH();

    MOV: {
        emu->regs[inst.operands[0]] = emu->regs[inst.operands[1]];
        DISPATCH();
    }
    MVI: {
        emu->regs[inst.operands[0]] = inst.immediate;
        DISPATCH();
    }
    ADD: {
        int32_t result = emu->regs[inst.operands[0]] + emu->regs[inst.operands[1]];
        updateFlagsRegister(emu, result);
        emu->regs[inst.operands[0]] = result;
        DISPATCH();
    }
    SUB: {
        int32_t result = emu->regs[inst.operands[0]] - emu->regs[inst.operands[1]];
        updateFlagsRegister(emu, result);
        emu->regs[inst.operands[0]] = result;
        DISPATCH();
    }
    LOAD: {
        uint32_t addr = getAddress(
            emu->regs[inst.operands[1]],
            emu->regs[inst.operands[2]]
        );

        emu->regs[inst.operands[0]] = memoryLoadWord(emu->mem, addr);
        DISPATCH();
    }
    STORE: {
        uint32_t addr = getAddress(
            emu->regs[inst.operands[0]],
            emu->regs[inst.operands[1]]
        );
        memorySaveWord(emu->mem, addr, inst.operands[2]);
        DISPATCH();
    }
    AND: {
        int32_t result = emu->regs[inst.operands[0]] & emu->regs[inst.operands[1]];
        updateFlagsRegister(emu, result);
        emu->regs[inst.operands[0]] = result;
        DISPATCH();
    }
    OR: {
        int32_t result = emu->regs[inst.operands[0]] | emu->regs[inst.operands[1]];
        updateFlagsRegister(emu, result);
        emu->regs[inst.operands[0]] = result;
        DISPATCH();
    }
    XOR: {
        int32_t result = emu->regs[inst.operands[0]] ^ emu->regs[inst.operands[1]];
        updateFlagsRegister(emu, result);
        emu->regs[inst.operands[0]] = result;
        DISPATCH();
    }
    SHF: {
        int32_t result;
        if (inst.operands[2] == 0)
            result = emu->regs[inst.operands[0]] << emu->regs[inst.operands[1]];
        else
            result = emu->regs[inst.operands[0]] >> emu->regs[inst.operands[1]];

        updateFlagsRegister(emu, result);
        emu->regs[inst.operands[0]] = result;
        DISPATCH();
    }
    JMP: {
        emu->pc = getAddress(inst.operands[0], inst.operands[1]);
        DISPATCH();
    }
    JIZ: {
        if (emu->regs[REG_F] & FLAG_ZERO)
            emu->pc = getAddress(inst.operands[0], inst.operands[1]);
        DISPATCH();
    }
    CALL: {
        // save the pc at the current sp and decrement the sp (stack grows downward)
        ((uint32_t*)emu->mem->data)[emu->sp--] = emu->pc; 

        // jump to given addr
        emu->pc = getAddress(inst.operands[0], inst.operands[1]);

        DISPATCH();
    }
    RET: {
        // return to value at sp and increment it
        emu->pc = ((uint32_t*)emu->mem->data)[++emu->sp];
        DISPATCH();
    }
    PORT: {
        uint16_t port = emu->regs[inst.operands[0]];
        if (port >= MAX_DEVICES) {
            DISPATCH();
        }

        uint8_t direction = inst.operands[2];

        Device* device = emu->devices[port];
        if (!device) {
            DISPATCH();
        }

        if (direction == 0) {
            uint16_t value = emu->regs[inst.operands[1]];

            if (device->inPos == DEVICE_BUFFER_LENGTH) {
                DISPATCH();
            }

            device->in[device->inPos] = value;
            device->inPos++;
        } else {
            if (device->outPos == 0) {
                emu->regs[inst.operands[1]] = 0;
                DISPATCH();
            }

            emu->regs[inst.operands[1]] = device->out[device->outPos-1];
            if (device->outPos > 0)
                device->outPos--;
        }

        DISPATCH();
    }
    HLT: {
        emu->halted = true;
        return;
    }
}

Emulator* initEmulator(uint32_t memorySize) {
    Emulator* newEmu = malloc(sizeof(Emulator));
    if (!newEmu)
        return NULL;

    // zero out registers
    memset(newEmu->regs, 0, sizeof(newEmu->regs));

    // zero out special regs
    newEmu->pc = MEM_PROGRAM_START;
    newEmu->sp = 0;
    newEmu->ihp = 0;

    // init memory
    newEmu->mem = initMemory(memorySize);
    if (!newEmu->mem) {
        free(newEmu);
        return NULL;
    }

    return newEmu;
}

void freeEmulator(Emulator* emu) {
    freeMemory(emu->mem);
    free(emu);
}

void installDevice(Emulator* emu, Device* device, uint8_t deviceNumber) {
    assert(deviceNumber < MAX_DEVICES);
    emu->devices[deviceNumber] = device;
}