#![allow(dead_code)]
#![allow(unused_variables)]

use chip_definitions::IOAdress;
use decoder::{self, IncrementMode, Instruction};
use devices::DeviceTree;
use regs::{GeneralPurposeRegister, GeneralPurposeRegisterPair, StatusFlag};

use std::fmt;

use slog;

#[derive(Debug)]
pub enum CPUError {
    UnimplementedInstruction(decoder::Instruction),
    OutOfBoundsException,
    UnsupportedAddress,
    DecodingError(decoder::DecodingError),
    Breakpoint,
    Exit,
}

// CPU
#[derive(Debug)]
pub struct CPU {
    // General purpose registers
    pub registers: [u8; 32],

    // PC
    pub pc: u32,

    // The same is true for the status register
    pub sreg: u8,

    logger: slog::Logger,
}

impl fmt::Display for CPU {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(
            f,
            "CPU @ 0x{:X} (file offset = 0x{:X}) ",
            self.pc,
            2 * self.pc
        )?;
        write!(f, "SREG: 0x{:02X}: ", self.sreg)?;
        for i in 0..7 {
            let t = self.sreg & (1 << i);
            if t > 0 {
                write!(f, "{}", StatusFlag::try_from(t).unwrap())?;
            } else {
                write!(f, "-")?;
            }
        }
        writeln!(f)?;
        for i in 0..32 {
            write!(f, " R{:-2}={:02X} ", i, self.registers[i])?;
            if (i + 1) % 10 == 0 {
                writeln!(f)?;
            }
        }
        writeln!(f)
    }
}

impl CPU {
    pub fn new(logger: slog::Logger) -> Self {
        CPU {
            registers: [0u8; 32],
            pc: 0,   // Reset vector
            sreg: 0, // Uninitialized as well
            logger,
        }
    }

    pub fn get_sp(&self, mem: &mut DeviceTree) -> u16 {
        let spl: u16 = mem.read(IOAdress::SPL as u32) as u16;
        let sph: u16 = mem.read(IOAdress::SPH as u32) as u16;
        sph << 8 | spl
    }

    fn set_sp(&self, mem: &mut DeviceTree, val: u16) {
        mem.write(IOAdress::SPL as u32, val as u8);
        mem.write(IOAdress::SPH as u32, (val >> 8) as u8);
    }

    fn test_flag(&self, flag: StatusFlag) -> bool {
        (self.sreg & (flag as u8)) > 0
    }

    fn set_flag(&mut self, flag: StatusFlag) {
        self.sreg |= flag as u8;
    }

    fn clear_flag(&mut self, flag: StatusFlag) {
        self.sreg &= !(flag as u8);
    }

    fn set_clear_flag(&mut self, flag: StatusFlag, test: bool) {
        // print!("[{}->{}] ", flag, test);
        if test {
            self.set_flag(flag);
        } else {
            self.clear_flag(flag);
        }
    }

    fn get_register_pair(&self, r: &GeneralPurposeRegisterPair) -> u16 {
        (u16::from(self.registers[r.high()]) << 8) | u16::from(self.registers[r.low()])
    }

    fn set_register_pair(&mut self, r: &GeneralPurposeRegisterPair, v: u16) {
        self.registers[r.high()] = (v >> 8) as u8;
        self.registers[r.low()] = (v & 0xFF) as u8;
    }

    fn get_register(&self, r: &GeneralPurposeRegister) -> u8 {
        self.registers[r.as_usize()]
    }

    fn set_register(&mut self, r: &GeneralPurposeRegister, v: u8) {
        self.registers[r.as_usize()] = v;
    }

    fn ram_write(&self, device_tree: &mut DeviceTree, addr: u16, val: u8) -> Result<(), CPUError> {
        device_tree.write(addr as u32, val);
        Ok(())
    }

    fn ram_read(&self, device_tree: &mut DeviceTree, addr: u16) -> Result<u8, CPUError> {
        Ok(device_tree.read(addr as u32))
    }

    fn push(&mut self, device_tree: &mut DeviceTree, val: u8) -> Result<(), CPUError> {
        let sp = self.get_sp(device_tree);
        self.ram_write(device_tree, sp, val)?;
        self.set_sp(device_tree, sp.wrapping_sub(1));
        Ok(())
    }

    fn pop(&mut self, device_tree: &mut DeviceTree) -> Result<u8, CPUError> {
        let sp = self.get_sp(device_tree);
        self.set_sp(device_tree, sp.wrapping_add(1));
        self.ram_read(device_tree, sp.wrapping_add(1))
    }

    // Flag update functions on a single value:
    fn update_flags_zns_8(&mut self, v: u8) {
        self.set_clear_flag(StatusFlag::Zero, v == 0);
        self.set_clear_flag(StatusFlag::Negative, v & 0x80 == 0x80);
        let V = self.test_flag(StatusFlag::TwosComplementOverflow);
        self.set_clear_flag(StatusFlag::SignBit, (v & 0x80 == 0x80) ^ V);
    }

    fn update_flags_zns_16(&mut self, v: u16) {
        self.set_clear_flag(StatusFlag::Zero, v == 0);
        self.set_clear_flag(StatusFlag::Negative, v & 0x8000 == 0x8000);
        let V = self.test_flag(StatusFlag::TwosComplementOverflow);
        self.set_clear_flag(StatusFlag::SignBit, (v & 0x8000 == 0x8000) ^ V);
    }

    // Copied from simavr, let's trust them for now.
    fn update_flags_add_zns(&mut self, result: u8, d: u8, r: u8) {
        let add_carry = (d & r) | (r & !result) | (!result & d);
        self.set_clear_flag(StatusFlag::HalfCarry, add_carry & 0b1000 != 0);
        self.set_clear_flag(StatusFlag::Carry, add_carry & 0b1000_0000 != 0);
        self.set_clear_flag(
            StatusFlag::TwosComplementOverflow,
            ((d & r & !result) | (!d & !r & result)) & 0x80 == 0x80,
        );
        self.update_flags_zns_8(result);
    }

    fn update_flags_sub_zns(&mut self, result: u8, d: u8, r: u8) {
        let sub_carry = (!d & r) | (r & result) | (result & !d);
        self.set_clear_flag(StatusFlag::HalfCarry, sub_carry & 0b1000 != 0);
        self.set_clear_flag(StatusFlag::Carry, sub_carry & 0b1000_0000 != 0);
        self.set_clear_flag(
            StatusFlag::TwosComplementOverflow,
            ((d & !r & !result) | (!d & r & result)) & 0x80 == 0x80,
        );
        self.update_flags_zns_8(result);
    }

    fn update_flags_Rzns(&mut self, r: u8) {
        if r != 0 {
            self.clear_flag(StatusFlag::Zero);
        }
        self.set_clear_flag(StatusFlag::Negative, r & 0x80 == 0x80);
        let b = self.test_flag(StatusFlag::TwosComplementOverflow);
        self.set_clear_flag(StatusFlag::SignBit, r & 0x80 == 0x80 && b);
    }

    fn update_flags_sub_Rzns(&mut self, result: u8, d: u8, r: u8) {
        let sub_carry = (!d & r) | (r & result) | (result & !d);
        self.set_clear_flag(StatusFlag::HalfCarry, sub_carry & 0b1000 != 0);
        self.set_clear_flag(StatusFlag::Carry, sub_carry & 0b1000_0000 != 0);
        self.set_clear_flag(
            StatusFlag::TwosComplementOverflow,
            ((d & !r & !result) | (!d & r & result)) & 0x80 == 0x80,
        );
        self.update_flags_Rzns(result);
    }

    fn update_flags_zcvs(&mut self, result: u8, vr: u8) {
        // Workaround lexicalic lifetimes.
        let z = result == 0;
        let c = vr & 1 == 1;
        let n = self.test_flag(StatusFlag::Negative);
        self.set_clear_flag(StatusFlag::Zero, z);
        self.set_clear_flag(StatusFlag::Carry, c);
        self.set_clear_flag(StatusFlag::TwosComplementOverflow, n ^ c);
        self.set_clear_flag(StatusFlag::SignBit, c);
    }

    fn update_flags_zcnvs(&mut self, result: u8, vr: u8) {
        let z = result == 0;
        let c = vr & 1 == 1;
        let n = result & 0x80 == 0x80;
        self.set_clear_flag(StatusFlag::Zero, z);
        self.set_clear_flag(StatusFlag::Negative, n);
        self.set_clear_flag(StatusFlag::Carry, c);
        self.set_clear_flag(StatusFlag::TwosComplementOverflow, n ^ c);
        self.set_clear_flag(StatusFlag::SignBit, c);
    }

    fn update_flags_znv0s(&mut self, v: u8) {
        self.clear_flag(StatusFlag::TwosComplementOverflow);
        self.update_flags_zns_8(v);
    }

    // Returns # of ticks the executed instruction took
    pub fn step(
        &mut self,
        rom: &mut [u8],
        device_tree: &mut DeviceTree,
    ) -> Result<usize, CPUError> {
        // Instruction fetch
        if (self.pc as usize) * 2 >= rom.len() {
            return Err(CPUError::OutOfBoundsException);
        }

        let ins = match decoder::decode(&rom[(self.pc as usize) * 2..]) {
            Ok(v) => v,
            Err(e) => return Err(CPUError::DecodingError(e)),
        };
        debug!(
            self.logger,
            "CPU: pc={:06X} sp={:04X} Fetch: {: <40}",
            self.pc,
            self.get_sp(device_tree),
            format!("{}", ins)
        );

        self.pc += (ins.size() / 2) as u32;

        // Instruction execute
        match ins {
            Instruction::JMP(v) => self.pc = v,
            Instruction::CLR(ref r) => {
                self.set_register(r, 0);
                self.update_flags_znv0s(0);
            }
            Instruction::EOR(ref d, ref r) => {
                self.registers[d.as_usize()] ^= self.registers[r.as_usize()];
                let r = self.registers[d.as_usize()];
                self.update_flags_znv0s(r);
            }
            Instruction::LDI(ref r, v) => self.set_register(r, v),
            Instruction::SER(ref r) => self.set_register(r, 0xFF),
            Instruction::RJMP(v) => {
                self.pc = self.pc.wrapping_add(v as _);
                if v == -1 && !self.test_flag(StatusFlag::GlobalInterruptEnable) {
                    info!(self.logger, "HALTED ");
                    return Err(CPUError::Exit);
                }
            }
            Instruction::CLR_FLAG(v) => self.clear_flag(v),
            Instruction::SET_FLAG(v) => self.set_flag(v),
            Instruction::CPI(ref r, v) => {
                let rv = self.get_register(r);
                self.update_flags_sub_zns(rv.wrapping_sub(v), rv, v);
            }
            Instruction::CP(ref r, ref i) => {
                let rv = self.get_register(r);
                let iv = self.get_register(i);
                self.update_flags_sub_zns(rv.wrapping_sub(iv), rv, iv);
            }
            Instruction::CPC(ref d, ref r) => {
                let rd: u8 = self.get_register(d);
                let rr: u8 = self.get_register(r);

                let s = if self.test_flag(StatusFlag::Carry) {
                    rd.wrapping_sub(rr).wrapping_sub(1)
                } else {
                    rd.wrapping_sub(rr)
                };

                self.update_flags_sub_Rzns(s, rd, rr);
            }
            Instruction::BR_IF(offset, flag, test) => {
                if self.test_flag(flag) == test {
                    self.pc = self.pc.wrapping_add(offset as _);
                }
            }
            Instruction::ST(ref ptr, ref src_reg, ref inc_mode) => {
                let base = self.get_register_pair(ptr);
                let addr = match *inc_mode {
                    IncrementMode::None => base,
                    IncrementMode::PreDecrement => {
                        self.set_register_pair(ptr, base.wrapping_sub(1));
                        base.wrapping_sub(1)
                    }
                    IncrementMode::PostIncrement => {
                        self.set_register_pair(ptr, base.wrapping_add(1));
                        base
                    }
                    IncrementMode::ConstantOffset(o) => base.wrapping_add(u16::from(o)),
                };
                self.ram_write(device_tree, addr, self.get_register(src_reg))?;
            }
            Instruction::LD(ref dst_reg, ref ptr, ref inc_mode) => {
                let base = self.get_register_pair(ptr);
                /*
                // TODO: RAMPX/Y/Z
                if ptr.low() == 26 && ram[IOAdress::RAMPX as usize] > 0 {
                    panic!("Unexpected");
                } else if ptr.low() == 28 && ram[IOAdress::RAMPY as usize] > 0 {
                    panic!("Unexpected");
                } else if ptr.low() == 30 && ram[IOAdress::RAMPZ as usize] > 0 {
                    panic!("Unexpected");
                }
                */
                let addr = match *inc_mode {
                    IncrementMode::None => base,
                    IncrementMode::PreDecrement => {
                        self.set_register_pair(ptr, base.wrapping_sub(1));
                        base.wrapping_sub(1)
                    }
                    IncrementMode::PostIncrement => {
                        self.set_register_pair(ptr, base.wrapping_add(1));
                        base
                    }
                    IncrementMode::ConstantOffset(o) => base.wrapping_add(u16::from(o)),
                };
                let v = self.ram_read(device_tree, addr)?;
                self.set_register(dst_reg, v);
            }
            Instruction::ELPM(ref dst_reg, ref inc_mode) => {
                let Zb = self.get_register_pair(&30u8.into());
                // TODO: Only use required bits, other read as zero (according to datasheet)
                let Z = Zb as usize | (device_tree.read(IOAdress::RAMPZ as u32) as usize) << 16;

                match *inc_mode {
                    IncrementMode::PostIncrement => {
                        self.set_register_pair(&30u8.into(), Zb.wrapping_add(1));
                        device_tree.write(IOAdress::RAMPZ as u32, (Z.wrapping_add(1) >> 16) as u8);
                    }
                    _ => {
                        // This instruction does only support None + PostIncrement
                        panic!("Invalid increment mode for ELPM: {:?}", inc_mode);
                    }
                }
                if Z >= rom.len() {
                    warn!(
                        self.logger,
                        "ELPM OOB: RAMPZ={:02X} Z={:04X} len={:06X} ",
                        Z >> 16,
                        Zb,
                        rom.len()
                    );
                    // return Err(CPUError::OutOfBoundsException);
                    return Ok(0xFF); // Hack I kno but emulator does it like that
                }
                let d = rom[Z as usize];
                self.set_register(dst_reg, d);
            }
            Instruction::LPM(ref dst_reg, ref inc_mode) => {
                let Z = self.get_register_pair(&30u8.into());
                let d = rom[Z as usize];
                self.set_register(dst_reg, d);
                match *inc_mode {
                    IncrementMode::PostIncrement => {
                        self.set_register_pair(&30u8.into(), Z.wrapping_add(1));
                    }
                    _ => {
                        // This instruction does only support None + PostIncrement
                        panic!("Invalid increment mode for LPM: {:?}", inc_mode);
                    }
                }
            }
            Instruction::OUT(ref addr, ref val) => {
                let val = self.get_register(val);
                self.ram_write(device_tree, *addr, val)?;
            }
            Instruction::IN(ref reg, ref addr) => {
                let v = self.ram_read(device_tree, *addr)?;
                self.set_register(reg, v);
            }
            Instruction::CALL(ref addr) => {
                let ret_to = self.pc;
                self.push(device_tree, ((ret_to >> 16) & 0xFF) as u8)?;
                self.push(device_tree, ((ret_to >> 8) & 0xFF) as u8)?;
                self.push(device_tree, (ret_to & 0xFF) as u8)?;
                self.pc = *addr;
            }
            Instruction::RCALL(ref addr) => {
                let ret_to = self.pc;
                self.push(device_tree, ((ret_to >> 16) & 0xFF) as u8)?;
                self.push(device_tree, ((ret_to >> 8) & 0xFF) as u8)?;
                self.push(device_tree, (ret_to & 0xFF) as u8)?;
                self.pc = (self.pc as i32 + *addr as i32) as u32;
            }
            Instruction::RET => {
                let mut ret_to = self.pop(device_tree)? as u32;
                ret_to += (self.pop(device_tree)? as u32) << 8;
                ret_to += (self.pop(device_tree)? as u32) << 16;
                self.pc = ret_to as _;
            }
            Instruction::POP(ref reg) => {
                let v = self.pop(device_tree)?;
                self.registers[reg.as_usize()] = v;
            }
            Instruction::PUSH(ref reg) => {
                let v = self.registers[reg.as_usize()];
                self.push(device_tree, v)?;
            }
            Instruction::SUBI(ref d, ref v) => {
                let dv = self.get_register(d);
                let vres = dv.wrapping_sub(*v);

                self.update_flags_sub_zns(vres, dv, *v);
                self.set_register(d, vres);
            }
            Instruction::SBC(ref d, ref r) => {
                let dv = self.get_register(d);
                let rv = self.get_register(r);
                let mut vres = dv.wrapping_sub(rv);
                if self.test_flag(StatusFlag::Carry) {
                    vres = vres.wrapping_sub(1);
                }

                self.update_flags_sub_Rzns(vres, dv, rv);
                self.set_register(d, vres);
            }
            Instruction::SBCI(ref d, ref v) => {
                let dv = self.get_register(d);
                let mut vres = dv.wrapping_sub(*v);
                if self.test_flag(StatusFlag::Carry) {
                    vres = vres.wrapping_sub(1);
                }

                self.update_flags_sub_Rzns(vres, dv, *v);
                self.set_register(d, vres);
            }
            Instruction::MOVW(ref d, ref r) => {
                let t = self.get_register_pair(r);
                self.set_register_pair(d, t);
            }
            Instruction::OR(ref d, ref r) => {
                let t = self.get_register(d) | self.get_register(r);
                self.set_register(d, t);
                self.update_flags_znv0s(t);
            }
            Instruction::ORI(ref d, ref v) => {
                let t = self.get_register(d) | *v;
                self.set_register(d, t);
                self.update_flags_znv0s(t);
            }
            Instruction::AND(ref d, ref r) => {
                let t = self.get_register(d) & self.get_register(r);
                self.set_register(d, t);
                self.update_flags_znv0s(t);
            }
            Instruction::TST(ref r) => {
                let t = self.get_register(r);
                self.update_flags_znv0s(t);
            }
            Instruction::ANDI(ref d, ref v) => {
                let t = self.get_register(d) & *v;
                self.set_clear_flag(StatusFlag::Zero, t == 0);
                self.set_register(d, t);
            }
            Instruction::ADD(ref d, ref r) => {
                let t = self.get_register(d).wrapping_add(self.get_register(r));
                let dd = self.get_register(d);
                self.set_clear_flag(StatusFlag::Carry, t < dd);
                self.set_clear_flag(StatusFlag::Zero, t == 0);
                self.set_register(d, t);
            }
            Instruction::ADC(ref d, ref r) => {
                let mut t = self.get_register(d).wrapping_add(self.get_register(r));
                if self.test_flag(StatusFlag::Carry) {
                    t = t.wrapping_add(1);
                }
                let dd = self.get_register(d);
                self.set_clear_flag(StatusFlag::Carry, t < dd);
                self.set_clear_flag(StatusFlag::Zero, t == 0);
                self.set_register(d, t);
            }
            Instruction::SUB(ref d, ref r) => {
                let dv = self.get_register(d);
                let rv = self.get_register(r);
                let res = dv.wrapping_sub(rv);
                self.update_flags_sub_zns(res, dv, rv);
                self.set_register(d, res);
            }
            Instruction::MOV(ref d, ref r) => {
                let v = self.get_register(r);
                self.set_register(d, v);
            }
            Instruction::ADIW(ref d, ref v) => {
                let dv = self.get_register_pair(d);
                let res = dv.wrapping_add(*v);
                self.set_register_pair(d, res);
                self.set_clear_flag(
                    StatusFlag::TwosComplementOverflow,
                    (dv & !res) & 0x8000 == 0x8000,
                );
                self.set_clear_flag(StatusFlag::Carry, (!dv & res) & 0x8000 == 0x8000);
                self.update_flags_zns_16(res);
            }
            Instruction::SBIW(ref d, ref v) => {
                let dv = self.get_register_pair(d);
                let res = dv.wrapping_sub(*v as _);
                self.set_clear_flag(
                    StatusFlag::TwosComplementOverflow,
                    (!dv & res) & 0x8000 == 0x8000,
                );
                self.set_clear_flag(StatusFlag::Carry, (dv & !res) & 0x8000 == 0x8000);
                self.update_flags_zns_16(res);
                self.set_register_pair(d, res);
            }
            Instruction::DEC(ref r) => {
                let rv = self.get_register(r);
                let res = rv.wrapping_sub(1);
                self.set_register(r, res);
                self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x7F);
                self.update_flags_zns_8(res);
            }
            Instruction::INC(ref r) => {
                let rv = self.get_register(r);
                let res = rv.wrapping_add(1);
                self.set_register(r, res);

                self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x80);
                self.update_flags_zns_8(res);
            }
            Instruction::STS16(ref addr, ref r) => {
                let rampd = device_tree.read(IOAdress::RAMPD as u32);
                if rampd != 0 {
                    panic!("This is unexpected (for now)");
                }
                self.ram_write(device_tree, *addr, self.get_register(r))?;
            }
            Instruction::STS8(ref addr, ref r) => {
                self.ram_write(device_tree, *addr as u16, self.get_register(r))?;
            }
            Instruction::LDS16(ref r, ref addr) => {
                let rampd = device_tree.read(IOAdress::RAMPD as u32);
                if rampd != 0 {
                    panic!("This is unexpected (for now)");
                }
                let v = self.ram_read(device_tree, *addr)?;
                self.set_register(r, v);
            }
            Instruction::LDS8(ref r, ref addr) => {
                let v = self.ram_read(device_tree, *addr as u16)?;
                self.set_register(r, v);
            }
            Instruction::LSL(ref r) => {
                let v = self.get_register(r);
                self.set_clear_flag(StatusFlag::Carry, v & 0x80 == 0x80);
                self.set_register(r, v << 1);
                self.set_clear_flag(StatusFlag::Zero, (v << 1) == 0);
            }
            Instruction::ROL(ref r) => {
                let v = self.get_register(r);
                let c = if self.test_flag(StatusFlag::Carry) {
                    1
                } else {
                    0
                };
                self.set_clear_flag(StatusFlag::Carry, v & 0x80 == 0x80);
                self.set_register(r, v << 1 | c);
                self.set_clear_flag(StatusFlag::Zero, (v << 1) | c == 0);
            }
            Instruction::ASR(ref r) => {
                let rv = self.get_register(r);
                let res = rv >> 1 | (rv & 0x80);
                self.set_register(r, res);
                self.update_flags_zcnvs(res, rv);
            }
            Instruction::LSR(ref r) => {
                let rv = self.get_register(r);
                self.set_register(r, rv >> 1);
                self.clear_flag(StatusFlag::Negative);
                self.update_flags_zcvs(rv >> 1, rv);
            }
            Instruction::ROR(ref r) => {
                let rv = self.get_register(r);
                let c = if self.test_flag(StatusFlag::Carry) {
                    0x80
                } else {
                    0
                };
                let res = rv >> 1 | c;
                self.set_register(r, res);
                self.update_flags_zcnvs(res, rv);
            }
            Instruction::SBRS(ref r, ref bit) => {
                let r = self.get_register(r);
                if (r & (1 << *bit)) > 0 {
                    self.pc += 1;
                }
            }
            Instruction::SBRC(ref r, ref bit) => {
                let r = self.get_register(r);
                if (r & (1 << *bit)) == 0 {
                    self.pc += 1;
                }
            }
            Instruction::CPSE(ref r, ref d) => {
                if self.get_register(r) == self.get_register(d) {
                    // TODO: assume 2b instruction after this one.
                    self.pc += 1;
                }
            }
            Instruction::COM(ref r) => {
                let rv = self.get_register(r);
                let res = 0xFFu8.wrapping_sub(rv);
                self.set_register(r, res);
                self.update_flags_znv0s(res);
                self.set_flag(StatusFlag::Carry);
            }
            Instruction::NEG(ref r) => {
                let rv = self.get_register(r);
                let res = 0u8.wrapping_sub(rv);
                self.set_register(r, res);
                self.set_clear_flag(StatusFlag::HalfCarry, (rv | res) & 0x08 == 0x08);
                self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x80);
                self.set_clear_flag(StatusFlag::Carry, res != 0);
                self.update_flags_zns_8(rv);
            }
            Instruction::MUL(ref r, ref d) => {
                //  R1:R0 ← Rd × Rr(unsigned ← unsigned × unsigned)
                let r = u16::from(self.get_register(r));
                let d = u16::from(self.get_register(d));
                let v = r * d;
                self.registers[0] = (v & 0xFF) as u8;
                self.registers[1] = ((v >> 8) & 0xFF) as u8;
                self.set_clear_flag(StatusFlag::Carry, v & 0x80 == 0x80);
                self.set_clear_flag(StatusFlag::Zero, v == 0);
            }
            Instruction::BST(ref r, ref v) => {
                let r = self.get_register(r);
                self.set_clear_flag(StatusFlag::BitCopyStorage, r & (1 << *v) != 0);
            }
            Instruction::BLD(ref r, ref v) => {
                let mut rv = self.get_register(r);
                if self.test_flag(StatusFlag::BitCopyStorage) {
                    rv |= 1 << *v;
                }
                self.set_register(r, rv);
            }
            Instruction::SWAP(ref r) => {
                let rv = self.get_register(r);
                self.set_register(r, rv >> 4 | ((rv << 4) & 0xF0));
            }
            Instruction::BREAK => {
                return Err(CPUError::Breakpoint);
            }
            Instruction::NOP => {}
            _ => return Err(CPUError::UnimplementedInstruction(ins)),
        }

        Ok(ins.cycles())
    }
}