#include "memory.h"

Memory* initMemory(uint32_t size) {
    Memory* newMemory = malloc(sizeof(Memory));
    if (!newMemory)
        return NULL;

    newMemory->size = size;
    newMemory->data = calloc(size, sizeof(uint8_t));
    if (!newMemory->data) {
        free(newMemory);
        return NULL;
    }

    return newMemory;
}

inline uint16_t memoryLoadWord(Memory* m, uint32_t address) {
    return m->data[address];
}

inline void memorySaveWord(Memory* m, uint32_t address, uint16_t value) {
    ((uint16_t*)m->data)[address] = value;
}

void putInst(Memory* m, SerializedInst inst, uint32_t offset) {
    memcpy((SerializedInst*)(&m->data[MEM_PROGRAM_START]) + offset, &inst, sizeof(SerializedInst));
}

uint32_t memorySerializeInst(Memory* m, Instruction inst, uint32_t offset) {
    SerializedInst packed = 0;

    packed |= (SerializedInst)(inst.opcode      & 0b0000000000001111)     ;
    packed |= (SerializedInst)(inst.operands[0] & 0b0000000000001111) << 4;
    packed |= (SerializedInst)(inst.operands[1] & 0b0000000000001111) << 8;
    packed |= (SerializedInst)(inst.operands[2] & 0b0000000000001111) << 12;

    memcpy(m->data + MEM_PROGRAM_START + (offset * sizeof(SerializedInst)), &packed, sizeof(SerializedInst));


    offset++;

    // write the imm16 if the instruction has one
    if (inst.hasImm16) {
        memcpy(m->data + MEM_PROGRAM_START + (offset * sizeof(SerializedInst)), &inst.immediate, sizeof(SerializedInst));

        offset++;
    }

    return offset;
}

void memoryLoadProgram(Memory* m, Instruction program[], size_t numInstructions) {
    size_t offset = 0;
    for (size_t i = 0; i < numInstructions; i++) {
        offset = memorySerializeInst(m, program[i], offset);
    }
}

Instruction decodeInstruction(Memory* m, uint32_t* pc) {
    SerializedInst serialized;
    memcpy(&serialized, &m->data[*pc], sizeof(SerializedInst));

    Instruction outputInst = {};

    uint8_t opcode = serialized & 0b0000000000001111; // get four lowest bits for the opcode
    outputInst.opcode = opcode;

    const OperandType* opTypes = INSTRUCTION_OPERAND_TYPES[opcode];

    uint8_t offset = 0;
    for (uint8_t i = 0; i < 3; i++) {
        switch (opTypes[i]) {
            case OP_REG: {
                // get the data at the current offset, and grab the 4 bits that are there with a bitwise
                // "and" operator
                outputInst.operands[i] = (serialized >> (4 * (offset + 1))) & 0b0000000000001111;
                offset++;
                break;
            }

            case OP_IMM16: { // the immediate 16 takes up the space where the next instruction would be
                (*pc) += sizeof(SerializedInst);

                uint16_t nextDataOver = m->data[*pc];
                outputInst.immediate = nextDataOver;
                outputInst.hasImm16 = true;
                break;
            }

            case OP_NONE: {
                break;
            }
        }

        if (opTypes[i] == OP_IMM16) break;
    }

    (*pc) += sizeof(SerializedInst);

    return outputInst;
}

void freeMemory(Memory* m) {
    free(m->data);
    free(m);
}

inline uint32_t getAddress(uint16_t low, uint16_t high) {
    return (high << 16) | low;
}