diff --git a/cpu/core.c b/cpu/core.c
index 027c8d8..5298890 100644
--- a/cpu/core.c
+++ b/cpu/core.c
@@ -47,55 +47,64 @@ void step(CPU *cpu) {
 
         case INC_RN:
             //Increment register
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
+            temp = read_reg(cpu, reg1);
+            write_reg(cpu, reg1, temp + 1);
             cpu->regs[reg1]++;
 
         case INC_ADDR:
             //Increment address
-            addrTemp = read_addr(cpu);
-            write_mem(cpu, read_addr(cpu), read_mem(cpu, addrTemp) + 1);
+            addrTemp = read_address(cpu);
+            temp = read_mem(cpu, addrTemp);
+            write_mem(cpu, addrTemp, temp + 1);
 
         case DEC_RN:
             //Decrement register
-            reg1 = read_reg(cpu);
-            cpu->regs[reg1]--;
+            reg1 = read_reg_number(cpu);
+            temp = read_reg(cpu, reg1);
+            write_reg(cpu, reg1, temp - 1);
+            cpu->regs[reg1]++;
 
         case DEC_ADDR:
             //Decrement address
-            addrTemp = read_addr(cpu);
-            write_mem(cpu, addrTemp, read_mem(cpu, addrTemp) - 1);
+            addrTemp = read_address(cpu);
+            temp = read_mem(cpu, addrTemp);
+            write_mem(cpu, addrTemp, temp - 1);
 
         case MOV_RN_IMM:
             //Load from immediate to register
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             imm = read_mem(cpu, cpu->pc++);
             cpu->regs[reg1] = imm;
             break;
 
         case MOV_RN_RM:
-            reg1 = read_reg(cpu);
-            reg2 = read_reg(cpu);
-            cpu->regs[reg1] = cpu->regs[reg2];
+            reg1 = read_reg_number(cpu);
+            reg2 = read_reg_number(cpu);
+            temp = read_reg(cpu, reg1);
+            write_reg(cpu, reg2, temp);
             break;
 
         case MOV_RN_ADDR:
-            // Load from memory into register.
-            reg1 = read_reg(cpu);
-            imm = read_mem(cpu, cpu->pc++);
-            cpu->regs[reg1] = read_mem(cpu, imm);
+            // Store register to memory
+            reg1 = read_reg_number(cpu);
+            addrTemp = read_address(cpu);
+            temp = read_reg(cpu, reg1);
+            write_mem(cpu, addrTemp, temp);
             break;
 
         case MOV_ADDR_RN:
-            // Store from register into memory.
-            imm = read_mem(cpu, cpu->pc++);
-            reg1 = read_reg(cpu);
-            write_mem(cpu, imm, cpu->regs[reg1]);
+            // Load memory to register
+            addrTemp = read_address(cpu);
+            reg1 = read_reg_number(cpu);
+            temp = read
+            write_mem(cpu, addrTemp, cpu->regs[reg1]);
             break;
 
         case SWAP: {
             // Swap contents of two registers.
-            reg1 = read_reg(cpu);
-            reg2 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
+            reg2 = read_reg_number(cpu);
             temp = cpu->regs[reg1];
             cpu->regs[reg1] = cpu->regs[reg2];
             cpu->regs[reg2] = temp;
@@ -104,57 +113,57 @@ void step(CPU *cpu) {
 
         case SWAPN: {
             // Swap the nibbles of a register.
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             uint8_t val = (uint8_t) cpu->regs[reg1];
             cpu->regs[reg1] = ((val & 0x0F) << 4) | ((val & 0xF0) >> 4);
             break;
         }
 
         case ADD_RN_RM:
-            reg1 = read_reg(cpu);
-            reg2 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
+            reg2 = read_reg_number(cpu);
             cpu->regs[reg1] += cpu->regs[reg2];
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case ADD_RN_IMM:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             imm = read_mem(cpu, cpu->pc++);
             cpu->regs[reg1] += imm;
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case SUB_RN_RM:
-            reg1 = read_reg(cpu);
-            reg2 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
+            reg2 = read_reg_number(cpu);
             cpu->regs[reg1] -= cpu->regs[reg2];
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case SUB_RN_IMM:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             imm = read_mem(cpu, cpu->pc++);
             cpu->regs[reg1] -= imm;
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case MUL_RN_RM:
-            reg1 = read_reg(cpu);
-            reg2 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
+            reg2 = read_reg_number(cpu);
             cpu->regs[reg1] *= cpu->regs[reg2];
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case MUL_RN_IMM:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             imm = read_mem(cpu, cpu->pc++);
             cpu->regs[reg1] *= imm;
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case DIV_RN_RM:
-            reg1 = read_reg(cpu);
-            reg2 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
+            reg2 = read_reg_number(cpu);
             if (cpu->regs[reg2] == 0) {
                 printf("Error: Division by zero!\n");
                 cpu->mode = Error;
@@ -165,7 +174,7 @@ void step(CPU *cpu) {
             break;
 
         case DIV_RN_IMM:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             imm = read_mem(cpu, cpu->pc++);
             if (imm == 0) {
                 printf("Error: Division by zero!\n");
@@ -177,8 +186,8 @@ void step(CPU *cpu) {
             break;
 
         case MOD_RN_RM:
-            reg1 = read_reg(cpu);
-            reg2 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
+            reg2 = read_reg_number(cpu);
             if (cpu->regs[reg2] == 0) {
                 printf("Error: Modulo by zero!\n");
                 cpu->mode = Error;
@@ -189,7 +198,7 @@ void step(CPU *cpu) {
             break;
 
         case MOD_RN_IMM:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             imm = read_mem(cpu, cpu->pc++);
             if (imm == 0) {
                 printf("Error: Modulo by zero!\n");
@@ -201,75 +210,75 @@ void step(CPU *cpu) {
             break;
 
         case NEG_RN:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             cpu->regs[reg1] = -((int32_t) cpu->regs[reg1]);
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case AND_RN_RM:
-            reg1 = read_reg(cpu);
-            reg2 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
+            reg2 = read_reg_number(cpu);
             cpu->regs[reg1] &= cpu->regs[reg2];
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case AND_RN_IMM:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             imm = read_mem(cpu, cpu->pc++);
             cpu->regs[reg1] &= imm;
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case OR_RN_RM:
-            reg1 = read_reg(cpu);
-            reg2 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
+            reg2 = read_reg_number(cpu);
             cpu->regs[reg1] |= cpu->regs[reg2];
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case OR_RN_IMM:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             imm = read_mem(cpu, cpu->pc++);
             cpu->regs[reg1] |= imm;
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case XOR_RN_RM:
-            reg1 = read_reg(cpu);
-            reg2 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
+            reg2 = read_reg_number(cpu);
             cpu->regs[reg1] ^= cpu->regs[reg2];
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case XOR_RN_IMM:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             imm = read_mem(cpu, cpu->pc++);
             cpu->regs[reg1] ^= imm;
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case NOT_RN:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             cpu->regs[reg1] = ~cpu->regs[reg1];
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case SHL_RN_IMM:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             imm = read_mem(cpu, cpu->pc++);
             cpu->regs[reg1] <<= imm;
             set_flags(cpu, cpu->regs[reg1]);
             break;
 
         case SHR_RN_IMM:
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             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_reg(cpu);
+            reg1 = read_reg_number(cpu);
             imm = read_mem(cpu, cpu->pc++);
             // Arithmetic right shift; cast to signed before shifting.
             cpu->regs[reg1] = ((int32_t) cpu->regs[reg1]) >> imm;
@@ -289,8 +298,8 @@ void step(CPU *cpu) {
 
         case CMP: {
             // Compare two registers: set flags (Zero, Negative)
-            reg1 = read_reg(cpu);
-            reg2 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
+            reg2 = read_reg_number(cpu);
             cmpResult = (int32_t) cpu->regs[reg1] - (int32_t) cpu->regs[reg2];
             set_flags(cpu, cmpResult);
             break;
@@ -298,7 +307,7 @@ void step(CPU *cpu) {
 
         case JE_BIT_RN: {
             // Jump if bit in register set
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             if (reg1 >= REG_COUNT) {
                 reg1 = REG_COUNT - 1;
             }
@@ -342,7 +351,7 @@ void step(CPU *cpu) {
 
         case JNE_BIT_RN: {
             // Jump if bit in register set
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             if (reg1 >= REG_COUNT) {
                 reg1 = REG_COUNT - 1;
             }
@@ -437,14 +446,14 @@ void step(CPU *cpu) {
 
         case PUSH: {
             // Push register value onto the stack.
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             write_mem(cpu, cpu->sp--, (uint8_t) cpu->regs[reg1]);
             break;
         }
 
         case POP: {
             // Pop a value from the stack into a register.
-            reg1 = read_reg(cpu);
+            reg1 = read_reg_number(cpu);
             cpu->regs[reg1] = read_mem(cpu, ++cpu->sp);
             break;
         }
diff --git a/cpu/core.h b/cpu/core.h
index 8392151..3d9d9bc 100644
--- a/cpu/core.h
+++ b/cpu/core.h
@@ -17,7 +17,7 @@ enum Mode {
     MaxSpeed
 };
 
-#define MEM_SIZE 8192
+#define MEM_SIZE 65535
 // Register count (register names R0 to R7)
 #define REG_COUNT 32
 
diff --git a/cpu/memory.c b/cpu/memory.c
index 9c0c8b5..95ad8f7 100644
--- a/cpu/memory.c
+++ b/cpu/memory.c
@@ -37,7 +37,7 @@ uint32_t read_mem32(CPU *cpu, uint32_t addr) {
     return read_mem16(cpu, addr) | (read_mem16(cpu, addr + 2) << 16);
 }
 
-uint32_t read_addr(CPU *cpu) {
+uint32_t read_address(CPU *cpu) {
     uint32_t out = read_mem16(cpu, cpu->pc) | (read_mem16(cpu, cpu->pc + 2) << 16);
     cpu->pc += 4;
     if (out >= MEM_SIZE) {
@@ -46,14 +46,29 @@ uint32_t read_addr(CPU *cpu) {
     return out;
 }
 
-uint8_t read_reg(CPU *cpu) {
+uint8_t read_reg_number(CPU *cpu) {
     uint8_t out = read_mem(cpu, cpu->pc++);
     if (out >= REG_COUNT) {
-        out = REG_COUNT - 1;
+        return 0;
     }
     return out;
 }
 
+uint8_t read_reg(CPU *cpu, uint8_t number) {
+    if (number >= REG_COUNT) {
+        return 0;
+    }
+    return cpu->regs[number];
+}
+
+uint8_t write_reg(CPU *cpu, uint8_t number, uint8_t value) {
+    if (number >= REG_COUNT) {
+        return 1;
+    }
+    cpu->regs[number] = value;
+    return 0;
+}
+
 uint8_t write_mem16(CPU *cpu, uint32_t addr, uint16_t value) {
     uint8_t status = write_mem(cpu, addr, value & 0xFF);
     if (status) {
diff --git a/cpu/memory.h b/cpu/memory.h
index 229e632..f01838e 100644
--- a/cpu/memory.h
+++ b/cpu/memory.h
@@ -14,9 +14,13 @@ uint8_t write_mem16(CPU *cpu, uint32_t addr, uint16_t value);
 
 uint32_t read_mem32(CPU *cpu, uint32_t addr);
 
-uint32_t read_addr(CPU *cpu);
+uint32_t read_address(CPU *cpu);
 
-uint8_t read_reg(CPU *cpu);
+uint8_t read_reg_number(CPU *cpu);
+
+uint8_t read_reg(CPU *cpu, uint8_t number);
+
+uint8_t write_reg(CPU *cpu, uint8_t number, uint8_t value);
 
 uint16_t read_mem16(CPU *cpu, uint32_t addr);