// cpu.c
// SplitBit CPU Emulator Core
// Written by Anachronaut
// 10/16/2024

#include "cpu.h"
#include "io.h"
#include "controller.h"
#include "../Assembler/assembly.h" // For the vector table layout, which both tools share.

// Reads one entry out of a vector table. Most significant byte first, matching the
// branch instructions and both file formats.
// ---- Every touch of memory, and what it costs ----
//
// One access, one cycle. These exist so that the cost is counted in the one place the
// access happens, rather than in a table of per instruction costs kept somewhere else -
// a table like that is a second copy of what the code does, and the two drift.
static inline uint8_t fetchProgram(CPURegisters *cpu, uint16_t at) {
    cpu->busCycles++;
    return cpu->Program[at];
}

static inline uint8_t readData(CPURegisters *cpu, uint16_t at) {
    cpu->busCycles++;
    return cpu->Data[at];
}

static inline void writeData(CPURegisters *cpu, uint16_t at, uint8_t value) {
    cpu->busCycles++;
    cpu->Data[at] = value;
}

// A device port is reached over the same bus as memory, so it costs the same. That is a
// claim about the hardware rather than an observation - a machine could put devices
// somewhere faster or slower - and it is the simple thing until there is a reason to say
// otherwise.
static inline void portOut(CPURegisters *cpu, uint8_t value, uint8_t port) {
    cpu->busCycles++;
    cpu->bus->out(value, port);
    // And whatever memory that made the controller move. The machine waits for it, which
    // is the conservative reading: a blit stalls the program that asked for one.
    cpu->busCycles += cpu->bus->takeStall();
}

static inline uint8_t portIn(CPURegisters *cpu, uint8_t port) {
    cpu->busCycles++;
    uint8_t value = cpu->bus->in(port);
    cpu->busCycles += cpu->bus->takeStall();
    // And whatever time went by while the device kept the machine waiting. Idle rather than
    // bus, because a machine stopped on a port is not using memory - the same distinction
    // WAIT makes, arrived at from the other direction.
    cpu->idleCycles += cpu->bus->takeIdle();
    return value;
}

static uint16_t readVector(const uint8_t *programMemory, uint16_t base, uint8_t index) {
    uint16_t address = base + (uint16_t)index * VECTOR_ENTRY_BYTES;
    return ((uint16_t)programMemory[address] << 8) | (uint16_t)programMemory[address + 1];
}

// Builds an interrupt frame and dispatches through a vector. The resume address is the
// address execution should carry on from once the handler returns, and it goes into the
// frame as a real address so that a handler can read it and make sense of it.
//
// Returns 0 if it dispatched. If the vector is empty there is nothing to dispatch to, so
// it raises a fault and returns 1 rather than jumping to the bottom of Program Memory
// and running whatever happens to be there.
//
// Note that a zero entry means "no handler" to everything that dispatches, including the
// two entries the CPU treats as start addresses when it reads them at reset. The
// exemption belongs to that one read, not to the entries themselves.
static uint8_t dispatch(CPURegisters *cpu, uint16_t base, uint8_t index,
                       uint16_t resumeAddress, int mayEscalate) {
    uint16_t handler = readVector(cpu->Program, base, index);
    if (handler == 0x0000) {
        // ---- Nowhere to go is itself something to report ----
        //
        // This is the one fault the machine used to have no way of handing over, because
        // the thing that would hand it over is the thing that has just found nothing to
        // hand it to. So it goes to a vector of its own instead, with the number of the
        // empty entry in Q - and a missing software vector and a device nobody is
        // listening to are separate entries, because they are separate mistakes.
        //
        // NOT WHEN ALREADY ESCALATING. If the fault vector for this is itself empty then
        // the machine really has run out of places to go, and stopping is the only honest
        // answer left.
        if (mayEscalate) {
            const uint8_t escalation = (base == HARDWARE_VECTOR_BASE)
                                     ? VECTOR_NO_DEVICE : VECTOR_NO_HANDLER;
            if (!dispatch(cpu, SOFTWARE_VECTOR_BASE, escalation, resumeAddress, 0)) {
                // After the frame, so the Q the interrupted program had is safely in it and
                // RETI will put it back. What the handler sees is which entry was empty.
                cpu->Q = index;
                return 0;
            }
        }
        cpu->Fault = FAULT_NO_HANDLER;
        cpu->FaultVector = index;
        cpu->Status |= STATUS_FAULT;
        cpu->Status |= STATUS_HALT;
        return 1;
    }
    // Order mirrors genericCall exactly: low byte then high byte, lowest numbered Data
    // Pointer first, so that anything walking the Stack sees a familiar shape.
    writeData(cpu, cpu->StackPointer, resumeAddress & 0xFF);
    cpu->StackPointer--;
    writeData(cpu, cpu->StackPointer, (resumeAddress >> 8) & 0xFF);
    cpu->StackPointer--;
    for (int i = 0; i < DATA_POINTERS; i++) {
        writeData(cpu, cpu->StackPointer, cpu->DataPointer[i] & 0xFF);
        cpu->StackPointer--;
        writeData(cpu, cpu->StackPointer, (cpu->DataPointer[i] >> 8) & 0xFF);
        cpu->StackPointer--;
    }
    writeData(cpu, cpu->StackPointer, cpu->B);
    cpu->StackPointer--;
    writeData(cpu, cpu->StackPointer, cpu->A);
    cpu->StackPointer--;
    writeData(cpu, cpu->StackPointer, cpu->Q);
    cpu->StackPointer--;
    writeData(cpu, cpu->StackPointer, cpu->Status);
    cpu->StackPointer--;
    // A handler runs with hardware interrupts held off unless it says otherwise, so an
    // interrupt cannot arrive inside the handler for another one and grow the Stack
    // without bound. The old setting rode into the frame inside the Status register, so
    // RETI puts it back without anything having to remember it separately.
    cpu->Status &= ~STATUS_INTERRUPT;
    // The Program Counter is stepped after every instruction, so land one short of the
    // handler and let that step land on its first byte. genericBranch does the same.
    cpu->ProgramCounter = handler - 1;
    return 0;
}

// Dispatching the ordinary way: through the vector asked for, and through the fault vector
// for a missing one if that is what it turns out to be.
static uint8_t enterInterrupt(CPURegisters *cpu, uint16_t base, uint8_t index,
                              uint16_t resumeAddress) {
    return dispatch(cpu, base, index, resumeAddress, 1);
}

// A device that refused what it was asked stops the machine where it stands, rather than
// raising a line and letting execution carry on past the mistake. The frame carries the
// address of the instruction that asked, so a handler can see which one it was, and so a
// bare RETI meets it again the way every other fault on this machine does.
//
// Returns 1 if the machine has stopped because nothing was installed to catch it.
static uint8_t answerRefusal(CPURegisters *cpu, uint16_t site) {
    uint8_t refusal = takeRefusal();
    if (refusal == 0) {
        return 0;
    }
    if (enterInterrupt(cpu, SOFTWARE_VECTOR_BASE, refusal, site)) {
        cpu->Fault = FAULT_DEVICE_REFUSED;
        cpu->FaultVector = refusingPort();
        cpu->ProgramCounter = site - 1;
    }
    return 0;
}

void initializeCPU(CPURegisters *cpu, uint8_t *programMemory, uint8_t *dataMemory) {
    // The machine's own, which is what every processor here was on when there could only be
    // one. Anything that wants a processor somewhere else changes this afterwards.
    cpu->bus = machineBus();
    cpu->A = 0;
    cpu->B = 0;
    cpu->Q = 0;
    cpu->Status = 0;
    // Execution begins wherever the boot vector points. It is a start address rather
    // than a handler, so a zero there is not "nothing installed" but the address
    // 0x0000, which is where a program carrying no vector table of its own begins.
    // That is what lets everything written before the table existed still run.
    cpu->ProgramCounter = readVector(programMemory, SOFTWARE_VECTOR_BASE, VECTOR_BOOT);
    // Every Data Pointer starts at the bottom of Data Memory.
    for (int i = 0; i < DATA_POINTERS; i++) {
        cpu->DataPointer[i] = 0x0000;
    }
    cpu->StackPointer = 0xFFFF;
    cpu->Program = programMemory;
    cpu->Data = dataMemory;
    cpu->busCycles = 0;
    cpu->idleCycles = 0;
    cpu->Waiting = 0;
    cpu->Fault = FAULT_NONE;
    cpu->FaultVector = 0;
}

void genericBranch(CPURegisters *cpu){
    // Load the next two bytes from program memory into the Program Counter.
    // Byte order is imporant. Most Significant first, then Least Significant.
    cpu->ProgramCounter++; // Move to the next byte. (MSB)
    uint16_t DestinationAddress;
    DestinationAddress = (uint16_t)fetchProgram(cpu, cpu->ProgramCounter) << 8; // Cast the 8 bit value to a 16 bit value and shifts it up to the high byte.
    cpu->ProgramCounter++; // Move to the next byte. (LSB)
    DestinationAddress = DestinationAddress | (uint16_t)fetchProgram(cpu, cpu->ProgramCounter); // Cast the 8 bit value to a 16 bit value and or it to add it to the desination.
    cpu->ProgramCounter = DestinationAddress-1;
}

void genericCall(CPURegisters *cpu){
    // Order, low byte, high byte
    writeData(cpu, cpu->StackPointer, cpu->ProgramCounter & 0xFF);
    cpu->StackPointer--;
    writeData(cpu, cpu->StackPointer, (cpu->ProgramCounter >> 8) & 0xFF);
    cpu->StackPointer--;
    // Push the preserved Data Pointers to the Stack, lowest numbered first.
    // Order within each one, low byte, high byte.
    // The pointers above PRESERVED_DATA_POINTERS are deliberately left alone, so a
    // subroutine can use one to hand an address back to whoever called it.
    for (int i = 0; i < PRESERVED_DATA_POINTERS; i++) {
        writeData(cpu, cpu->StackPointer, cpu->DataPointer[i] & 0xFF);
        cpu->StackPointer--;
        writeData(cpu, cpu->StackPointer, (cpu->DataPointer[i] >> 8) & 0xFF);
        cpu->StackPointer--;
    }
    // Push B to the Stack.
    writeData(cpu, cpu->StackPointer, cpu->B);
    cpu->StackPointer--;
    // Push A to the Stack.
    writeData(cpu, cpu->StackPointer, cpu->A);
    cpu->StackPointer--;
    // Perform a Generic Branch to the Address.
    genericBranch(cpu);
}

uint16_t *selectDataPointer(CPURegisters *cpu) {
    // Every instruction that works through a Data Pointer names which one in the
    // byte immediately after the opcode. Out of range selectors are masked down
    // rather than rejected, the way a narrow field in hardware would be. It is the
    // assembler's job to refuse to emit one in the first place.
    cpu->ProgramCounter++;
    return &cpu->DataPointer[fetchProgram(cpu, cpu->ProgramCounter) & (DATA_POINTERS - 1)];
}

uint8_t executeOperation(uint8_t Instruction, CPURegisters *cpu) {
    // ADD and SUB share this. It is declared here rather than after a case label
    // because a label may only be followed by a statement in ISO C, and a
    // declaration is not one.
    uint16_t result;
    switch(Instruction) {
        // 0x - Arithmetic and Logic Operations.
        case 0x10:
            // ADD - A + B + Carry -> Q
            result = (uint16_t)cpu->A + (uint16_t)cpu->B + (cpu->Status & STATUS_CARRY);
            if (result > 255) {
                cpu->Status |= STATUS_CARRY;
            } else {
                cpu->Status &= ~STATUS_CARRY;
            }
            cpu->Q = result & 0xFF;
        break;
        case 0x11:
            // SUB - A - B - Carry -> Q
            result = (uint16_t)cpu->A - (uint16_t)cpu->B - (cpu->Status & STATUS_CARRY);
            if (result > 255) {
                cpu->Status |= STATUS_CARRY;
            } else {
                cpu->Status &= ~STATUS_CARRY;
            }
            cpu->Q = result & 0xFF;
        break;
        case 0x12:
            // AND - A and B -> Q
            cpu->Q = cpu->A&cpu->B;
        break;
        case 0x13:
            // OR - A or B -> Q
            cpu->Q = cpu->A|cpu->B;
        break;
        case 0x14:
            // XOR - A xor B -> Q
            cpu->Q = cpu->A^cpu->B;
        break;
        case 0x15:
            // NOTA - not A -> Q
            cpu->Q = ~cpu->A;
        break;
        case 0x16:
            // NOTB - not B -> Q
            cpu->Q = ~cpu->B;
        break;
        case 0x17:
            // SHL - Shift AB left.
            //
            // A local, and it always was one in effect: written and read inside this one
            // instruction and never carried to the next. It sat at file scope until there
            // was a second processor to share it with, which is a poor time to find out.
        {
            uint16_t shiftRegister = ((uint16_t)cpu->A << 8) | cpu->B;
            shiftRegister = (shiftRegister << 1) | (shiftRegister >> 15);
            cpu->A = shiftRegister >> 8;
            cpu->B = shiftRegister & 0xFF;
        }
        break;
        case 0x18:
            // SHR - Shift AB right.
            //
            // A local, and it always was one in effect: written and read inside this one
            // instruction and never carried to the next. It sat at file scope until there
            // was a second processor to share it with, which is a poor time to find out.
        {
            uint16_t shiftRegister = ((uint16_t)cpu->A << 8) | cpu->B;
            shiftRegister = (shiftRegister >> 1) | (shiftRegister << 15);
            cpu->A = shiftRegister >> 8;
            cpu->B = shiftRegister & 0xFF;
        }
        break;
        //
        // 1x - Branch Operations:
        //
        case 0x60:
            // BRI - Branch Immediately
            genericBranch(cpu);
        break;
        case 0x61:
            // BRQ - Branch if Q = 0
            if(cpu->Q == 0) {
                genericBranch(cpu);
            } else {

                cpu->ProgramCounter+=2;
            }
        break;
        case 0x62:
            // BRA - Branch if A = 0
            if(cpu->A == 0) {
                genericBranch(cpu);

            } else {
                cpu->ProgramCounter+=2;
            }
        break;
        case 0x63:
            // BRB -  if B = 0
            if(cpu->B == 0) {

                genericBranch(cpu);
            } else {
                cpu->ProgramCounter+=2;
            }
        break;
        case 0x64:
            // BRC - Do an immediate branch if the Carry Flag is set.
            if (cpu->Status & STATUS_CARRY) {
                genericBranch(cpu);
            } else {
                cpu->ProgramCounter+=2;
            }
        break;
        case 0x65: {
            // BRD - Branch to the address held in a Data Pointer.
            // This is the only branch whose destination is not written into the
            // program, which is what makes a table of addresses something a program
            // can dispatch through rather than only read.
            uint16_t destination = *selectDataPointer(cpu);
            // stepCPU adds one after every instruction, so aim one short.
            cpu->ProgramCounter = destination - 1;
        }
        break;
        case 0x70:
            // RCAL - Call, pushing nothing but the return address.
            //
            // The unsafe one, and it says so in its name. CALL puts A, B and the first
            // three Data Pointers back the way it found them, which costs ten bytes of
            // Stack and means a subroutine can only hand anything back through Q, DP3 or
            // memory. RCAL costs two bytes and puts nothing back at all: everything the
            // callee touches, the caller has lost.
            //
            // It must be returned from with RRET. The two frames are different sizes, so
            // returning from one through the other walks the Stack to somewhere that was
            // never a return address.
            writeData(cpu, cpu->StackPointer, cpu->ProgramCounter & 0xFF);
            cpu->StackPointer--;
            writeData(cpu, cpu->StackPointer, (cpu->ProgramCounter >> 8) & 0xFF);
            cpu->StackPointer--;
            genericBranch(cpu);
        break;
        case 0x71:
            // CALL - Push the Program Counter to the Stack, and perform an immediate branch.
            genericCall(cpu);
        break;
        case 0x66:
            // BNQ - Branch if Q is not 0.
            if(cpu->Q != 0) {
                genericBranch(cpu);
            } else {
                cpu->ProgramCounter+=2;
            }
        break;
        case 0x67:
            // BNA - Branch if A is not 0.
            if(cpu->A != 0) {
                genericBranch(cpu);
            } else {
                cpu->ProgramCounter+=2;
            }
        break;
        case 0x68:
            // BNB - Branch if B is not 0.
            if(cpu->B != 0) {
                genericBranch(cpu);
            } else {
                cpu->ProgramCounter+=2;
            }
        break;
        case 0x69:
            // BNC - Branch if the Carry Flag is clear.
            if (!(cpu->Status & STATUS_CARRY)) {
                genericBranch(cpu);
            } else {
                cpu->ProgramCounter+=2;
            }
        break;
        case 0x72: {
            // SWI - Software Interrupt. The byte after the opcode names the vector.
            // Never masked: this is an instruction the program deliberately ran, not
            // something a device asked for.
            uint16_t site = cpu->ProgramCounter;
            cpu->ProgramCounter++;
            uint8_t vector = fetchProgram(cpu, cpu->ProgramCounter);
            // Execution resumes after the operand, which the Program Counter is sitting
            // on, so the resume address is one further on than that.
            if (enterInterrupt(cpu, SOFTWARE_VECTOR_BASE, vector, cpu->ProgramCounter + 1)) {
                // No handler. Leave the Program Counter on the SWI itself rather than
                // its operand, so the report names the instruction that failed.
                cpu->ProgramCounter = site - 1;
            }
        } break;
        case 0x73: {
            // RETI - Return from an interrupt. Pops the frame in the exact reverse of
            // the order enterInterrupt pushed it.
            cpu->StackPointer++;
            cpu->Status = readData(cpu, cpu->StackPointer);
            cpu->StackPointer++;
            cpu->Q = readData(cpu, cpu->StackPointer);
            cpu->StackPointer++;
            cpu->A = readData(cpu, cpu->StackPointer);
            cpu->StackPointer++;
            cpu->B = readData(cpu, cpu->StackPointer);
            for (int i = DATA_POINTERS - 1; i >= 0; i--) {
                cpu->StackPointer++;
                cpu->DataPointer[i] = (uint16_t)readData(cpu, cpu->StackPointer) << 8;
                cpu->StackPointer++;
                cpu->DataPointer[i] |= (uint16_t)readData(cpu, cpu->StackPointer);
            }
            uint16_t resumeAddress;
            cpu->StackPointer++;
            resumeAddress = (uint16_t)readData(cpu, cpu->StackPointer) << 8;
            cpu->StackPointer++;
            resumeAddress = resumeAddress | (uint16_t)readData(cpu, cpu->StackPointer);
            // The frame holds the address to carry on from. The Program Counter is
            // stepped after every instruction, so land one short of it. RET does the
            // same job with its +2, for the same reason.
            cpu->ProgramCounter = resumeAddress - 1;
        } break;
        case 0x76: {
            // SRET - Return from a handler that has an answer.
            //
            // THE SAME FRAME AS RETI, WITH THE CALL CONVENTION'S RULE APPLIED TO IT. CALL
            // saves A, B and Data Pointers 0 to 2 and nothing else, which is exactly why
            // Q and DP3 are how a subroutine hands something back. An interrupt saves all
            // of it, so a handler with an answer had to reach into its own frame and
            // un-save two fields by hand - thirty places in CosmOS did that, each of them
            // knowing the frame's layout by an offset, and all thirty would have gone
            // quietly wrong the day the frame gained a field.
            //
            // So: RETI restores everything and is how a hardware handler says it was never
            // here. SRET restores what a RET restores and is how a service says it has
            // replied. The saved Q and DP3 are stepped over and dropped.
            cpu->StackPointer++;
            uint8_t savedStatus = readData(cpu, cpu->StackPointer);
            // The Interrupt Flag, and only that. Entering a handler clears it and the
            // frame is what puts it back, so dropping the whole byte would leave a service
            // silently turning interrupts off. Everything else in Status - the carry
            // above all - is left as the handler leaves it, because that is what a RET
            // does and the point of this instruction is that there is one rule.
            cpu->Status = (uint8_t)((cpu->Status & ~STATUS_INTERRUPT)
                                  | (savedStatus & STATUS_INTERRUPT));
            cpu->StackPointer++;    // The saved Q, dropped: the handler's answer stands.
            cpu->StackPointer++;
            cpu->A = readData(cpu, cpu->StackPointer);
            cpu->StackPointer++;
            cpu->B = readData(cpu, cpu->StackPointer);
            for (int i = DATA_POINTERS - 1; i >= 0; i--) {
                cpu->StackPointer++;
                uint16_t high = (uint16_t)readData(cpu, cpu->StackPointer) << 8;
                cpu->StackPointer++;
                uint16_t low = (uint16_t)readData(cpu, cpu->StackPointer);
                // Data Pointer 3 is stepped over for the same reason as Q. The other three
                // come back, exactly as a RET brings them back.
                if (i != DATA_POINTERS - 1) {
                    cpu->DataPointer[i] = high | low;
                }
            }
            uint16_t resumeAddress;
            cpu->StackPointer++;
            resumeAddress = (uint16_t)readData(cpu, cpu->StackPointer) << 8;
            cpu->StackPointer++;
            resumeAddress = resumeAddress | (uint16_t)readData(cpu, cpu->StackPointer);
            cpu->ProgramCounter = resumeAddress - 1;
        } break;
        case 0x74:
            // RRET - Return from an RCAL, taking back nothing but the return address.
            cpu->StackPointer++;
            cpu->ProgramCounter = (uint16_t)readData(cpu, cpu->StackPointer) << 8;
            cpu->StackPointer++;
            cpu->ProgramCounter = cpu->ProgramCounter | (uint16_t)readData(cpu, cpu->StackPointer);
            // Two on, to step over the address the RCAL branched through, exactly as RET
            // does. Everything else RET restores, this deliberately does not.
            cpu->ProgramCounter += 2;
        break;
        case 0x75:
            // RET - Return from subroutine, restore the registers and set the Program Counter to the Return Address.
            // Pop A from the Stack.
            cpu->StackPointer++;
            cpu->A = readData(cpu, cpu->StackPointer);
            // Pop B from the Stack.
            cpu->StackPointer++;
            cpu->B = readData(cpu, cpu->StackPointer);
            // Pop the preserved Data Pointers from the Stack. This walks the pointers
            // in the opposite order to genericCall, and takes the high byte before the
            // low byte, so that it exactly mirrors the way they were pushed.
            for (int i = PRESERVED_DATA_POINTERS - 1; i >= 0; i--) {
                cpu->StackPointer++;
                cpu->DataPointer[i] = (uint16_t)readData(cpu, cpu->StackPointer) << 8;
                cpu->StackPointer++;
                cpu->DataPointer[i] |= (uint16_t)readData(cpu, cpu->StackPointer);
            }
            // Pop the Return Address from the Stack.
            cpu->StackPointer++;
            cpu->ProgramCounter = (uint16_t)readData(cpu, cpu->StackPointer) << 8;
            cpu->StackPointer++;
            cpu->ProgramCounter = cpu->ProgramCounter | (uint16_t)readData(cpu, cpu->StackPointer);
            // Add 2 to the Program Counter to skip over the address when it returns.
            cpu->ProgramCounter += 2;

        break;
        //
        // 2x - Register Operations:
        //
        case 0x20:
            // RSTA - Reset A to 0.
            cpu->A = 0;
        break;
        case 0x21:
            // RSTB - Reset B to 0.
            cpu->B = 0;
        break;
        case 0x22:
            // INCA - Add 1 to A.
            // Set the Carry Flag if the register overflows.
            if (cpu->A == 0xFF) {
                cpu->Status |= STATUS_CARRY;
            } else {
                cpu->Status &= ~STATUS_CARRY;
            }
            cpu->A++;
        break;
        case 0x23:
            // INCB - Add 1 to B.
            // Set the Carry Flag if the register overflows.
            if (cpu->B == 0xFF) {
                cpu->Status |= STATUS_CARRY;
            } else {
                cpu->Status &= ~STATUS_CARRY;
            }
            cpu->B++;
        break;
        case 0x24:
            // DECA - Subtract 1 from A.
            // Set the Carry Flag if the register underflows.
            if (cpu->A == 0x00) {
                cpu->Status |= STATUS_CARRY;
            } else {
                cpu->Status &= ~STATUS_CARRY;
            }
            cpu->A--;
        break;
        case 0x25:
            // DECB - Subtract 1 from B.
            // Set the Carry Flag if the register underflows.
            if (cpu->B == 0x00) {
                cpu->Status |= STATUS_CARRY;
            } else {
                cpu->Status &= ~STATUS_CARRY;
            }
            cpu->B--;
        break;
        case 0x26:
            // INIA - Initialize A Immediately from Program Memory.
            cpu->ProgramCounter++;
            cpu->A = fetchProgram(cpu, cpu->ProgramCounter);
        break;
        case 0x27:
            // INIB - Initialize A Immediately from Program Memory.
            cpu->ProgramCounter++;
            cpu->B = fetchProgram(cpu, cpu->ProgramCounter);
        break;
        case 0x28:
            // CCF - Clear the Carry Flag.
            cpu->Status &= ~STATUS_CARRY;
        break;
        case 0x29:
            // MVQA - Copy Q into A.
            cpu->A = cpu->Q;
        break;
        case 0x2A:
            // MVQB - Copy Q into B.
            cpu->B = cpu->Q;
        break;
        case 0x2B:
            // SIF - Set the Interrupt Flag, enabling hardware interrupts.
            cpu->Status |= STATUS_INTERRUPT;
        break;
        case 0x2C:
            // CIF - Clear the Interrupt Flag, disabling hardware interrupts.
            // Software interrupts and faults are delivered either way, so this
            // only ever holds off a device.
            cpu->Status &= ~STATUS_INTERRUPT;
        break;
        //
        // 3x - Stack Operations:
        //
        case 0x30:
            // PSHQ - Push Q to the Stack.
            writeData(cpu, cpu->StackPointer, cpu->Q);
            cpu->StackPointer--;
        break;
        case 0x31:
            // PSHA - Push A to the Stack.
            writeData(cpu, cpu->StackPointer, cpu->A);
            cpu->StackPointer--;
        break;
        case 0x32:
            // PSHB - Push B to the Stack.
            writeData(cpu, cpu->StackPointer, cpu->B);
            cpu->StackPointer--;
        break;
        case 0x33: {
            // PSHD - Push the selected Data Pointer Address to the Stack.
            // Order, high byte, low byte
            // This ordering makes it easier to add offsets with register math.
            uint16_t pushed = *selectDataPointer(cpu);
            writeData(cpu, cpu->StackPointer, (pushed >> 8) & 0xFF);
            cpu->StackPointer--;
            writeData(cpu, cpu->StackPointer, pushed & 0xFF);
            cpu->StackPointer--;
        }
        break;
        case 0x34:
            // POPA - Pop A from the Stack.
            cpu->StackPointer++;
            cpu->A = readData(cpu, cpu->StackPointer);

        break;
        case 0x35:
            // POPB - Pop B from the Stack.
            cpu->StackPointer++;
            cpu->B = readData(cpu, cpu->StackPointer);
        break;
        case 0x36: {
            // POPD - Pop a Data Address from the Stack into the selected Data Pointer.
            uint16_t *popped = selectDataPointer(cpu);
            cpu->StackPointer++;
            *popped = (uint16_t)readData(cpu, cpu->StackPointer);
            cpu->StackPointer++;
            *popped |= (uint16_t)readData(cpu, cpu->StackPointer) << 8;
        }
        break;
        //
        // 4x - Data Operations:
        //
        case 0x40:
            // INCD - Increment the selected Data Pointer.
            (*selectDataPointer(cpu))++;
        break;
        case 0x41:
            // DECD - Decrement the selected Data Pointer.
            (*selectDataPointer(cpu))--;
        break;
        case 0x42:
            // LDA - Load A from Data.
            cpu->A = readData(cpu, *selectDataPointer(cpu));
        break;
        case 0x43:
            // LDB - Load B from Data.
            cpu->B = readData(cpu, *selectDataPointer(cpu));
        break;
        case 0x44:
            // STQ - Store Q into Data.
            writeData(cpu, *selectDataPointer(cpu), cpu->Q);
        break;
        case 0x45:
            // STA - Store A into Data.
            writeData(cpu, *selectDataPointer(cpu), cpu->A);
        break;
        case 0x46:
            // STB - Store B into Data.
            writeData(cpu, *selectDataPointer(cpu), cpu->B);
        break;
        case 0x47: {
            // SETD - Set the selected Data Pointer.
            uint16_t *destination = selectDataPointer(cpu);
            cpu->ProgramCounter++;
            uint16_t Address;
            Address = (uint16_t)fetchProgram(cpu, cpu->ProgramCounter) << 8; // Cast the 8 bits to a 16 bit value and shift them to the high byte.
            cpu->ProgramCounter++;
            Address |= (uint16_t)fetchProgram(cpu, cpu->ProgramCounter);
            *destination = Address;
        }
        break;
        case 0x48: {
            // DPUP - Offset the selected Data Pointer up by the value of the next byte of Program Memory.
            uint16_t *target = selectDataPointer(cpu);
            cpu->ProgramCounter++;
            *target += fetchProgram(cpu, cpu->ProgramCounter);
        }
        break;
        case 0x49: {
            // DPDN - Offset the selected Data Pointer down by the value of the next byte of Program Memory.
            uint16_t *target = selectDataPointer(cpu);
            cpu->ProgramCounter++;
            *target -= fetchProgram(cpu, cpu->ProgramCounter);
        }
        break;
        case 0x4E: {
            // DPUA - Offset the selected Data Pointer up by A.
            //
            // A rather than Q, because Q is what the ALU last worked out and would be
            // gone by the time anything had been added to it. Working a step out and then
            // moving a pointer by it took a store and a reload before this existed.
            uint16_t *target = selectDataPointer(cpu);
            *target += cpu->A;
        }
        break;
        case 0x4F: {
            // DPDA - Offset the selected Data Pointer down by A.
            uint16_t *target = selectDataPointer(cpu);
            *target -= cpu->A;
        }
        break;
        case 0x50: {
            // DPUW - Offset the selected Data Pointer up by A and B together, A being the
            // most significant, which is how every sixteen bit value on this machine is
            // carried between a pair of registers.
            uint16_t *target = selectDataPointer(cpu);
            *target += ((uint16_t)cpu->A << 8) | (uint16_t)cpu->B;
        }
        break;
        case 0x51: {
            // DPDW - Offset the selected Data Pointer down by A and B together.
            uint16_t *target = selectDataPointer(cpu);
            *target -= ((uint16_t)cpu->A << 8) | (uint16_t)cpu->B;
        }
        break;
        case 0x4A: {
            // LDD - Load the first Data Pointer from the two bytes of Data Memory
            // addressed by the second. Byte order matches everywhere else an address
            // is stored: most significant first, then least significant.
            // The source is taken by value before anything is written, so LDD.0.0
            // follows the pointer in DP0 rather than tripping over itself.
            uint16_t *destination = selectDataPointer(cpu);
            uint16_t source = *selectDataPointer(cpu);
            *destination = (uint16_t)readData(cpu, source) << 8;
            *destination |= (uint16_t)readData(cpu, (uint16_t)(source + 1));
        }
        break;
        case 0x4B: {
            // STD - Store the first Data Pointer into the two bytes of Data Memory
            // addressed by the second. The cast on the second address keeps it inside
            // Data Memory when the pointer sits at the very top of it.
            uint16_t value = *selectDataPointer(cpu);
            uint16_t address = *selectDataPointer(cpu);
            writeData(cpu, address, (value >> 8) & 0xFF);
            writeData(cpu, (uint16_t)(address + 1), value & 0xFF);
        }
        break;
        case 0x4C: {
            // MVSD - Copy the Stack Pointer into the selected Data Pointer.
            //
            // The Stack Pointer still cannot be written, so this does not let a program
            // move the Stack. It lets a program find it, which is what reading anything
            // already on the Stack requires. An interrupt handler needs this to reach
            // its own frame, and so does anything that wants to walk back through the
            // calls that led to where it is.
            uint16_t *target = selectDataPointer(cpu);
            *target = cpu->StackPointer;
        }
        break;
        case 0x4D: {
            // MVDS - Copy the selected Data Pointer into the Stack Pointer.
            //
            // This one is dangerous and is meant to be used rarely. Moving the Stack
            // under a running program abandons every return address on it, so a RET
            // after this goes wherever the new Stack happens to say.
            //
            // It exists because a system that runs other programs has no other way to
            // get its Stack back. A program that gives up part way through leaves
            // whatever it pushed behind, and the interrupt frame that carried the
            // request to stop is on there too. Without this the Stack only ever grows
            // downward, one abandoned program at a time, and a shell cannot outlive
            // many of them.
            uint16_t *source = selectDataPointer(cpu);
            cpu->StackPointer = *source;
        }
        break;
        //
        // Dx - Output Operations:
        //
        case 0xD0: {
            // OUTQ - Write the value of Q to an output port.
            uint16_t site = cpu->ProgramCounter;
            cpu->ProgramCounter++;
            portOut(cpu, cpu->Q, fetchProgram(cpu, cpu->ProgramCounter));
            answerRefusal(cpu, site);
        }
        break;
        case 0xD1: {
            // OUTA - Write the value of A to an output port.
            uint16_t site = cpu->ProgramCounter;
            cpu->ProgramCounter++;
            portOut(cpu, cpu->A, fetchProgram(cpu, cpu->ProgramCounter));
            answerRefusal(cpu, site);
        }
        break;
        case 0xD2: {
            // OUTB - Write the value of B to an output port.
            uint16_t site = cpu->ProgramCounter;
            cpu->ProgramCounter++;
            portOut(cpu, cpu->B, fetchProgram(cpu, cpu->ProgramCounter));
            answerRefusal(cpu, site);
        }
        break;
        //
        // Ex - Input Operations:
        //
        case 0xE0: {
            // INA - Read an Input to A.
            uint16_t site = cpu->ProgramCounter;
            cpu->ProgramCounter++;
            cpu->A = portIn(cpu, fetchProgram(cpu, cpu->ProgramCounter));
            answerRefusal(cpu, site);
        }
        break;
        case 0xE1: {
            // INB - Read an Input to B.
            uint16_t site = cpu->ProgramCounter;
            cpu->ProgramCounter++;
            cpu->B = portIn(cpu, fetchProgram(cpu, cpu->ProgramCounter));
            answerRefusal(cpu, site);
        }
        break;
        //
        // Fx - Special Operations:
        //
        case 0xF0:
            // NOP - Do nothing.
        break;
        case 0xFE:
            // WAIT - Stop fetching until a device asks for attention.
            //
            // NOT A HALT. The machine is still clocked and devices still run; what stops
            // is the CPU's use of the bus. HALT is how a program says it has finished and
            // must stay that way, so this is a separate instruction rather than a gentler
            // HALT - and its state is a field of its own rather than a Status bit, for
            // the reason cpu.h gives.
            //
            // A LINE ALREADY UP MEANS THERE IS NOTHING TO WAIT FOR, and that is what
            // makes the ordinary idiom race-free: a program tests its device, finds it
            // busy, and waits. If the device finished in between, the line is standing
            // and this does nothing at all rather than sleeping through the answer.
            if (cpu->bus->nextInterrupt() < 0) {
                cpu->Waiting = 1;
            }
        break;
        case 0xFF:
            // HALT - Set the Halt Bit of the Status Register.
            cpu->Status |= STATUS_HALT;
        break;
        default:
            // Unknown Instruction.
            return 1;
    }
    return 0;
}

void stepCPU(CPURegisters *cpu) {
    if (!(cpu->Status & STATUS_HALT)) {
        // Devices get their moment before the lines are read, and unconditionally: a
        // device is entitled to notice something whether or not the CPU is currently
        // willing to be interrupted about it. Masking decides when a request is answered,
        // not whether the outside world is allowed to have happened.
        serviceDevices();
        // ---- Stopped in a WAIT ----
        //
        // Nothing is fetched and nothing is executed. A clock still passes, because a
        // device that takes time has to be able to reach the end of it, and it is charged
        // to idle rather than to the bus: the CPU is not using memory.
        //
        // A LINE OF ANY KIND ENDS THE WAIT, masked or not. Masking says who answers a
        // request, not whether it happened - so a program can sleep on a device it has no
        // handler for and simply read its status afterwards, which is the whole reason
        // this is worth having and is what the filesystem does with it.
        if (cpu->Waiting) {
            if (cpu->bus->nextInterrupt() < 0) {
                cpu->idleCycles++;
                return;
            }
            cpu->Waiting = 0;
            // Woken while masked, so nobody is going to answer this line and take it
            // down. IT HAS TO BE TAKEN DOWN HERE. Left standing it would be found by the
            // next WAIT, which would return at once, and by the one after that - the
            // program would spin exactly as it did before while appearing to sleep.
            //
            // Unmasked, the dispatch below takes it down instead, and a line with no
            // handler faults there the way it always has. Waiting changes what the CPU
            // does between instructions; it does not change interrupt policy.
            if (!(cpu->Status & STATUS_INTERRUPT)) {
                cpu->bus->clearInterrupt((uint8_t)cpu->bus->nextInterrupt());
            }
        }
        // A device asking for attention is answered between instructions and never
        // inside one, so the address that goes into the frame is always the start of an
        // instruction and RETI always lands somewhere meaningful.
        //
        // A line that is up while the Interrupt Flag is clear stays up. Masking holds a
        // device off; it does not lose what the device was asking for.
        if (cpu->Status & STATUS_INTERRUPT) {
            int port = cpu->bus->nextInterrupt();
            if (port >= 0) {
                cpu->bus->clearInterrupt((uint8_t)port);
                if (enterInterrupt(cpu, HARDWARE_VECTOR_BASE, (uint8_t)port, cpu->ProgramCounter)) {
                    // The device asked and nobody was listening. enterInterrupt has
                    // already stopped the machine; correct the cause, because the empty
                    // entry is in the hardware table rather than the software one.
                    cpu->Fault = FAULT_NO_DEVICE_HANDLER;
                    return;
                }
                // Entering the handler is what this cycle did, so no instruction runs.
                // The step puts the Program Counter on the handler's first byte, the
                // same way it does everywhere else.
                cpu->ProgramCounter++;
                return;
            }
        }
        // The CPU is not halted, so do a cycle.
        if (executeOperation(fetchProgram(cpu, cpu->ProgramCounter), cpu)) {
            // Nothing decodes that byte. Hand it to the fault vector, which gets the
            // address of the offending byte itself rather than the one after it, so
            // that a handler can read the byte that failed and say what it was.
            //
            // A handler returning with a bare RETI will therefore meet the same byte
            // again. That is the documented behaviour: resuming past a fault means
            // deciding where to resume, which is the handler's business and not the
            // CPU's guess.
            uint16_t faultingAddress = cpu->ProgramCounter;
            if (enterInterrupt(cpu, SOFTWARE_VECTOR_BASE, VECTOR_INVALID_OPCODE, faultingAddress)) {
                // Nothing is installed, so stop where we are. The Program Counter is
                // still on the offending byte, which is what the report wants. The
                // cause is the byte, not the empty vector, so say so.
                cpu->Fault = FAULT_BAD_OPCODE;
                return;
            }
            // Dispatched. Fall through, so the step below lands on the handler.
        }
        cpu->ProgramCounter++;
    }
}