Bweh/src/cpu.rs

//! The [CPU] executes [instructions](Insn), reading and writing from a [`BusIO`] device
use self::{
    disasm::{Cond, Insn, Prefixed, R16Indirect, R16Stack, R16, R8},
    split_register::SplitRegister,
};
use crate::{
    error::{Error, ErrorKind::*},
    memory::io::BusIO,
};
use bitfield_struct::bitfield;
use std::{
    collections::BTreeSet,
    fmt::Display,
    num::Wrapping,
    ops::{Index, IndexMut},
};

pub mod disasm;

pub mod split_register;

#[bitfield(u8, order = Msb)]
/// Stores the flags
#[derive(PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct Flags {
    z: bool,
    n: bool,
    h: bool,
    c: bool,
    #[bits(4)]
    _reserved: (),
}

/// Performs the half-carry calculation for addition
fn half_carry_add(a: u8, b: u8) -> bool {
    (a & 0xf) + (b & 0xf) > 0xf
}
/// Performs the half-carry calculation for subtraction
fn half_carry_sub(a: u8, b: u8) -> bool {
    (a & 0xf) < (b & 0xf)
}

#[derive(Clone, Copy, Debug, Default, PartialEq, PartialOrd)]
pub enum Ime {
    #[default]
    Disabled,
    ShouldEnable,
    Enabled,
}

/// Processes interrupts, executes, fetches and decodes instructions
#[derive(Clone, Debug, Default, PartialEq, PartialOrd)]
pub struct CPU {
    /// The last-fetched instruction
    ir: Insn,
    /// The Stack Pointer stores the location of the program stack
    sp: Wrapping<u16>,
    /// The Program Counter increments for each byte of an instruction read
    pc: Wrapping<u16>,
    /// Combines the Accumulator and Flags registers
    af: SplitRegister,
    /// Combines the B and C registers
    bc: SplitRegister,
    /// Combines the D and E registers
    de: SplitRegister,
    /// Combines the H and L registers
    hl: SplitRegister,
    /// The number of processor cycles executed in the last instruction
    m_cycles: u8,
    /// [Interrupt Master Enable](https://gbdev.io/pandocs/Interrupts.html#ime-interrupt-master-enable-flag-write-only)
    ime: Ime,
    /// The set of breakpoints
    breakpoints: BTreeSet<u16>,
}

/// Lifecycle management
impl CPU {
    /// Initializes a new [CPU]
    ///
    /// This sets up the registers [as if it were a GBA][CPU Registers]
    ///
    /// [CPU Registers]: https://gbdev.io/pandocs/Power_Up_Sequence.html#cpu-registers
    pub fn new() -> Self {
        let mut out = Self::default();
        out.af.wide_mut().0 = 0x1100 | Flags::new().with_z(true).into_bits() as u16;
        out.bc.wide_mut().0 = 0x0100;
        out.de.wide_mut().0 = 0xff56;
        out.hl.wide_mut().0 = 0x000d;
        out.pc.0 = 0x0100;
        out.sp.0 = 0xfffe;
        out
    }
}

/// Getters for private data
impl CPU {
    /// Gets the address of the next instruction
    pub fn pc(&self) -> u16 {
        self.pc.0
    }
    /// Sets the address of the next instruction
    pub fn set_pc(&mut self, addr: u16) {
        self.pc.0 = addr
    }
    /// Gets the current instruction
    pub fn ir(&self) -> Insn {
        self.ir
    }
    /// Sets a breakpoint. Returns true if the breakpoint was not already set.
    pub fn set_break(&mut self, addr: u16) -> bool {
        self.breakpoints.insert(addr)
    }
    /// Removes a breakpoint. Returns true if the breakpoint was set.
    pub fn unset_break(&mut self, addr: u16) -> bool {
        self.breakpoints.remove(&addr)
    }
}

/// Bus operations
impl CPU {
    /// Read an 8-bit immediate at the program counter, post-incrementing it (adds one cycle)
    pub fn imm8(&mut self, bus: &impl BusIO) -> Result<u8, Error> {
        let out = self.read(self.pc.0, bus)?;
        self.pc += 1;
        Ok(out)
    }
    /// Read a 16-bit immediate at the program counter, post-incrementing it twice (adds two cycles)
    pub fn imm16(&mut self, bus: &impl BusIO) -> Result<u16, Error> {
        let out = self.read16(self.pc.0, bus)?;
        self.pc += 2;
        Ok(out)
    }
    /// Read an 8-bit value at address `addr` on the `bus` (adds one cycle)
    pub fn read(&mut self, addr: u16, bus: &impl BusIO) -> Result<u8, Error> {
        // Reading a byte takes one cycle
        self.wait();
        bus.read(addr as usize).ok_or(InvalidAddress(addr).into())
    }
    /// Read a 16-bit LE value at address `addr` on the `bus` (adds two cycles)
    pub fn read16(&mut self, addr: u16, bus: &impl BusIO) -> Result<u16, Error> {
        let out = self.read(addr, bus)? as u16;
        Ok(out | (self.read(addr.wrapping_add(1), bus)? as u16) << 8)
    }

    /// Write an 8-bit value to address `addr` on the `bus` (adds one cycle)
    pub fn write(&mut self, addr: u16, data: u8, bus: &mut impl BusIO) -> Result<(), Error> {
        // Writing a byte takes one cycle
        self.wait();
        bus.write(addr as usize, data)
            .ok_or(InvalidAddress(addr).into())
    }
    /// Write a 16-bit LE value to address `addr..=addr+1` on the `bus` (adds two cycles)
    pub fn write16(&mut self, addr: u16, data: u16, bus: &mut impl BusIO) -> Result<(), Error> {
        let data = data.to_le_bytes();
        self.write(addr, data[0], bus)?;
        self.write(addr.wrapping_add(1), data[1], bus)
    }

    /// Push a byte onto the stack (adds one cycle)
    pub fn push(&mut self, data: u8, bus: &mut impl BusIO) -> Result<(), Error> {
        self.sp -= 1;
        self.write(self.sp.0, data, bus)
    }
    /// Push a 16-bit value onto the stack (adds two cycles)
    pub fn push16(&mut self, data: u16, bus: &mut impl BusIO) -> Result<(), Error> {
        let data = data.to_le_bytes();
        self.push(data[0], bus)?;
        self.push(data[1], bus)
    }
    /// Pop a byte off the stack (adds one cycle)
    pub fn pop(&mut self, bus: &mut impl BusIO) -> Result<u8, Error> {
        let out = self.read(self.sp.0, bus);
        self.sp += 1;
        out
    }
    /// Pop a double word off the stack (adds two cycles)
    pub fn pop16(&mut self, bus: &mut impl BusIO) -> Result<u16, Error> {
        // pushes LH, pops HL
        Ok(u16::from_be_bytes([self.pop(bus)?, self.pop(bus)?]))
    }

    /// Jump to a memory address (adds one cycle)
    pub fn jumpto(&mut self, addr: u16) {
        // Jumping takes a cycle
        self.wait().pc.0 = addr;
    }

    /// Waits one M-Cycle (does not delay)
    pub fn wait(&mut self) -> &mut Self {
        self.m_cycles += 1;
        self
    }
}

/// Interrupts
impl CPU {
    pub fn service_irq(&mut self, bus: &mut impl BusIO) -> Result<(), Error> {
        const IRQ_HANDLERS: [u16; 5] = [0x40, 0x48, 0x50, 0x58, 0x60];
        if self.ime != Ime::Enabled {
            return Ok(());
        }

        let enabled = self.read(0xffff, bus)?;
        let interrupt = (enabled & self.read(0xff0f, bus)?).trailing_zeros() as usize;
        if interrupt < IRQ_HANDLERS.len() {
            eprintln!("Interrupt requested: {interrupt}");
            self.di()?;
            self.jumpto(IRQ_HANDLERS[interrupt]);
        }
        Ok(())
    }
}

mod instructions {
    use super::*;

    type IResult = std::result::Result<(), Error>;

    /// Control instructions (Nop, Stop, Halt, D/EI)
    impl CPU {
        pub fn nop(&mut self) -> IResult {
            Ok(())
        }
        pub fn stop(&mut self) -> IResult {
            Err(UnimplementedInsn(self.ir).into())
        }
        pub fn halt(&mut self) -> IResult {
            Ok(())
        }
        pub fn di(&mut self) -> IResult {
            self.ime = Ime::Disabled;
            Ok(())
        }
        pub fn ei(&mut self) -> IResult {
            // TODO: this is incorrect
            self.ime = Ime::ShouldEnable;
            Ok(())
        }
    }
    /// Loads and stores
    impl CPU {
        pub fn ld(&mut self, dst: R8, src: R8, bus: &mut impl BusIO) -> IResult {
            let src = self.get_r8(src, bus);
            self.set_r8(dst, src, bus);
            Ok(())
        }
        pub fn ld_imm(&mut self, dst: R8, bus: &mut impl BusIO) -> IResult {
            // TODO: open bus behavior
            let src = self.imm8(bus)?;
            self.set_r8(dst, src, bus);
            Ok(())
        }
        pub fn ld_imm16(&mut self, dst: R16, bus: &mut impl BusIO) -> IResult {
            let imm16 = self.imm16(bus)?;
            self[dst].0 = imm16;
            Ok(())
        }
        pub fn ld_ind(&mut self, src: R16Indirect, bus: &mut impl BusIO) -> IResult {
            self[R8::A].0 = self.read_r16ind(src, bus)?;
            Ok(())
        }
        pub fn st_ind(&mut self, dst: R16Indirect, bus: &mut impl BusIO) -> IResult {
            self.write_r16ind(dst, self[R8::A].0, bus);
            Ok(())
        }
        pub fn ldc(&mut self, bus: &mut impl BusIO) -> IResult {
            let addr = self[R8::C].0 as u16 | 0xff00;
            self[R8::A].0 = self.read(addr, bus)?;
            Ok(())
        }
        pub fn stc(&mut self, bus: &mut impl BusIO) -> IResult {
            let addr = self[R8::C].0 as u16 | 0xff00;
            self.write(addr, self[R8::A].0, bus)?;
            Ok(())
        }
        pub fn ld_abs(&mut self, bus: &mut impl BusIO) -> IResult {
            let addr = self.imm16(bus)?;
            self[R8::A].0 = self.read(addr, bus)?;
            Ok(())
        }
        pub fn st_abs(&mut self, bus: &mut impl BusIO) -> IResult {
            let addr = self.imm16(bus)?;
            self.write(addr, self[R8::A].0, bus)?;
            Ok(())
        }
        pub fn st_sp_abs(&mut self, bus: &mut impl BusIO) -> IResult {
            let addr = self.imm16(bus)?;
            self.write16(addr, self[R16::SP].0, bus)?;
            Ok(())
        }
        pub fn ld_sp_hl(&mut self) -> IResult {
            self[R16::HL].0 = self[R16::SP].0;
            Ok(())
        }
        pub fn ld_hl_sp_rel(&mut self, bus: &mut impl BusIO) -> IResult {
            let offset = self.imm8(bus)? as i8 as i16 as u16;
            self[R16::HL].0 = self.sp.0.wrapping_add(offset);
            Ok(())
        }
        pub fn ldh(&mut self, bus: &mut impl BusIO) -> IResult {
            let addr = self.imm8(bus)? as u16 | 0xff00;
            self[R8::A].0 = self.read(addr, bus)?;
            Ok(())
        }
        pub fn sth(&mut self, bus: &mut impl BusIO) -> IResult {
            let addr = self.imm8(bus)? as u16 | 0xff00;
            self.write(addr, self[R8::A].0, bus)?;
            Ok(())
        }
    }
    /// Inc and Dec
    impl CPU {
        pub fn inc(&mut self, reg: R8, bus: &mut impl BusIO) -> IResult {
            let (data, carry) = self.get_r8(reg, bus).overflowing_add(1);
            self.set_r8(reg, data, bus);
            *self.flags_mut() = Flags::new().with_z(data == 0).with_c(carry);
            Ok(())
        }
        pub fn dec(&mut self, reg: R8, bus: &mut impl BusIO) -> IResult {
            let (data, carry) = self.get_r8(reg, bus).overflowing_sub(1);
            self.set_r8(reg, data, bus);
            *self.flags_mut() = Flags::new().with_z(data == 0).with_c(carry);
            Ok(())
        }
        pub fn inc16(&mut self, reg: R16) -> IResult {
            self[reg] -= 1;
            Ok(())
        }
        pub fn dec16(&mut self, reg: R16) -> IResult {
            self[reg] -= 1;
            Ok(())
        }
    }
    /// Rotates and shifts
    impl CPU {
        pub fn rlnca(&mut self) -> IResult {
            self[R8::A].0 = self[R8::A].0.rotate_left(1);
            let carry = self[R8::A].0 & 1 != 0;
            self.set_z(false).set_n(false).set_h(false).set_c(carry);
            Ok(())
        }
        pub fn rrnca(&mut self) -> IResult {
            let carry = self[R8::A].0 & 1 != 0;
            self[R8::A].0 = self[R8::A].0.rotate_right(1);
            self.set_z(false).set_n(false).set_h(false).set_c(carry);
            Ok(())
        }
        pub fn rla(&mut self) -> IResult {
            // TODO: optimize RLA/RRA
            let carry = self[R8::A].0 & 0x80 != 0;
            self[R8::A] <<= 1;
            let prev_carry = self.c();
            self[R8::A] |= prev_carry as u8;
            self.set_z(false).set_n(false).set_h(false).set_c(carry);
            Ok(())
        }
        pub fn rra(&mut self) -> IResult {
            let carry = self[R8::A].0 & 1 != 0;
            self[R8::A] >>= 1;
            let prev_carry = self.c();
            self[R8::A] |= (prev_carry as u8) << 7;
            self.set_z(false).set_n(false).set_h(false).set_c(carry);
            Ok(())
        }
    }
    /// Funky instructions
    impl CPU {
        pub fn daa(&mut self) -> IResult {
            Err(UnimplementedInsn(self.ir))?
        }
        pub fn cpl(&mut self) -> IResult {
            self[R8::A] ^= 0xff;
            self.set_n(false).set_h(false);
            Ok(())
        }
        pub fn scf(&mut self) -> IResult {
            let flags = self.flags_mut();
            *flags = flags.with_n(false).with_h(false).with_c(true);
            Ok(())
        }
        pub fn ccf(&mut self) -> IResult {
            let flags = self.flags_mut();
            *flags = flags.with_n(false).with_h(false).with_c(!flags.c());
            Ok(())
        }
    }
    /// Addition
    impl CPU {
        fn add_impl(&mut self, value: u8) -> IResult {
            let hc = half_carry_add(self[R8::A].0, value);
            let (out, carry) = self[R8::A].0.overflowing_add(value);
            self[R8::A].0 = out;
            self.set_z(out == 0).set_n(false).set_h(hc).set_c(carry);
            Ok(())
        }
        pub fn add(&mut self, reg: R8, bus: &mut impl BusIO) -> IResult {
            let src = self.get_r8(reg, bus);
            self.add_impl(src)
        }
        pub fn addi(&mut self, bus: &mut impl BusIO) -> IResult {
            let src = self.imm8(bus)?;
            self.add_impl(src)?;
            Ok(())
        }
        pub fn add16(&mut self, reg: R16) -> IResult {
            let hl = self.hl.wide().0.to_le_bytes();
            let addn = self[reg].0.to_le_bytes();
            eprintln!("Add {hl:?} to {addn:?} and store the half- and full-carry flags");
            Ok(())
        }
        #[allow(unused)]
        pub fn add16_spi(&mut self, bus: &mut impl BusIO) -> IResult {
            let offset = self.imm8(bus)? as i8 as i16; // sign extend

            Err(UnimplementedInsn(self.ir).into())
        }
    }
    /// Addition with carry
    impl CPU {
        fn addc_impl(&mut self, value: u8) -> IResult {
            let (src, carry_in) = value.overflowing_add(self.c() as u8);
            let hc = half_carry_add(self[R8::A].0, src);
            let (out, carry) = self[R8::A].0.overflowing_add(src);
            self[R8::A].0 = out;
            self.set_z(out == 0)
                .set_n(false)
                .set_h(hc)
                .set_c(carry | carry_in);
            Ok(())
        }
        pub fn adc(&mut self, reg: R8, bus: &mut impl BusIO) -> IResult {
            let src = self.get_r8(reg, bus);
            self.addc_impl(src)
        }
        pub fn adci(&mut self, bus: &mut impl BusIO) -> IResult {
            let src = self.imm8(bus)?;
            self.addc_impl(src)
        }
    }
    /// Subtraction
    impl CPU {
        fn sub_impl(&mut self, value: u8) -> IResult {
            let hc = half_carry_sub(self[R8::A].0, value);
            let (out, carry) = self[R8::A].0.overflowing_sub(value);
            self.set_z(out == 0).set_n(true).set_h(hc).set_c(carry);
            self[R8::A].0 = out;
            Ok(())
        }
        pub fn sub(&mut self, reg: R8, bus: &mut impl BusIO) -> IResult {
            let value = self.get_r8(reg, bus);
            self.sub_impl(value)
        }
        pub fn subi(&mut self, bus: &mut impl BusIO) -> IResult {
            let value = self.imm8(bus)?;
            self.sub_impl(value)
        }
    }
    /// Subtraction with carry
    impl CPU {
        #[inline]
        fn subc_impl(&mut self, value: u8) -> IResult {
            let (src, cin) = value.overflowing_sub(self.c() as u8);
            let (out, carry) = self[R8::A].0.overflowing_sub(src);
            let hc = half_carry_sub(self[R8::A].0, src);
            self[R8::A].0 = out;
            self.set_z(out == 0)
                .set_n(true)
                .set_h(hc)
                .set_c(carry | cin);
            Ok(())
        }
        pub fn sbc(&mut self, reg: R8, bus: &mut impl BusIO) -> IResult {
            let value = self.get_r8(reg, bus);
            self.subc_impl(value)
        }
        pub fn sbci(&mut self, bus: &mut impl BusIO) -> IResult {
            let value = self.imm8(bus)?;
            self.subc_impl(value)
        }
    }
    /// Bitwise AND
    impl CPU {
        fn and_impl(&mut self, value: u8) -> IResult {
            self[R8::A] &= value;
            self.set_z(self[R8::A].0 == 0)
                .set_n(false)
                .set_h(true)
                .set_c(false);
            Ok(())
        }
        pub fn and(&mut self, reg: R8, bus: &mut impl BusIO) -> IResult {
            let value = self.get_r8(reg, bus);
            self.and_impl(value)
        }
        pub fn andi(&mut self, bus: &mut impl BusIO) -> IResult {
            let value = self.imm8(bus)?;
            self.and_impl(value)
        }
    }
    /// Bitwise XOR
    impl CPU {
        fn xor_impl(&mut self, value: u8) -> IResult {
            self[R8::A] ^= value;
            *self.flags_mut() = Flags::new().with_z(self[R8::A].0 == 0);
            Ok(())
        }
        pub fn xor(&mut self, reg: R8, bus: &mut impl BusIO) -> IResult {
            let value = self.get_r8(reg, bus);
            self.xor_impl(value)
        }
        pub fn xori(&mut self, bus: &mut impl BusIO) -> IResult {
            let value = self.imm8(bus)?;
            self.xor_impl(value)
        }
    }
    /// Bitwise OR
    impl CPU {
        fn or_impl(&mut self, value: u8) -> IResult {
            self[R8::A] |= value;
            *self.flags_mut() = Flags::new().with_z(self[R8::A].0 == 0);
            Ok(())
        }
        pub fn or(&mut self, reg: R8, bus: &mut impl BusIO) -> IResult {
            let value = self.get_r8(reg, bus);
            self.or_impl(value)
        }
        pub fn ori(&mut self, bus: &mut impl BusIO) -> IResult {
            let value = self.imm8(bus)?;
            self.or_impl(value)
        }
    }
    /// Comparison
    impl CPU {
        fn cmp_impl(&mut self, src: u8) -> IResult {
            let dst = self[R8::A].0;
            self.sub_impl(src)?;
            self[R8::A].0 = dst;
            Ok(())
        }
        pub fn cmp(&mut self, reg: R8, bus: &mut impl BusIO) -> IResult {
            let src = self.get_r8(reg, bus);
            self.cmp_impl(src)
        }
        pub fn cmpi(&mut self, bus: &mut impl BusIO) -> IResult {
            let src = self.imm8(bus)?;
            self.cmp_impl(src)
        }
    }
    /// Stack ops
    impl CPU {
        pub fn insn_push(&mut self, reg: R16Stack, bus: &mut impl BusIO) -> IResult {
            self.push16(self[reg].0, bus)
        }
        pub fn insn_pop(&mut self, reg: R16Stack, bus: &mut impl BusIO) -> IResult {
            self[reg].0 = self.pop16(bus)?;
            Ok(())
        }
    }
    /// Jumps, branches, calls, and returns
    impl CPU {
        pub fn jr(&mut self, bus: &mut impl BusIO) -> IResult {
            let rel = self.imm8(bus)? as i8 as i16;
            self.pc += rel as u16;
            Ok(())
        }
        pub fn jrc(&mut self, cond: Cond, bus: &mut impl BusIO) -> IResult {
            let rel = self.imm8(bus)? as i8 as i16; // sign extend to 16 bits
            if self.cond(cond) {
                self.pc += rel as u16;
            }
            Ok(())
        }
        pub fn jp(&mut self, bus: &mut impl BusIO) -> IResult {
            // Jump takes an extra cycle
            let addr = self.imm16(bus)?;
            self.jumpto(addr);
            Ok(())
        }
        pub fn jpc(&mut self, cond: Cond, bus: &mut impl BusIO) -> IResult {
            let addr = self.imm16(bus)?;
            if self.cond(cond) {
                self.jumpto(addr)
            }
            Ok(())
        }
        pub fn jp_hl(&mut self) -> IResult {
            self.pc = *self.hl.wide();
            Ok(())
        }

        /// Pushes PC onto the stack, then jumps to the provided
        #[inline]
        pub fn call_addr(&mut self, addr: u16, bus: &mut impl BusIO) -> IResult {
            self.push16(self.pc.0, bus)?;
            self.jumpto(addr);
            Ok(())
        }
        pub fn call(&mut self, bus: &mut impl BusIO) -> IResult {
            let addr = self.imm16(bus)?;
            self.call_addr(addr, bus)
        }
        pub fn callc(&mut self, cond: Cond, bus: &mut impl BusIO) -> IResult {
            let addr = self.imm16(bus)?;
            match self.cond(cond) {
                true => self.call_addr(addr, bus),
                false => Ok(()),
            }
        }
        pub fn ret(&mut self, bus: &mut impl BusIO) -> IResult {
            let addr = self.pop16(bus)?;
            self.jumpto(addr);
            Ok(())
        }
        pub fn retc(&mut self, cond: Cond, bus: &mut impl BusIO) -> IResult {
            // Condition checking here takes an extra cycle: https://gist.github.com/SonoSooS/c0055300670d678b5ae8433e20bea595#ret-cc
            match self.wait().cond(cond) {
                true => self.ret(bus),
                false => Ok(()),
            }
        }
        pub fn reti(&mut self, bus: &mut impl BusIO) -> IResult {
            self.ime = Ime::Enabled; // Technically this should go after ret's pop
            self.ret(bus)
        }
        pub fn reset(&mut self, addr: u8, bus: &mut impl BusIO) -> IResult {
            self.call_addr(addr as u16 * 8, bus)
        }
    }
    /// Prefixed instructions
    impl CPU {
        /// Rotates left, setting the carry flag to the highest bit
        pub fn rlc(&mut self, reg: R8, bus: &mut impl BusIO) -> Result<(), Error> {
            let value = self.get_r8(reg, bus);
            self.set_r8(reg, value.rotate_left(1), bus);
            *self.flags_mut() = Flags::new().with_z(value == 0).with_c(value & 0x80 > 0);
            Ok(())
        }
        /// Rotates right, setting the carry flag to the lowest bit
        pub fn rrc(&mut self, reg: R8, bus: &mut impl BusIO) -> Result<(), Error> {
            let value = self.get_r8(reg, bus);
            self.set_r8(reg, value.rotate_right(1), bus);
            *self.flags_mut() = Flags::new().with_z(value == 0).with_c(value & 1 > 0);
            Ok(())
        }
        /// Rotates left through the carry flag
        pub fn rl(&mut self, reg: R8, bus: &mut impl BusIO) -> Result<(), Error> {
            let value = self.get_r8(reg, bus);
            let carry = self.c() as u8;
            self.set_r8(reg, (value << 1) | carry, bus);
            *self.flags_mut() = Flags::new().with_z(value == 0).with_c(value & 0x80 > 0);
            Ok(())
        }
        /// Rotates right through the carry flag
        pub fn rr(&mut self, reg: R8, bus: &mut impl BusIO) -> Result<(), Error> {
            let value = self.get_r8(reg, bus);
            let carry = self.c() as u8;
            self.set_r8(reg, (carry << 7) | (value >> 1), bus);
            *self.flags_mut() = Flags::new().with_z(value == 0).with_c(value & 1 > 0);
            Ok(())
        }
        /// Shifts left arithmetically
        pub fn sla(&mut self, reg: R8, bus: &mut impl BusIO) -> Result<(), Error> {
            let value = self.get_r8(reg, bus);
            *self.flags_mut() = Flags::new().with_z(value == 0).with_c(value & 0x80 > 0);
            self.set_r8(reg, value << 1, bus);
            Ok(())
        }
        /// Shifts right arithmetically (preserves bit 7)
        pub fn sra(&mut self, reg: R8, bus: &mut impl BusIO) -> Result<(), Error> {
            let value = self.get_r8(reg, bus);
            self.set_r8(reg, (value >> 1) | (value & 0x80), bus);
            *self.flags_mut() = Flags::new().with_z(value == 0).with_c(value & 1 > 0);
            Ok(())
        }
        /// Swaps the upper and lower nybbles of a byte
        pub fn swap(&mut self, reg: R8, bus: &mut impl BusIO) -> Result<(), Error> {
            let mut value = self.get_r8(reg, bus);
            value = (value & 0xf << 4) | (value & 0xf0 >> 4);
            self.set_r8(reg, value, bus);
            *self.flags_mut() = Flags::new().with_z(value == 0);
            Ok(())
        }
        /// Shifts right logically (shifting in zeroes)
        pub fn srl(&mut self, reg: R8, bus: &mut impl BusIO) -> Result<(), Error> {
            let value = self.get_r8(reg, bus);
            self.set_r8(reg, value >> 1, bus);
            *self.flags_mut() = Flags::new().with_z(value == 0).with_c(value & 1 > 0);
            Ok(())
        }
        /// Tests whether the given bit of r8 is set
        pub fn bit(&mut self, reg: R8, bit: u8, bus: &impl BusIO) -> Result<(), Error> {
            let value = self.get_r8(reg, bus) & (1 << bit) == 1;
            self.set_z(value).set_n(false).set_h(true);
            Ok(())
        }
        /// Sets the given bit of the given register to 0
        pub fn res(&mut self, reg: R8, bit: u8, bus: &mut impl BusIO) -> Result<(), Error> {
            let value = self.get_r8(reg, bus);
            self.set_r8(reg, value & !(1 << bit), bus);
            Ok(())
        }
        pub fn set(&mut self, reg: R8, bit: u8, bus: &mut impl BusIO) -> Result<(), Error> {
            let value = self.get_r8(reg, bus);
            self.set_r8(reg, value | 1 << bit, bus);
            Ok(())
        }
    }
}

impl CPU {
    pub fn run(&mut self, bus: &mut impl BusIO) -> Result<usize, Error> {
        self.m_cycles = 0;
        // Run the current instruction
        self.execute(bus)?;

        let prefetch_pc = self.pc.0;
        // Handle interrupts
        match self.ime {
            Ime::Disabled => {}
            Ime::ShouldEnable => self.ime = Ime::Enabled,
            Ime::Enabled => self.service_irq(bus)?,
        }
        // Fetch the next instruction
        self.fetch(bus)?;

        // Process breakpoints
        if self.breakpoints.contains(&prefetch_pc) {
            Err(HitBreak(prefetch_pc, self.m_cycles).into())
        } else {
            Ok(self.m_cycles as usize)
        }
    }
    pub fn fetch(&mut self, bus: &mut impl BusIO) -> Result<(), Error> {
        self.ir = match self.ir {
            // HALT fetches the current instruction, rather than the next
            Insn::Halt => Insn::from(self.read(self.pc.0, bus)?),
            // STOP becomes NOP when the clock resumes
            Insn::Stop => Insn::Nop,
            _ => Insn::from(self.imm8(bus)?),
        };
        Ok(())
    }
    pub fn execute(&mut self, bus: &mut impl BusIO) -> Result<(), Error> {
        #[allow(unused_variables)]
        match self.ir {
            Insn::Nop => self.nop(),
            Insn::Stop => self.stop(),
            Insn::Halt => self.halt(),

            Insn::Ld(dst, src) => self.ld(dst, src, bus),
            Insn::LdImm(dst) => self.ld_imm(dst, bus),
            Insn::LdImm16(dst) => self.ld_imm16(dst, bus),
            Insn::LdInd(src) => self.ld_ind(src, bus),
            Insn::StInd(dst) => self.st_ind(dst, bus),
            Insn::LdC => self.ldc(bus),
            Insn::StC => self.stc(bus),
            Insn::LdAbs => self.ld_abs(bus),
            Insn::StAbs => self.st_abs(bus),
            Insn::StSpAbs => self.st_sp_abs(bus),
            Insn::LdSpHl => self.ld_sp_hl(),
            Insn::LdHlSpRel => self.ld_hl_sp_rel(bus),
            Insn::Ldh => self.ldh(bus),
            Insn::Sth => self.sth(bus),

            Insn::Inc(reg) => self.inc(reg, bus),
            Insn::Inc16(reg) => self.inc16(reg),
            Insn::Dec(reg) => self.dec(reg, bus),
            Insn::Dec16(reg) => self.dec16(reg),

            Insn::Rlnca => self.rlnca(),
            Insn::Rrnca => self.rrnca(),
            Insn::Rla => self.rla(),
            Insn::Rra => self.rra(),

            Insn::Daa => self.daa(),
            Insn::Cpl => self.cpl(),
            Insn::Scf => self.scf(),
            Insn::Ccf => self.ccf(),

            Insn::Add(reg) => self.add(reg, bus),
            Insn::Add16HL(reg) => self.add16(reg),
            Insn::AddI => self.addi(bus),
            Insn::Add16SpI => self.add16_spi(bus),
            Insn::Adc(reg) => self.adc(reg, bus),
            Insn::AdcI => self.adci(bus),
            Insn::Sub(reg) => self.sub(reg, bus),
            Insn::SubI => self.subi(bus),
            Insn::Sbc(reg) => self.sbc(reg, bus),
            Insn::SbcI => self.sbci(bus),
            Insn::And(reg) => self.and(reg, bus),
            Insn::AndI => self.andi(bus),
            Insn::Xor(reg) => self.xor(reg, bus),
            Insn::XorI => self.xori(bus),
            Insn::Or(reg) => self.or(reg, bus),
            Insn::OrI => self.ori(bus),
            Insn::Cp(reg) => self.cmp(reg, bus),
            Insn::CpI => self.cmpi(bus),
            Insn::Push(reg) => self.insn_push(reg, bus),
            Insn::Pop(reg) => self.insn_pop(reg, bus),
            // Jumps
            Insn::Jr => self.jr(bus),
            Insn::Jrc(cond) => self.jrc(cond, bus),
            Insn::Jp => self.jp(bus),
            Insn::Jpc(cond) => self.jpc(cond, bus),
            Insn::JpHL => self.jp_hl(),
            Insn::Call => self.call(bus),
            Insn::Callc(cond) => self.callc(cond, bus),
            Insn::Ret => self.ret(bus),
            Insn::Retc(cond) => self.retc(cond, bus),
            Insn::Di => self.di(),
            Insn::Ei => self.ei(),
            Insn::Reti => self.reti(bus),
            Insn::Rst(addr) => self.reset(addr, bus),
            Insn::PrefixCB => self.prefixed(bus),
            Insn::Invalid(b) => Err(InvalidInstruction(b).into()),
        }
    }
    pub fn prefixed(&mut self, bus: &mut impl BusIO) -> Result<(), Error> {
        match Prefixed::from(self.imm8(bus)?) {
            Prefixed::Rlc(reg) => self.rlc(reg, bus),
            Prefixed::Rrc(reg) => self.rrc(reg, bus),
            Prefixed::Rl(reg) => self.rl(reg, bus),
            Prefixed::Rr(reg) => self.rr(reg, bus),
            Prefixed::Sla(reg) => self.sla(reg, bus),
            Prefixed::Sra(reg) => self.sra(reg, bus),
            Prefixed::Swap(reg) => self.swap(reg, bus),
            Prefixed::Srl(reg) => self.srl(reg, bus),
            Prefixed::Bit(reg, bit) => self.bit(reg, bit as _, bus),
            Prefixed::Res(reg, bit) => self.res(reg, bit as _, bus),
            Prefixed::Set(reg, bit) => self.set(reg, bit as _, bus),
        }
    }

    /// Gets an 8-bit register. This requires bus access, since `[HL]` is a valid location
    ///
    /// Panic-free alternative to [Index]
    pub fn get_r8(&mut self, r8: R8, bus: &impl BusIO) -> u8 {
        match r8 {
            R8::HLmem => {
                self.m_cycles += 1;
                self.read(self.hl.wide().0, bus).unwrap_or(0xff)
            }
            _ => self[r8].0,
        }
    }
    /// Sets an 8-bit register. This requires bus access, since `[HL]` is a valid location
    ///
    /// Panic-free alternative to [IndexMut]
    pub fn set_r8(&mut self, r8: R8, data: u8, bus: &mut impl BusIO) {
        match r8 {
            R8::HLmem => {
                self.m_cycles += 1;
                let _ = self.write(self.hl.wide().0, data, bus);
            }
            _ => self[r8].0 = data,
        }
    }
    pub fn get_r16(&self, r16: R16) -> u16 {
        self[r16].0
    }
    pub fn set_r16(&mut self, r16: R16, data: u16) {
        self[r16].0 = data;
    }
    pub fn get_r16ind_addr(&mut self, r16: R16Indirect) -> u16 {
        match r16 {
            R16Indirect::BC => self.bc.wide().0,
            R16Indirect::DE => self.de.wide().0,
            R16Indirect::HLI => {
                let addr = self.hl.wide().0;
                *self.hl.wide_mut() += 1;
                addr
            }
            R16Indirect::HLD => {
                let addr = self.hl.wide().0;
                *self.hl.wide_mut() -= 1;
                addr
            }
        }
    }
    /// Reads data at an R16-indirect address
    pub fn read_r16ind(&mut self, r16: R16Indirect, bus: &impl BusIO) -> Result<u8, Error> {
        let addr = self.get_r16ind_addr(r16);
        self.read(addr, bus)
    }
    pub fn write_r16ind(&mut self, r16: R16Indirect, data: u8, bus: &mut impl BusIO) {
        let addr = self.get_r16ind_addr(r16);
        let _ = self.write(addr, data, bus);
    }
    pub fn flags_mut(&mut self) -> &mut Flags {
        self.af.flags_mut()
    }
    pub fn flags(&self) -> Flags {
        self.af.flags().to_owned()
    }
    flags_getters_setters! {
        /// the `Zero` Flag
        z, set_z;
        /// the `Subtract` Flag
        n, set_n;
        /// the `Half-Carry` Flag
        h, set_h;
        /// the `Carry` Flag
        c, set_c;
    }
    /// Compares the given [Cond] to the CPU's condition flags
    pub fn cond(&self, cond: Cond) -> bool {
        match cond {
            Cond::NZ => !self.z(),
            Cond::Z => self.z(),
            Cond::NC => !self.c(),
            Cond::C => self.c(),
        }
    }
}

/// Takes arguments of the form
/// ```rust,ignore
/// /// the `Flag` Flag
/// #[attributes ... ]
/// flag, set_flag, Flags::FLAG;
/// ```
macro flags_getters_setters($($(#[$meta:meta])* $get:ident, $set:ident);*$(;)?) {$(
    /// Gets
    $(#[$meta])*
    pub fn $get (&self) -> bool {
        self.af.flags().$get()
    }
    /// Sets
    $(#[$meta])*
    pub fn $set (&mut self, to: bool) -> &mut Self{
        self.af.flags_mut().$set(to);
        self
    }
)*}

impl Index<R8> for CPU {
    type Output = Wrapping<u8>;
    fn index(&self, index: R8) -> &Self::Output {
        match index {
            R8::A => self.af.hi(),
            R8::B => self.bc.hi(),
            R8::C => self.bc.lo(),
            R8::D => self.de.hi(),
            R8::E => self.de.lo(),
            R8::H => self.hl.hi(),
            R8::L => self.hl.lo(),
            R8::HLmem => unimplemented!("Cannot index CPU with R8::HLmem without Bus"),
        }
    }
}
impl IndexMut<R8> for CPU {
    /// Performs the mutable indexing (`container[index]`) operation.
    /// # Panics
    /// Will panic if the index is [`R8::HLmem`], as reading that requires a Bus
    fn index_mut(&mut self, index: R8) -> &mut Self::Output {
        match index {
            R8::A => self.af.hi_mut(),
            R8::B => self.bc.hi_mut(),
            R8::C => self.bc.lo_mut(),
            R8::D => self.de.hi_mut(),
            R8::E => self.de.lo_mut(),
            R8::H => self.hl.hi_mut(),
            R8::L => self.hl.lo_mut(),
            R8::HLmem => unimplemented!("Cannot index CPU with R8::HLmem without Bus"),
        }
    }
}
impl Index<R16> for CPU {
    type Output = Wrapping<u16>;
    /// Performs the indexing (`container[index]`) operation.
    fn index(&self, index: R16) -> &Self::Output {
        match index {
            R16::BC => self.bc.wide(),
            R16::DE => self.de.wide(),
            R16::HL => self.hl.wide(),
            R16::SP => &self.sp,
        }
    }
}
impl IndexMut<R16> for CPU {
    /// Performs the mutable indexing (`container[index]`) operation.
    fn index_mut(&mut self, index: R16) -> &mut Self::Output {
        match index {
            R16::BC => self.bc.wide_mut(),
            R16::DE => self.de.wide_mut(),
            R16::HL => self.hl.wide_mut(),
            R16::SP => &mut self.sp,
        }
    }
}

impl Index<R16Stack> for CPU {
    type Output = Wrapping<u16>;
    fn index(&self, index: R16Stack) -> &Self::Output {
        match index {
            R16Stack::BC => self.bc.wide(),
            R16Stack::DE => self.de.wide(),
            R16Stack::HL => self.hl.wide(),
            R16Stack::AF => self.af.wide(),
        }
    }
}
impl IndexMut<R16Stack> for CPU {
    fn index_mut(&mut self, index: R16Stack) -> &mut Self::Output {
        match index {
            R16Stack::BC => self.bc.wide_mut(),
            R16Stack::DE => self.de.wide_mut(),
            R16Stack::HL => self.hl.wide_mut(),
            R16Stack::AF => self.af.wide_mut(),
        }
    }
}

impl Display for CPU {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        #[rustfmt::skip]
        let Self { ir, sp, pc, af, bc, de, hl, m_cycles, ime, breakpoints } = self;
        writeln!(f, "Current instruction: {ir}\nPC: {pc:04x}\nSP: {sp:04x}")?;
        writeln!(f, "A: {:02x}, F: {}", af.hi(), flag!(af, z, n, h, c))?;
        // writeln!(f, "A: {:02x}, F: {:04b}", af.hi(), af.lo() >> 4)?;
        writeln!(f, "B: {:02x}, C: {:02x}", bc.hi(), bc.lo())?;
        writeln!(f, "D: {:02x}, E: {:02x}", de.hi(), de.lo())?;
        writeln!(f, "H: {:02x}, L: {:02x}", hl.hi(), hl.lo())?;
        write!(
            f,
            "Cycles: {m_cycles}\nInterrupts {}",
            match ime {
                Ime::Disabled => "Disabled",
                Ime::ShouldEnable => "Should be Enabled",
                Ime::Enabled => "Enabled",
            }
        )?;

        if breakpoints.is_empty() {
            Ok(())
        } else {
            write!(f, "\nBreakpoints: {breakpoints:04x?}")
        }
    }
}

macro flag($reg:ident, $($name:ident),+) {
    String::new() $( + if $reg.flags().$name() {
        stringify!($name)
    } else {
        concat!("\x1b[30m", stringify!($name), "\x1b[0m")
    })+
}