<commit_before>use memory::{EmptyMemory, Memory, pack_u16, high_byte, low_byte, low_nibble, high_nibble};
use extensions::Incrementor;
use cpu::{Register, Flags, CarryFlag, HalfCarryFlag, ZeroFlag, SubtractFlag};

fn half_carry_for_add(val1: u8, val2: u8, with_carry: bool) -> bool {
    let mut c = 0;
    if with_carry { c = 1; }
    return (low_nibble(val1) + low_nibble(val2) + c) > 0x0F
}

fn carry_for_add(val1: u8, val2: u8, with_carry: bool) -> bool {
    let mut c = 0;
    if with_carry { c = 1; }
    return val1 as u16 + val2 as u16 + c > 0xFF;
}

fn add_internal(first: &mut Register<u8>, second: u8, freg: &mut Register<Flags>, with_carry: bool) {
    let val1 = first.read();
    let val2 = second;
    let mut result = val1 + val2;
    let mut do_carry = false;

    if freg.read().contains(CarryFlag) && with_carry {
        do_carry = true;
        result += 1;
    }

    let mut flags = Flags::empty();
    
    if half_carry_for_add(val1, val2, do_carry) {
        flags = HalfCarryFlag;
    }

    if carry_for_add(val1, val2, do_carry) {
        flags = flags | CarryFlag;
    }

    if result == 0 && flags == Flags::empty() {
        flags = ZeroFlag;
    }

    first.write(result);
    freg.write(flags);
}

/// Add the value of two registers and store it in the first register
pub fn add(first: &mut Register<u8>, second: u8, freg: &mut Register<Flags>) {
    add_internal(first, second, freg, false);
}

/// Load the value from one register into another
pub fn ld_reg_to_reg(target: &mut Register<u8>, source: &Register<u8>) {
    debug!("ld_reg_to_reg: {:X}", source.read());
    let val = source.read();
    target.write(val);
}

/// Loads the memory pointed to by the next two bytes into a register
pub fn ld_u8_immediate(mem: &Memory, pc: &mut Register<u16>, reg: &mut Register<u8>) {
    let val = u8_immediate(mem, pc); 
    debug!("ld_next_byte_to_reg: {:X}", val);
    reg.write(val);
}

/// Loads the next two bytes into the passed in registers
pub fn ld_u16_immediate(mem: &Memory, pc: &mut Register<u16>, 
                                 hb: &mut Register<u8>, lb: &mut Register<u8>) {
    let val = u16_immediate(mem, pc); 
    debug!("ld_next_two_bytes_into_reg_pair: {:X}", val);
    hb.write(high_byte(val));
    lb.write(low_byte(val));
}

/// Writes the passed in value to memory at the address pointed to by combined address parameters
pub fn write_value_to_memory_at_address(mem: &mut Memory, val: u8, addr_msb: u8, addr_lsb: u8) {
   let addr = pack_u16(addr_msb, addr_lsb);
   debug!("write val to mem addr: {:X} val: {:X}", addr, val);
   mem.write_byte(addr as u16, val);
}

/// Increments the pair of registers as if they represent a 16-bit value
pub fn increment_register_pair(msb: &mut Register<u8>,lsb: &mut Register<u8>) {
    debug!("increment register pair");
    let incremented_val = ((msb.read() as uint) << 8) + lsb.read() as uint + 1;
    msb.write(high_byte(incremented_val as u16));
    lsb.write(low_byte(incremented_val as u16));
}

/// Increment register by 1
/// Set ZeroFlag if result is 0
/// Set HalfCarryFlag if there is a carry from bit 3
pub fn increment_register(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = reg.read();
    let mut flags = freg.read();
    flags.remove(SubtractFlag);
    flags.remove(HalfCarryFlag);
    flags.remove(ZeroFlag);

    if low_nibble(val) == 0xF {
        flags.insert(HalfCarryFlag);
    }
    reg.increment();

    if reg.read() == 0 {
        flags.insert(ZeroFlag);
    }
    debug!("increment reg new_val: {:X}", reg.read());
    freg.write(flags);
}

/// Decrement register by 1
/// Set ZeroFlag if result is 0
/// Set SubtractFlag
/// Set HalfCarryFlag if there is no borrow from bit 4
pub fn decrement_register(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = reg.read();
    let mut flags = freg.read() | SubtractFlag;
    flags.remove(ZeroFlag);
    flags.remove(HalfCarryFlag);

    if (val & 0x0F) > 0 {
        flags = flags | HalfCarryFlag;
    }
    
    reg.decrement();

    if reg.read() == 0x00 {
        flags = flags | ZeroFlag;
    }
    freg.write(flags);
}

fn internal_rotate_left_with_carry(val: u8, freg: &mut Register<Flags>) -> u8 {
    freg.write(Flags::empty());

    let bit_7 = (val & 0x80) >> 7;

    if val == 0 {
        freg.write(ZeroFlag);
        return val;
    }

    if val & 0x80 != 0 {
        freg.write(CarryFlag);
    }

    return (val << 1) | bit_7;
}

/// Rotate register left
/// Set ZeroFlag if result is zero
/// Set CarryFlag if bit 7 is 1
pub fn rotate_left_with_carry(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_rotate_left_with_carry(reg.read(), freg);
    reg.write(val);
}

pub fn rotate_left_with_carry_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_rotate_left_with_carry(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

/// Write sp to address with value of next two bytes
pub fn write_u16_immediate_address(mem: &mut Memory, pc: &mut Register<u16>, val: u16){
    let addr = u16_immediate(mem, pc);

    debug!("Writing {} to {}", val, addr);
    mem.write_word(addr, val);
}

/// Performs no operation and consumes a cycle
pub fn nop() {
}

/// Adds two sets of registers as 16 bit numbers with carries counted on bit 11 and 15
pub fn add_register_pair_to_register_pair(rega: &mut Register<u8>, regb: &mut Register<u8>, reg1: u8, reg2: u8, freg: &mut Register<Flags>) {
    let first = pack_u16(rega.read(), regb.read());
    let second = pack_u16(reg1, reg2);

    let sum = first + second;

    // Reset subtract flag, leave ZeroFlag alone
    let mut flags = freg.read();
    flags.remove(SubtractFlag);
    flags.remove(CarryFlag);
    flags.remove(HalfCarryFlag);

    if high_nibble(rega.read()) + high_nibble(reg1) > 15 {
        flags.insert(CarryFlag)
    }

    if low_nibble(rega.read()) + low_nibble(reg1) > 15 {
        flags.insert(HalfCarryFlag);
    }

    rega.write(high_byte(sum));
    regb.write(low_byte(sum));
    freg.write(flags);
}

pub fn ld_from_address(mem: &Memory, reg: &mut Register<u8>, addr: u16) {
    let val = mem.read_byte(addr);
    reg.write(val);
}

pub fn decrement_register_pair(reg1: &mut Register<u8>, reg2: &mut Register<u8>) {
    let val = pack_u16(reg1.read(), reg2.read());
    let ans = val - 1;

    reg1.write(high_byte(ans));
    reg2.write(low_byte(ans));
}

fn internal_rotate_right_with_carry(val: u8, freg: &mut Register<Flags>) -> u8 {    
    let bit_1 = (val & 0x01) << 7;

    if val == 0 {
        freg.write(ZeroFlag);
        return 0;
    } else {
        if val & 0x01 == 1 {
            freg.write(CarryFlag);
        }
        return ((val >> 1) | bit_1);
    }
}

pub fn rotate_right_with_carry(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_rotate_right_with_carry(reg.read(), freg);
    reg.write(val);
}

pub fn rotate_right_with_carry_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_rotate_right_with_carry(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

/// Add n to current address and jump to it - n = one byte signed immediate value
pub fn jump_by_signed_immediate(mem: &Memory, pc: &mut Register<u16>) {
    let offset = u8_immediate(mem, pc); 
    let current_pc = pc.read();
    let mut new_pc = 0;
    if (offset & 0x80) == 0 {
        new_pc = current_pc + offset as u16;
        debug!("jmp signed immediate new_pc: {:X}", new_pc);
    } else {
        new_pc = (current_pc as i16 + (offset as i8) as i16) as u16;
        debug!("jmp signed immediate new_pc: {:X}", new_pc);
    }
    debug!("jmp {:X}", new_pc);
    pc.write(new_pc);
}

pub fn write_value_to_memory_at_address_and_increment_register(mem: &mut Memory, val: u8, high_reg: &mut Register<u8>, low_reg: &mut Register<u8>) {
    let address = pack_u16(high_reg.read(), low_reg.read());
    mem.write_byte(address, val);
    let new_address = address + 1;
    high_reg.write(high_byte(new_address));
    low_reg.write(low_byte(new_address));
}

pub fn write_value_to_memory_at_address_and_decrement_register(mem: &mut Memory, val: u8, high_reg: &mut Register<u8>, low_reg: &mut Register<u8>) {
    let address = pack_u16(high_reg.read(), low_reg.read());
    mem.write_byte(address, val);
    let new_address = address - 1;
    high_reg.write(high_byte(new_address));
    low_reg.write(low_byte(new_address));
}

pub fn relative_jmp_by_signed_immediate_if_true(mem: &Memory, pc: &mut Register<u16>, should_jump: bool) {
    if should_jump {
        jump_by_signed_immediate(mem, pc);
    } else {
        pc.increment();
    }
}

pub fn ld_from_address_pointed_to_by_register_pair_and_increment_register_pair(mem: &Memory, reg: &mut Register<u8>, high_byte: &mut Register<u8>, low_byte: &mut Register<u8>) {
   let address = pack_u16(high_byte.read(), low_byte.read()); 

   let val = mem.read_byte(address);
   reg.write(val);
   increment_register_pair(high_byte, low_byte);
   debug!("ld from address and inc reg pair - addr: {:X} val: {:X} reg_pair_val: {:X}", address, val, pack_u16(high_byte.read(), low_byte.read()));
}

pub fn complement(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = reg.read();
    reg.write(!val);

    let mut flags = freg.read();
    flags.insert(HalfCarryFlag);
    flags.insert(SubtractFlag);
    freg.write(flags);
}

/// Loads the next two bytes into the passed in register (sp)
pub fn ld_next_two_bytes_into_reg(mem: &Memory, pc: &mut Register<u16>, reg: &mut Register<u16>) {
    let val = u16_immediate(mem, pc);
    debug!("ld_next_two_bytes_into_reg: {:X}", val);
    reg.write(val);
}

pub fn increment_value_at_address(mem: &mut Memory, hb: u8, lb: u8, freg: &mut Register<Flags>) {
    let addr = pack_u16(hb, lb);
    let val = mem.read_byte(addr);
    let mut reg = Register::new(val);
    increment_register(&mut reg, freg);
    mem.write_byte(addr, reg.read());
}

pub fn decrement_value_at_address(mem: &mut Memory, hb: u8, lb: u8, freg: &mut Register<Flags>) {
    let addr = pack_u16(hb, lb);
    let val = mem.read_byte(addr);
    let mut reg = Register::new(val);
    decrement_register(&mut reg, freg);
    mem.write_byte(addr, reg.read());
}

pub fn ld_u8_immediate_into_address(mem: &mut Memory, pc: &mut Register<u16>, hb: u8, lb: u8) {
    let addr = pack_u16(hb, lb);
    let val = u8_immediate(mem, pc);
    mem.write_byte(addr, val);
}

pub fn set_flag(freg: &mut Register<Flags>, flag: Flags) {
    let mut flags = freg.read();
    flags.insert(flag);
    freg.write(flags);
}

pub fn ld_from_address_pointed_to_by_register_pair_and_decrement_register_pair(mem: &Memory, reg: &mut Register<u8>, high_byte: &mut Register<u8>, low_byte: &mut Register<u8>) {
   let address = pack_u16(high_byte.read(), low_byte.read()); 

   let val = mem.read_byte(address);

   reg.write(val);
   decrement_register_pair(high_byte, low_byte);
}

pub fn ccf(freg: &mut Register<Flags>) {
    let mut f = freg.read();
    f.remove(SubtractFlag);
    f.remove(HalfCarryFlag);
    f.toggle(CarryFlag);
    freg.write(f);
}

pub fn ld_u8(reg: &mut Register<u8>, val: u8) {
    debug!("load val: {:X} into register", val);
    reg.write(val);
}

pub fn add_value_at_address(mem: &Memory, reg: &mut Register<u8>, hb: u8, lb: u8, freg: &mut Register<Flags>) {
   let val = mem.read_byte(pack_u16(hb, lb));
   add(reg, val, freg);
}

pub fn adc(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    add_internal(reg, val, freg, true);
}

pub fn adc_value_at_address(mem: &Memory, reg: &mut Register<u8>, address: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(address);
    adc(reg, val, freg);
}

fn half_carry_for_subtract(val1: u8, val2: u8, carry: u8) -> bool {
    if low_nibble(val2) + carry == 0x10 && val1 >= 0x10 {
        return true;
    } else {
        return low_nibble(val1) >= (low_nibble(val2) + carry);
    }

}

fn carry_for_subtract(val1: u8, val2: u8, carry: u8) -> bool {
    return val1 >= (val2 + carry);
}

pub fn internal_sub(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>, with_carry: bool) {
    let reg_val = reg.read();
    let mut flags = SubtractFlag;
    let mut carry = 0;
    
    if freg.read().contains(CarryFlag) && with_carry {
        carry = 1;
    }

    if half_carry_for_subtract(reg_val, val, carry) {
        flags.insert(HalfCarryFlag);
    }

    if carry_for_subtract(reg_val, val, carry) {
        flags.insert(CarryFlag);
    }

    let result = reg_val - val - carry;
    
    if result == 0 && flags.contains(CarryFlag) {
        flags.insert(ZeroFlag);
    }
    debug!("internal sub result {:X}", result);
    reg.write(result);
    freg.write(flags);
}


pub fn sub(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    internal_sub(reg, val, freg, false);
}

pub fn sbc(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    internal_sub(reg, val, freg, true);
}

pub fn sub_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    internal_sub(reg, val, freg, false);
}

pub fn sbc_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    internal_sub(reg, val, freg, true);
}

pub fn and(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    let mut flags = HalfCarryFlag;

    let result = reg.read() & val;

    if result == 0 {
        flags.insert(ZeroFlag);
    }
    
    reg.write(result);
    freg.write(flags);
}

pub fn xor(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    let mut flags = Flags::empty();

    let result = reg.read() ^ val;

    if result == 0 {
        flags.insert(ZeroFlag);
    }
    
    reg.write(result);
    freg.write(flags);
}

pub fn and_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    and(reg, val, freg);
}

pub fn xor_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    xor(reg, val, freg);
}

pub fn or(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    let mut flags = Flags::empty();

    let result = reg.read() | val;

    if result == 0 {
        flags.insert(ZeroFlag);
    }
    
    reg.write(result);
    freg.write(flags);
}

pub fn compare(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    let reg_val = reg.read();
    let mut flags = SubtractFlag;
    let mut carry = 0;
    
    if half_carry_for_subtract(reg_val, val, carry) {
        flags.insert(HalfCarryFlag);
    }

    if carry_for_subtract(reg_val, val, carry) {
        flags.insert(CarryFlag);
    }

    if (reg_val == val) && flags.contains(CarryFlag) {
        flags.insert(ZeroFlag);
    }
    freg.write(flags);
}

pub fn or_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    or(reg, val, freg);
}

pub fn compare_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    compare(reg, val, freg);
}

pub fn push(mem: &mut Memory, sp: &mut Register<u16>, val: u16) {
    sp.decrement();
    sp.decrement();
    debug!("push sp:{:X} val:{:X}", sp.read(), val);
    mem.write_word(sp.read(), val);
}

fn pop_internal(mem: &Memory, sp: &mut Register<u16>) -> u16 {
    let val = mem.read_word(sp.read());
    sp.increment();
    sp.increment();
    return val;
}

pub fn pop(mem: &Memory, sp: &mut Register<u16>, hb: &mut Register<u8>, lb: &mut Register<u8>) {
    let val = pop_internal(mem, sp);
    debug!("pop val: {:X}", val);
    hb.write(high_byte(val));
    lb.write(low_byte(val));
}

pub fn ret(mem: &Memory, pc: &mut Register<u16>, sp: &mut Register<u16>, should_execute: bool) {
    if should_execute {
        let addr = pop_internal(mem, sp); 
        debug!("ret to {:X}", addr);
        pc.write(addr);
    }
}

pub fn jp_u16_immediate(mem: &Memory, pc: &mut Register<u16>) {
    let addr = u16_immediate(mem, pc); 
    debug!("jmp {:X}", addr);
    pc.write(addr);
}

pub fn jp_u16_immediate_if_true(mem: &Memory,pc: &mut Register<u16>,should_jump: bool) {
    if should_jump {
        jp_u16_immediate(mem, pc);
    } else {
        pc.increment();
        pc.increment();
    }
}

pub fn call_immediate_if_true(mem: &mut Memory, pc: &mut Register<u16>, sp: &mut Register<u16>, should_jump: bool) {
    if should_jump {
        let new_addr = u16_immediate(mem, pc); 
        push(mem, sp, pc.read());
        debug!("call immediate {:X}", new_addr);
        pc.write(new_addr);
    } else {
        pc.increment();
        pc.increment();
    }
}

pub fn add_u8_immediate(mem: &Memory, pc: &mut Register<u16>, reg: &mut Register<u8>, freg: &mut Register<Flags>, with_carry: bool) {
    let val = u8_immediate(mem, pc); 
    add_internal(reg, val, freg, with_carry);
}

pub fn call(mem: &mut Memory, pc: &mut Register<u16>, sp: &mut Register<u16>, addr: u16) {
    push(mem, sp, pc.read());
    pc.write(addr);
}

pub fn sub_u8_immediate(mem: &Memory, pc: &mut Register<u16>, reg: &mut Register<u8>, freg: &mut Register<Flags>, with_carry: bool) {
    let val = u8_immediate(mem, pc);
    debug!("sub u8 immediate {:X}", val);
    internal_sub(reg, val, freg, with_carry);
}

pub fn reti(mem: &Memory, pc: &mut Register<u16>, sp: &mut Register<u16>, ime: &mut bool) {
    ret(mem, pc, sp, true);
    *ime = true;
}

pub fn u8_immediate(mem: &Memory, pc: &mut Register<u16>) -> u8 {
    let val = mem.read_byte(pc.read());
    pc.increment();
    return val;
}

pub fn u16_immediate(mem: &Memory, pc: &mut Register<u16>) -> u16 {
    let val = mem.read_word(pc.read());
    pc.increment();
    pc.increment();
    return val;
}

pub fn jp(pc: &mut Register<u16>, addr: u16) {
    pc.write(addr);
}

pub fn bit(reg_val: u8, pos: u8, freg: &mut Register<Flags>) {
    let mut flags = freg.read();
    flags.remove(SubtractFlag);
    flags.insert(HalfCarryFlag);
    
    assert!(pos < 8, "Bit positions are 0-7");

    let mask = 0x01 << pos as uint;
    let val = reg_val & mask;

    if val == 0 {
        flags.insert(ZeroFlag);
    } else {
        flags.remove(ZeroFlag);
    }
    freg.write(flags);
}

pub fn byte_at_address(mem: &Memory, addr: u16) -> u8 {
    return mem.read_byte(addr);
}

pub fn res(reg: &mut Register<u8>, pos: u8) {
    let mask = 0x01 << pos as uint;
    let val = reg.read();
    let reset_val = val & (!mask);
    reg.write(reset_val);
}

pub fn res_at_addr(mem: &mut Memory, address: u16, pos: u8) {
    let mask = 0x01 << pos as uint;
    let val = mem.read_byte(address);
    let reset_val = val & (!mask);
    mem.write_byte(address, reset_val);
}

pub fn set(reg: &mut Register<u8>, pos: u8) {
    let mask = 0x01 << pos as uint;
    let val = reg.read();
    let reset_val = val | mask;
    reg.write(reset_val);
}

pub fn set_at_addr(mem: &mut Memory, address: u16, pos: u8) {
    let mask = 0x01 << pos as uint;
    let val = mem.read_byte(address);
    let reset_val = val | mask;
    mem.write_byte(address, reset_val);
}

pub fn rotate_left(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_rotate_left(reg.read(), freg);
    reg.write(val);
}

pub fn rotate_left_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_rotate_left(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

fn internal_rotate_left(val: u8, freg: &mut Register<Flags>) -> u8 {
    let mut carry = 0;
    if freg.read().contains(CarryFlag) {
        carry = 1;
    }

    freg.write(Flags::empty());
    if val == 0 {
        freg.write(ZeroFlag);
        return val;
    }

    if val & 0x80 != 0 {
        freg.write(CarryFlag);
    }

    return (val << 1) | carry;
}

fn internal_rotate_right(val: u8, freg: &mut Register<Flags>) -> u8 {
    let mut carry = 0;
    if freg.read().contains(CarryFlag) {
        carry = 0x80;
    }

    freg.write(Flags::empty());
    if val == 0 {
        freg.write(ZeroFlag);
        return val;
    }

    if val & 0x01 != 0 {
        freg.write(CarryFlag);
    }

    return (val >> 1) | carry;
}

pub fn rotate_right(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_rotate_right(reg.read(), freg);
    reg.write(val);
}

pub fn rotate_right_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_rotate_right(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

fn internal_sla(val: u8, freg: &mut Register<Flags>) -> u8 {
    freg.write(Flags::empty());
    if val == 0 {
        freg.write(ZeroFlag);
        return 0;
    }

    if val & 0x80 != 0 {
        freg.write(CarryFlag);
    }

    return val << 1;
}

pub fn sla(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_sla(reg.read(), freg);
    reg.write(val);
}

pub fn sla_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_sla(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

fn internal_sra(val: u8, freg: &mut Register<Flags>) -> u8 {
    freg.write(Flags::empty());
    if val == 0 {
        freg.write(ZeroFlag);
        return 0;
    }

    if val & 0x01 == 1 {
        freg.write(CarryFlag);
    }

    let bit_7 = val & 0x80;
    return (val >> 1) | bit_7;
}

pub fn sra(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_sra(reg.read(), freg);
    reg.write(val);
}

pub fn sra_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_sra(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

fn internal_swap(val: u8, freg: &mut Register<Flags>) -> u8 {
    freg.write(Flags::empty());

    if val == 0 {
        freg.write(ZeroFlag);
    }

    return (low_nibble(val) << 4) + high_nibble(val);
}

pub fn swap(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_swap(reg.read(), freg);
    reg.write(val);
}

pub fn swap_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_swap(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

fn internal_srl(val: u8, freg: &mut Register<Flags>) -> u8 {
    freg.write(Flags::empty());

    if val == 0 {
        freg.write(ZeroFlag);
    }

    if val & 0x01 == 1 {
        freg.write(CarryFlag);
    }

    return val >> 1;
}

pub fn srl(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_sra(reg.read(), freg);
    reg.write(val);
}

pub fn srl_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_sra(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

pub fn disable_interrupts(ime: &mut bool) {
    debug!("disable interrupts");
    *ime = false;
}

pub fn write_value_to_u16_immediate(mem: &mut Memory, pc: &mut Register<u16>, val: u8) {
    let addr = u16_immediate(mem, pc);
    debug!("write value to u16 immediate val: {:X} addr: {:X}", val, addr);
    mem.write_byte(addr, val);
}

pub fn write_val_FF00_plus_immediate(mem: &mut Memory, pc: &mut Register<u16>, val: u8) {
    let lb = u8_immediate(mem, pc);
    let addr = 0xFF00 + lb as u16;
    debug!("write val: {:X} to addr: {:X}", val, addr);
    mem.write_byte(addr, val); 
}

pub fn pop_flags(mem: &Memory, sp: &mut Register<u16>, a: &mut Register<u8>, f: &mut Register<Flags>) {
    let val = pop_internal(mem, sp);
    debug!("POP AF val: {:X}", val);
    a.write(high_byte(val));
    f.write(Flags::from_bits_truncate(low_byte(val)));
}

pub fn load_val_FF00_plus_immediate(mem: &Memory, pc: &mut Register<u16>, reg: &mut Register<u8>) {
    let lb = u8_immediate(mem, pc);
    let addr = 0xFF00 + lb as u16;
    let val = mem.read_byte(addr); 
    debug!("load val: {:X} into a", val);
    reg.write(val);
}

#[test]
fn test_load_val_FF00_plus_immediate() {
}

#[test]
fn test_pop_flags() {
    let mut a = Register::new(0x00);
    let mut f = Register::new(HalfCarryFlag);
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut sp = Register::new(0xFFFC);
    mem.write_word(sp.read(), 0xBCDF);

    pop_flags(&mem, &mut sp, &mut a, &mut f);

    assert!(sp.read() == 0xFFFE);
    assert!(a.read() == 0xBC);
    assert!(f.read() == ZeroFlag | SubtractFlag | CarryFlag);
}

#[test]
fn test_write_val_FF00_plus_immediate() {
}

#[test]
fn test_write_value_to_u16_immediate() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x8284;
    let val = 0x92;
    let mut pc = Register::new(0x1234);

    mem.write_word(0x1234, addr);

    write_value_to_u16_immediate(&mut mem, &mut pc, val);
    assert!(pc.read() == 0x1236);
    assert!(mem.read_byte(addr) == val);
}

#[test]
fn test_disable_interrupts() {
    let mut a = true;
    
    disable_interrupts(&mut a);

    assert!(a == false);
}

#[test]
fn test_srl_at_address() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut freg = Register::new(HalfCarryFlag);
    let mut val = 0x00;
    let addr = 0x3727;
    mem.write_byte(addr, val);
    srl_at_address(&mut mem, addr, &mut freg);

    assert!(freg.read() == ZeroFlag);
    assert!(mem.read_byte(addr) == 0x00);

    val = 0b00110011;
    mem.write_byte(addr, val);
    srl_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0b00011001);
    assert!(freg.read() == CarryFlag);

    srl_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0b00001100);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_srl() {
    let mut reg = Register::new(0x00);
    let mut freg = Register::new(HalfCarryFlag);
    srl(&mut reg, &mut freg);

    assert!(freg.read() == ZeroFlag);
    assert!(reg.read() == 0x00);

    reg.write(0b00110011);
    srl(&mut reg, &mut freg);
    assert!(reg.read() == 0b00011001);
    assert!(freg.read() == CarryFlag);

    srl(&mut reg, &mut freg);
    assert!(reg.read() == 0b00001100);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_swap_at_address() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x2321;
    let mut freg = Register::new(Flags::empty());
    mem.write_byte(addr, 0xCB);
    swap_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0xBC);
}

#[test]
fn test_swap() {
    let mut reg = Register::new(0x00);
    let mut freg = Register::new(Flags::empty());

    swap(&mut reg, &mut freg);
    assert!(reg.read() == 0x00);
    assert!(freg.read() == ZeroFlag);

    reg.write(0xAC);
    swap(&mut reg, &mut freg);
    assert!(reg.read() == 0xCA);
    assert!(freg.read() == Flags::empty());
}

#[test]
fn test_sra_at_address() {
    let mut val = 0b10010001;
    let mut freg = Register::new(Flags::empty());
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x2372;

    mem.write_byte(addr, val);
    sra_at_address(&mut mem, addr, &mut freg);

    assert!(mem.read_byte(addr) == 0b11001000);
    assert!(freg.read() == CarryFlag);
    
    sra_at_address(&mut mem, addr, &mut freg);

    assert!(mem.read_byte(addr) == 0b11100100);
    assert!(freg.read() == Flags::empty());

    mem.write_byte(addr, 0x00);
    sra_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0x00);
    assert!(freg.read() == ZeroFlag);

    mem.write_byte(addr, 0b00001111);
    sra_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0b00000111);
    assert!(freg.read() == CarryFlag);

}

#[test]
fn test_sra() {
    let mut reg = Register::new(0b10010001);
    let mut freg = Register::new(Flags::empty());

    sra(&mut reg, &mut freg);

    assert!(reg.read() == 0b11001000);
    assert!(freg.read() == CarryFlag);
    
    sra(&mut reg, &mut freg);

    assert!(reg.read() == 0b11100100);
    assert!(freg.read() == Flags::empty());

    reg.write(0x00);
    sra(&mut reg, &mut freg);
    assert!(reg.read() == 0x00);
    assert!(freg.read() == ZeroFlag);

    reg.write(0b00001111);
    sra(&mut reg, &mut freg);
    assert!(reg.read() == 0b00000111);
    assert!(freg.read() == CarryFlag);

}

#[test]
fn test_sla_at_address() {
    let mut val = 0b00001111;
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1423;
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    mem.write_byte(addr, val);
    sla_at_address(&mut mem, addr, &mut freg);
    
    // Rotate should happen
    assert!(mem.read_byte(addr) == 0b00011110);
    assert!(freg.read() == Flags::empty());
    
    mem.write_byte(addr, 0x00);

    sla_at_address(&mut mem, addr, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(mem.read_byte(addr) == 0x00);
    assert!(freg.read() == ZeroFlag);
    
    mem.write_byte(addr, 0b11001100);

    sla_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b10011000);
    assert!(freg.read() == CarryFlag);
    
    freg.write(CarryFlag);
    mem.write_byte(addr, 0b11001100);

    sla_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b10011000);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_sla() {
    let mut reg = Register::new(0b00001111);
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    sla(&mut reg, &mut freg);
    
    // Rotate should happen
    assert!(reg.read() == 0b00011110);
    assert!(freg.read() == Flags::empty());
    
    let mut regb = Register::new(0x00);

    sla(&mut regb, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(regb.read() == 0x00);
    assert!(freg.read() == ZeroFlag);

    let mut regc = Register::new(0b11001100);
    freg.write(CarryFlag);
    sla(&mut regc, &mut freg);

    assert!(regc.read() == 0b10011000);
    assert!(freg.read() == CarryFlag);

    regc.write(0b00110011);
    sla(&mut regc, &mut freg);

    assert!(regc.read() == 0b01100110);
    assert!(freg.read() == Flags::empty());
}

#[test]
fn test_rotate_right_at_address() {
    let mut val = 0b00001111;
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1423;
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    mem.write_byte(addr, val);
    rotate_right_at_address(&mut mem, addr, &mut freg);
    
    // Rotate should happen
    assert!(mem.read_byte(addr) == 0b00000111);
    assert!(freg.read() == CarryFlag);
    
    mem.write_byte(addr, 0x00);
    freg.write(Flags::empty());

    rotate_right_at_address(&mut mem, addr, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(mem.read_byte(addr) == 0x00);
    assert!(freg.read() == ZeroFlag);
    
    mem.write_byte(addr, 0b11001101);

    rotate_right_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b01100110);
    assert!(freg.read() == CarryFlag);
    
    freg.write(CarryFlag);
    mem.write_byte(addr, 0b11001100);

    rotate_right_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b11100110);
    assert!(freg.read() == Flags::empty());
}

#[test]
fn test_rotate_right() {
    let mut reg = Register::new(0b10011001);
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    rotate_right(&mut reg, &mut freg);

    assert!(reg.read() == 0b01001100);
    assert!(freg.read() == CarryFlag);

    reg.write(0x0);
    freg.write(Flags::empty());
    rotate_right(&mut reg, &mut freg);

    assert!(reg.read() == 0x0);
    assert!(freg.read() == ZeroFlag);

    reg.write(0x01);
    freg.write(CarryFlag);
    rotate_right(&mut reg, &mut freg);
    assert!(reg.read() == 0x80);
    assert!(freg.read() == CarryFlag);

    reg.write(0x01);
    freg.write(Flags::empty());
    rotate_right(&mut reg, &mut freg);
    assert!(reg.read() == 0x00);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_rotate_left_at_address() {
    let mut val = 0b00001111;
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1423;
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    mem.write_byte(addr, val);
    rotate_left_at_address(&mut mem, addr, &mut freg);
    
    // Rotate should happen
    assert!(mem.read_byte(addr) == 0b00011110);
    assert!(freg.read() == Flags::empty());
    
    mem.write_byte(addr, 0x00);

    rotate_left_at_address(&mut mem, addr, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(mem.read_byte(addr) == 0x00);
    assert!(freg.read() == ZeroFlag);
    
    mem.write_byte(addr, 0b11001100);

    rotate_left_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b10011000);
    assert!(freg.read() == CarryFlag);
    
    freg.write(CarryFlag);
    mem.write_byte(addr, 0b11001100);

    rotate_left_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b10011001);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_rotate_left() {
    let mut reg = Register::new(0b00001111);
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    rotate_left(&mut reg, &mut freg);
    
    // Rotate should happen
    assert!(reg.read() == 0b00011110);
    assert!(freg.read() == Flags::empty());
    
    let mut regb = Register::new(0x00);

    rotate_left(&mut regb, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(regb.read() == 0x00);
    assert!(freg.read() == ZeroFlag);

    let mut regc = Register::new(0b11001100);
    freg.write(CarryFlag);
    rotate_left(&mut regc, &mut freg);

    assert!(regc.read() == 0b10011001);
    assert!(freg.read() == CarryFlag);

    regc.write(0b00110011);
    freg.write(Flags::empty());
    rotate_left(&mut regc, &mut freg);

    assert!(regc.read() == 0b01100110);
    assert!(freg.read() == Flags::empty());


}

#[test]
fn test_rotate_right_with_carry_at_address() {
    let mut val = 0b10011001;
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1237;
    mem.write_byte(addr, val);
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    rotate_right_with_carry_at_address(&mut mem, addr, &mut freg);

    assert!(mem.read_byte(addr) == 0b11001100);
    assert!(freg.read() == CarryFlag);

    mem.write_byte(addr, 0x0);
    rotate_right_with_carry_at_address(&mut mem, addr, &mut freg);

    assert!(mem.read_byte(addr) == 0x0);
    assert!(freg.read() == ZeroFlag);

    mem.write_byte(addr, 0x01);
    freg.write(CarryFlag);
    rotate_right_with_carry_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0x80);
    assert!(freg.read() == CarryFlag);
    
    mem.write_byte(addr, 0x01);
    rotate_right_with_carry_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0x80);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_rotate_left_with_carry_at_address() {
    let mut val = 0b00001111;
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1423;
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    mem.write_byte(addr, val);
    rotate_left_with_carry_at_address(&mut mem, addr, &mut freg);
    
    // Rotate should happen
    assert!(mem.read_byte(addr) == 0b00011110);
    assert!(freg.read() == Flags::empty());
    
    mem.write_byte(addr, 0x00);

    rotate_left_with_carry_at_address(&mut mem, addr, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(mem.read_byte(addr) == 0x00);
    assert!(freg.read() == ZeroFlag);

    mem.write_byte(addr, 0b11001100);

    rotate_left_with_carry_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b10011001);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_set_at_addr() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1235;

    mem.write_byte(addr, 0b11001100); 

    set_at_addr(&mut mem, addr, 0);
    assert!(mem.read_byte(addr) == 0b11001101);
}

#[test]
fn test_set() {
    let mut reg = Register::new(0b00110011);

    set(&mut reg, 7);
    assert!(reg.read() == 0b10110011);

    set(&mut reg, 1);
    assert!(reg.read() == 0b10110011);

    set(&mut reg, 4);
    assert!(reg.read() == 0b10110011);

}

#[test]
fn test_res_at_addr() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1235;

    mem.write_byte(addr, 0b11001100); 

    res_at_addr(&mut mem, addr, 2);
    assert!(mem.read_byte(addr) == 0b11001000);
    
}

#[test]
fn test_res() {
    let mut reg = Register::new(0b00110011);

    res(&mut reg, 0);
    assert!(reg.read() == 0b00110010);

    res(&mut reg, 5);
    assert!(reg.read() == 0b00010010);

    res(&mut reg, 7);
    assert!(reg.read() == 0b00010010);
}

#[test]
fn test_byte_at_address() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1234;

    mem.write_byte(addr, 0xFE);

    let val = byte_at_address(&mem, addr);
    assert!(val == 0xFE);
}

#[test]
fn test_bit() {
    let reg = 0b0001111;
    let mut freg = Register::new(SubtractFlag | CarryFlag);

    bit(reg, 3, &mut freg);

    assert!(freg.read() == HalfCarryFlag | CarryFlag);

    bit(reg, 7, &mut freg);

    assert!(freg.read() == HalfCarryFlag | CarryFlag | ZeroFlag);
}

#[test]
fn test_jp() {
    let mut pc = Register::new(0x2311);
    let addr = 0x1223;

    jp(&mut pc, addr);

    assert!(pc.read() == addr);
}

#[test]
fn test_u16_immediate() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x123);
    mem.write_word(pc.read(), 0xBEEF);

    let val = u16_immediate(&mem, &mut pc);

    assert!(val == 0xBEEF);
    assert!(pc.read() == 0x125);

}

#[test]
fn test_u8_immediate() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x123);
    mem.write_byte(pc.read(), 0x43);

    let val = u8_immediate(&mem, &mut pc);

    assert!(val == 0x43);
    assert!(pc.read() == 0x124);
}

#[test]
fn test_reti() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut ime = false;
    let mut pc = Register::new(0x123);
    let mut sp = Register::new(0xFFFC);
    mem.write_word(sp.read(), 0xADCD);

    reti(&mem, &mut pc, &mut sp, &mut ime);
    assert!(pc.read() == 0xADCD);
    assert!(ime == true);
    assert!(sp.read() == 0xFFFE);
}

#[test]
fn test_sub_u8_immediate() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x2736);
    let mut reg = Register::new(0x35);
    let mut freg = Register::new(Flags::empty());
    mem.write_byte(pc.read(), 0x11);

    sub_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, false);

    assert!(pc.read() == 0x2737);
    assert!(reg.read() == 0x24);
    assert!(freg.read() == SubtractFlag | HalfCarryFlag | CarryFlag);

    mem.write_byte(pc.read(), 0x0C);
    sub_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, false);

    assert!(pc.read() == 0x2738);
    assert!(reg.read() == 0x18);
    assert!(freg.read() == SubtractFlag | CarryFlag);

    mem.write_byte(pc.read(), 0x0F);
    freg.write(CarryFlag);
    sub_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, true);

    assert!(pc.read() == 0x2739);
    assert!(reg.read() == 0x08);
    assert!(freg.read() == SubtractFlag | CarryFlag | HalfCarryFlag);

}

#[test]
fn test_call() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x4324);
    let mut sp = Register::new(0xFFFE);
    let addr = 0x10;

    call(&mut mem, &mut pc, &mut sp, addr);

    assert!(pc.read() == 0x0010);
    assert!(sp.read() == 0xFFFC);
    assert!(mem.read_word(sp.read()) == 0x4324);
}

#[test]
fn test_add_u8_immediate() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x2736);
    let mut reg = Register::new(0x05);
    let mut freg = Register::new(Flags::empty());
    mem.write_byte(pc.read(), 0x11);

    add_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, false);

    assert!(pc.read() == 0x2737);
    assert!(reg.read() == 0x16);
    assert!(freg.read() == Flags::empty());

    mem.write_byte(pc.read(), 0x0C);
    add_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, false);

    assert!(pc.read() == 0x2738);
    assert!(reg.read() == 0x22);
    assert!(freg.read() == HalfCarryFlag);

    mem.write_byte(pc.read(), 0x0F);
    freg.write(CarryFlag);
    add_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, true);

    assert!(pc.read() == 0x2739);
    assert!(reg.read() == 0x32);
    assert!(freg.read() == HalfCarryFlag);

}

#[test]
fn test_push() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut sp = Register::new(0xFFAB);
     
    push(&mut mem, &mut sp, 0x8735);
    assert!(sp.read() == 0xFFA9);
    assert!(mem.read_word(sp.read()) == 0x8735);
}

#[test]
fn test_call_immediate_if_true() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x6542);<commit_msg>Change Add's ZeroFlag behavior to mimic increment. When the register rolls over to 0 then ZeroFlag should be set.<commit_after>use memory::{EmptyMemory, Memory, pack_u16, high_byte, low_byte, low_nibble, high_nibble};
use extensions::Incrementor;
use cpu::{Register, Flags, CarryFlag, HalfCarryFlag, ZeroFlag, SubtractFlag};

fn half_carry_for_add(val1: u8, val2: u8, with_carry: bool) -> bool {
    let mut c = 0;
    if with_carry { c = 1; }
    return (low_nibble(val1) + low_nibble(val2) + c) > 0x0F
}

fn carry_for_add(val1: u8, val2: u8, with_carry: bool) -> bool {
    let mut c = 0;
    if with_carry { c = 1; }
    return val1 as u16 + val2 as u16 + c > 0xFF;
}

fn add_internal(first: &mut Register<u8>, second: u8, freg: &mut Register<Flags>, with_carry: bool) {
    let val1 = first.read();
    let val2 = second;
    let mut result = val1 + val2;
    let mut do_carry = false;

    if freg.read().contains(CarryFlag) && with_carry {
        do_carry = true;
        result += 1;
    }

    let mut flags = Flags::empty();
    
    if half_carry_for_add(val1, val2, do_carry) {
        flags = HalfCarryFlag;
    }

    if carry_for_add(val1, val2, do_carry) {
        flags = flags | CarryFlag;
    }

    if result == 0 {
        flags = ZeroFlag;
    }

    first.write(result);
    freg.write(flags);
}

/// Add the value of two registers and store it in the first register
pub fn add(first: &mut Register<u8>, second: u8, freg: &mut Register<Flags>) {
    add_internal(first, second, freg, false);
}

/// Load the value from one register into another
pub fn ld_reg_to_reg(target: &mut Register<u8>, source: &Register<u8>) {
    debug!("ld_reg_to_reg: {:X}", source.read());
    let val = source.read();
    target.write(val);
}

/// Loads the memory pointed to by the next two bytes into a register
pub fn ld_u8_immediate(mem: &Memory, pc: &mut Register<u16>, reg: &mut Register<u8>) {
    let val = u8_immediate(mem, pc); 
    debug!("ld_next_byte_to_reg: {:X}", val);
    reg.write(val);
}

/// Loads the next two bytes into the passed in registers
pub fn ld_u16_immediate(mem: &Memory, pc: &mut Register<u16>, 
                                 hb: &mut Register<u8>, lb: &mut Register<u8>) {
    let val = u16_immediate(mem, pc); 
    debug!("ld_next_two_bytes_into_reg_pair: {:X}", val);
    hb.write(high_byte(val));
    lb.write(low_byte(val));
}

/// Writes the passed in value to memory at the address pointed to by combined address parameters
pub fn write_value_to_memory_at_address(mem: &mut Memory, val: u8, addr_msb: u8, addr_lsb: u8) {
   let addr = pack_u16(addr_msb, addr_lsb);
   debug!("write val to mem addr: {:X} val: {:X}", addr, val);
   mem.write_byte(addr as u16, val);
}

/// Increments the pair of registers as if they represent a 16-bit value
pub fn increment_register_pair(msb: &mut Register<u8>,lsb: &mut Register<u8>) {
    debug!("increment register pair");
    let incremented_val = ((msb.read() as uint) << 8) + lsb.read() as uint + 1;
    msb.write(high_byte(incremented_val as u16));
    lsb.write(low_byte(incremented_val as u16));
}

/// Increment register by 1
/// Set ZeroFlag if result is 0
/// Set HalfCarryFlag if there is a carry from bit 3
pub fn increment_register(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = reg.read();
    let mut flags = freg.read();
    flags.remove(SubtractFlag);
    flags.remove(HalfCarryFlag);
    flags.remove(ZeroFlag);

    if low_nibble(val) == 0xF {
        flags.insert(HalfCarryFlag);
    }
    reg.increment();

    if reg.read() == 0 {
        flags.insert(ZeroFlag);
    }
    debug!("increment reg new_val: {:X}", reg.read());
    freg.write(flags);
}

/// Decrement register by 1
/// Set ZeroFlag if result is 0
/// Set SubtractFlag
/// Set HalfCarryFlag if there is no borrow from bit 4
pub fn decrement_register(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = reg.read();
    let mut flags = freg.read() | SubtractFlag;
    flags.remove(ZeroFlag);
    flags.remove(HalfCarryFlag);

    if (val & 0x0F) > 0 {
        flags = flags | HalfCarryFlag;
    }
    
    reg.decrement();

    if reg.read() == 0x00 {
        flags = flags | ZeroFlag;
    }
    freg.write(flags);
}

fn internal_rotate_left_with_carry(val: u8, freg: &mut Register<Flags>) -> u8 {
    freg.write(Flags::empty());

    let bit_7 = (val & 0x80) >> 7;

    if val == 0 {
        freg.write(ZeroFlag);
        return val;
    }

    if val & 0x80 != 0 {
        freg.write(CarryFlag);
    }

    return (val << 1) | bit_7;
}

/// Rotate register left
/// Set ZeroFlag if result is zero
/// Set CarryFlag if bit 7 is 1
pub fn rotate_left_with_carry(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_rotate_left_with_carry(reg.read(), freg);
    reg.write(val);
}

pub fn rotate_left_with_carry_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_rotate_left_with_carry(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

/// Write sp to address with value of next two bytes
pub fn write_u16_immediate_address(mem: &mut Memory, pc: &mut Register<u16>, val: u16){
    let addr = u16_immediate(mem, pc);

    debug!("Writing {} to {}", val, addr);
    mem.write_word(addr, val);
}

/// Performs no operation and consumes a cycle
pub fn nop() {
}

/// Adds two sets of registers as 16 bit numbers with carries counted on bit 11 and 15
pub fn add_register_pair_to_register_pair(rega: &mut Register<u8>, regb: &mut Register<u8>, reg1: u8, reg2: u8, freg: &mut Register<Flags>) {
    let first = pack_u16(rega.read(), regb.read());
    let second = pack_u16(reg1, reg2);

    let sum = first + second;

    // Reset subtract flag, leave ZeroFlag alone
    let mut flags = freg.read();
    flags.remove(SubtractFlag);
    flags.remove(CarryFlag);
    flags.remove(HalfCarryFlag);

    if high_nibble(rega.read()) + high_nibble(reg1) > 15 {
        flags.insert(CarryFlag)
    }

    if low_nibble(rega.read()) + low_nibble(reg1) > 15 {
        flags.insert(HalfCarryFlag);
    }

    rega.write(high_byte(sum));
    regb.write(low_byte(sum));
    freg.write(flags);
}

pub fn ld_from_address(mem: &Memory, reg: &mut Register<u8>, addr: u16) {
    let val = mem.read_byte(addr);
    reg.write(val);
}

pub fn decrement_register_pair(reg1: &mut Register<u8>, reg2: &mut Register<u8>) {
    let val = pack_u16(reg1.read(), reg2.read());
    let ans = val - 1;

    reg1.write(high_byte(ans));
    reg2.write(low_byte(ans));
}

fn internal_rotate_right_with_carry(val: u8, freg: &mut Register<Flags>) -> u8 {    
    let bit_1 = (val & 0x01) << 7;

    if val == 0 {
        freg.write(ZeroFlag);
        return 0;
    } else {
        if val & 0x01 == 1 {
            freg.write(CarryFlag);
        }
        return ((val >> 1) | bit_1);
    }
}

pub fn rotate_right_with_carry(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_rotate_right_with_carry(reg.read(), freg);
    reg.write(val);
}

pub fn rotate_right_with_carry_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_rotate_right_with_carry(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

/// Add n to current address and jump to it - n = one byte signed immediate value
pub fn jump_by_signed_immediate(mem: &Memory, pc: &mut Register<u16>) {
    let offset = u8_immediate(mem, pc); 
    let current_pc = pc.read();
    let mut new_pc = 0;
    if (offset & 0x80) == 0 {
        new_pc = current_pc + offset as u16;
        debug!("jmp signed immediate new_pc: {:X}", new_pc);
    } else {
        new_pc = (current_pc as i16 + (offset as i8) as i16) as u16;
        debug!("jmp signed immediate new_pc: {:X}", new_pc);
    }
    debug!("jmp {:X}", new_pc);
    pc.write(new_pc);
}

pub fn write_value_to_memory_at_address_and_increment_register(mem: &mut Memory, val: u8, high_reg: &mut Register<u8>, low_reg: &mut Register<u8>) {
    let address = pack_u16(high_reg.read(), low_reg.read());
    mem.write_byte(address, val);
    let new_address = address + 1;
    high_reg.write(high_byte(new_address));
    low_reg.write(low_byte(new_address));
}

pub fn write_value_to_memory_at_address_and_decrement_register(mem: &mut Memory, val: u8, high_reg: &mut Register<u8>, low_reg: &mut Register<u8>) {
    let address = pack_u16(high_reg.read(), low_reg.read());
    mem.write_byte(address, val);
    let new_address = address - 1;
    high_reg.write(high_byte(new_address));
    low_reg.write(low_byte(new_address));
}

pub fn relative_jmp_by_signed_immediate_if_true(mem: &Memory, pc: &mut Register<u16>, should_jump: bool) {
    if should_jump {
        jump_by_signed_immediate(mem, pc);
    } else {
        pc.increment();
    }
}

pub fn ld_from_address_pointed_to_by_register_pair_and_increment_register_pair(mem: &Memory, reg: &mut Register<u8>, high_byte: &mut Register<u8>, low_byte: &mut Register<u8>) {
   let address = pack_u16(high_byte.read(), low_byte.read()); 

   let val = mem.read_byte(address);
   reg.write(val);
   increment_register_pair(high_byte, low_byte);
   debug!("ld from address and inc reg pair - addr: {:X} val: {:X} reg_pair_val: {:X}", address, val, pack_u16(high_byte.read(), low_byte.read()));
}

pub fn complement(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = reg.read();
    reg.write(!val);

    let mut flags = freg.read();
    flags.insert(HalfCarryFlag);
    flags.insert(SubtractFlag);
    freg.write(flags);
}

/// Loads the next two bytes into the passed in register (sp)
pub fn ld_next_two_bytes_into_reg(mem: &Memory, pc: &mut Register<u16>, reg: &mut Register<u16>) {
    let val = u16_immediate(mem, pc);
    debug!("ld_next_two_bytes_into_reg: {:X}", val);
    reg.write(val);
}

pub fn increment_value_at_address(mem: &mut Memory, hb: u8, lb: u8, freg: &mut Register<Flags>) {
    let addr = pack_u16(hb, lb);
    let val = mem.read_byte(addr);
    let mut reg = Register::new(val);
    increment_register(&mut reg, freg);
    mem.write_byte(addr, reg.read());
}

pub fn decrement_value_at_address(mem: &mut Memory, hb: u8, lb: u8, freg: &mut Register<Flags>) {
    let addr = pack_u16(hb, lb);
    let val = mem.read_byte(addr);
    let mut reg = Register::new(val);
    decrement_register(&mut reg, freg);
    mem.write_byte(addr, reg.read());
}

pub fn ld_u8_immediate_into_address(mem: &mut Memory, pc: &mut Register<u16>, hb: u8, lb: u8) {
    let addr = pack_u16(hb, lb);
    let val = u8_immediate(mem, pc);
    mem.write_byte(addr, val);
}

pub fn set_flag(freg: &mut Register<Flags>, flag: Flags) {
    let mut flags = freg.read();
    flags.insert(flag);
    freg.write(flags);
}

pub fn ld_from_address_pointed_to_by_register_pair_and_decrement_register_pair(mem: &Memory, reg: &mut Register<u8>, high_byte: &mut Register<u8>, low_byte: &mut Register<u8>) {
   let address = pack_u16(high_byte.read(), low_byte.read()); 

   let val = mem.read_byte(address);

   reg.write(val);
   decrement_register_pair(high_byte, low_byte);
}

pub fn ccf(freg: &mut Register<Flags>) {
    let mut f = freg.read();
    f.remove(SubtractFlag);
    f.remove(HalfCarryFlag);
    f.toggle(CarryFlag);
    freg.write(f);
}

pub fn ld_u8(reg: &mut Register<u8>, val: u8) {
    debug!("load val: {:X} into register", val);
    reg.write(val);
}

pub fn add_value_at_address(mem: &Memory, reg: &mut Register<u8>, hb: u8, lb: u8, freg: &mut Register<Flags>) {
   let val = mem.read_byte(pack_u16(hb, lb));
   add(reg, val, freg);
}

pub fn adc(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    add_internal(reg, val, freg, true);
}

pub fn adc_value_at_address(mem: &Memory, reg: &mut Register<u8>, address: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(address);
    adc(reg, val, freg);
}

fn half_carry_for_subtract(val1: u8, val2: u8, carry: u8) -> bool {
    if low_nibble(val2) + carry == 0x10 && val1 >= 0x10 {
        return true;
    } else {
        return low_nibble(val1) >= (low_nibble(val2) + carry);
    }

}

fn carry_for_subtract(val1: u8, val2: u8, carry: u8) -> bool {
    return val1 >= (val2 + carry);
}

pub fn internal_sub(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>, with_carry: bool) {
    let reg_val = reg.read();
    let mut flags = SubtractFlag;
    let mut carry = 0;
    
    if freg.read().contains(CarryFlag) && with_carry {
        carry = 1;
    }

    if half_carry_for_subtract(reg_val, val, carry) {
        flags.insert(HalfCarryFlag);
    }

    if carry_for_subtract(reg_val, val, carry) {
        flags.insert(CarryFlag);
    }

    let result = reg_val - val - carry;
    
    if result == 0 && flags.contains(CarryFlag) {
        flags.insert(ZeroFlag);
    }
    debug!("internal sub result {:X}", result);
    reg.write(result);
    freg.write(flags);
}


pub fn sub(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    internal_sub(reg, val, freg, false);
}

pub fn sbc(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    internal_sub(reg, val, freg, true);
}

pub fn sub_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    internal_sub(reg, val, freg, false);
}

pub fn sbc_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    internal_sub(reg, val, freg, true);
}

pub fn and(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    let mut flags = HalfCarryFlag;

    let result = reg.read() & val;

    if result == 0 {
        flags.insert(ZeroFlag);
    }
    
    reg.write(result);
    freg.write(flags);
}

pub fn xor(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    let mut flags = Flags::empty();

    let result = reg.read() ^ val;

    if result == 0 {
        flags.insert(ZeroFlag);
    }
    
    reg.write(result);
    freg.write(flags);
}

pub fn and_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    and(reg, val, freg);
}

pub fn xor_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    xor(reg, val, freg);
}

pub fn or(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    let mut flags = Flags::empty();

    let result = reg.read() | val;

    if result == 0 {
        flags.insert(ZeroFlag);
    }
    
    reg.write(result);
    freg.write(flags);
}

pub fn compare(reg: &mut Register<u8>, val: u8, freg: &mut Register<Flags>) {
    let reg_val = reg.read();
    let mut flags = SubtractFlag;
    let mut carry = 0;
    
    if half_carry_for_subtract(reg_val, val, carry) {
        flags.insert(HalfCarryFlag);
    }

    if carry_for_subtract(reg_val, val, carry) {
        flags.insert(CarryFlag);
    }

    if (reg_val == val) && flags.contains(CarryFlag) {
        flags.insert(ZeroFlag);
    }
    freg.write(flags);
}

pub fn or_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    or(reg, val, freg);
}

pub fn compare_value_at_address(mem: &Memory, reg: &mut Register<u8>, addr: u16, freg: &mut Register<Flags>) {
    let val = mem.read_byte(addr);
    compare(reg, val, freg);
}

pub fn push(mem: &mut Memory, sp: &mut Register<u16>, val: u16) {
    sp.decrement();
    sp.decrement();
    debug!("push sp:{:X} val:{:X}", sp.read(), val);
    mem.write_word(sp.read(), val);
}

fn pop_internal(mem: &Memory, sp: &mut Register<u16>) -> u16 {
    let val = mem.read_word(sp.read());
    sp.increment();
    sp.increment();
    return val;
}

pub fn pop(mem: &Memory, sp: &mut Register<u16>, hb: &mut Register<u8>, lb: &mut Register<u8>) {
    let val = pop_internal(mem, sp);
    debug!("pop val: {:X}", val);
    hb.write(high_byte(val));
    lb.write(low_byte(val));
}

pub fn ret(mem: &Memory, pc: &mut Register<u16>, sp: &mut Register<u16>, should_execute: bool) {
    if should_execute {
        let addr = pop_internal(mem, sp); 
        debug!("ret to {:X}", addr);
        pc.write(addr);
    }
}

pub fn jp_u16_immediate(mem: &Memory, pc: &mut Register<u16>) {
    let addr = u16_immediate(mem, pc); 
    debug!("jmp {:X}", addr);
    pc.write(addr);
}

pub fn jp_u16_immediate_if_true(mem: &Memory,pc: &mut Register<u16>,should_jump: bool) {
    if should_jump {
        jp_u16_immediate(mem, pc);
    } else {
        pc.increment();
        pc.increment();
    }
}

pub fn call_immediate_if_true(mem: &mut Memory, pc: &mut Register<u16>, sp: &mut Register<u16>, should_jump: bool) {
    if should_jump {
        let new_addr = u16_immediate(mem, pc); 
        push(mem, sp, pc.read());
        debug!("call immediate {:X}", new_addr);
        pc.write(new_addr);
    } else {
        pc.increment();
        pc.increment();
    }
}

pub fn add_u8_immediate(mem: &Memory, pc: &mut Register<u16>, reg: &mut Register<u8>, freg: &mut Register<Flags>, with_carry: bool) {
    let val = u8_immediate(mem, pc); 
    add_internal(reg, val, freg, with_carry);
}

pub fn call(mem: &mut Memory, pc: &mut Register<u16>, sp: &mut Register<u16>, addr: u16) {
    push(mem, sp, pc.read());
    pc.write(addr);
}

pub fn sub_u8_immediate(mem: &Memory, pc: &mut Register<u16>, reg: &mut Register<u8>, freg: &mut Register<Flags>, with_carry: bool) {
    let val = u8_immediate(mem, pc);
    debug!("sub u8 immediate {:X}", val);
    internal_sub(reg, val, freg, with_carry);
}

pub fn reti(mem: &Memory, pc: &mut Register<u16>, sp: &mut Register<u16>, ime: &mut bool) {
    ret(mem, pc, sp, true);
    *ime = true;
}

pub fn u8_immediate(mem: &Memory, pc: &mut Register<u16>) -> u8 {
    let val = mem.read_byte(pc.read());
    pc.increment();
    return val;
}

pub fn u16_immediate(mem: &Memory, pc: &mut Register<u16>) -> u16 {
    let val = mem.read_word(pc.read());
    pc.increment();
    pc.increment();
    return val;
}

pub fn jp(pc: &mut Register<u16>, addr: u16) {
    pc.write(addr);
}

pub fn bit(reg_val: u8, pos: u8, freg: &mut Register<Flags>) {
    let mut flags = freg.read();
    flags.remove(SubtractFlag);
    flags.insert(HalfCarryFlag);
    
    assert!(pos < 8, "Bit positions are 0-7");

    let mask = 0x01 << pos as uint;
    let val = reg_val & mask;

    if val == 0 {
        flags.insert(ZeroFlag);
    } else {
        flags.remove(ZeroFlag);
    }
    freg.write(flags);
}

pub fn byte_at_address(mem: &Memory, addr: u16) -> u8 {
    return mem.read_byte(addr);
}

pub fn res(reg: &mut Register<u8>, pos: u8) {
    let mask = 0x01 << pos as uint;
    let val = reg.read();
    let reset_val = val & (!mask);
    reg.write(reset_val);
}

pub fn res_at_addr(mem: &mut Memory, address: u16, pos: u8) {
    let mask = 0x01 << pos as uint;
    let val = mem.read_byte(address);
    let reset_val = val & (!mask);
    mem.write_byte(address, reset_val);
}

pub fn set(reg: &mut Register<u8>, pos: u8) {
    let mask = 0x01 << pos as uint;
    let val = reg.read();
    let reset_val = val | mask;
    reg.write(reset_val);
}

pub fn set_at_addr(mem: &mut Memory, address: u16, pos: u8) {
    let mask = 0x01 << pos as uint;
    let val = mem.read_byte(address);
    let reset_val = val | mask;
    mem.write_byte(address, reset_val);
}

pub fn rotate_left(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_rotate_left(reg.read(), freg);
    reg.write(val);
}

pub fn rotate_left_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_rotate_left(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

fn internal_rotate_left(val: u8, freg: &mut Register<Flags>) -> u8 {
    let mut carry = 0;
    if freg.read().contains(CarryFlag) {
        carry = 1;
    }

    freg.write(Flags::empty());
    if val == 0 {
        freg.write(ZeroFlag);
        return val;
    }

    if val & 0x80 != 0 {
        freg.write(CarryFlag);
    }

    return (val << 1) | carry;
}

fn internal_rotate_right(val: u8, freg: &mut Register<Flags>) -> u8 {
    let mut carry = 0;
    if freg.read().contains(CarryFlag) {
        carry = 0x80;
    }

    freg.write(Flags::empty());
    if val == 0 {
        freg.write(ZeroFlag);
        return val;
    }

    if val & 0x01 != 0 {
        freg.write(CarryFlag);
    }

    return (val >> 1) | carry;
}

pub fn rotate_right(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_rotate_right(reg.read(), freg);
    reg.write(val);
}

pub fn rotate_right_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_rotate_right(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

fn internal_sla(val: u8, freg: &mut Register<Flags>) -> u8 {
    freg.write(Flags::empty());
    if val == 0 {
        freg.write(ZeroFlag);
        return 0;
    }

    if val & 0x80 != 0 {
        freg.write(CarryFlag);
    }

    return val << 1;
}

pub fn sla(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_sla(reg.read(), freg);
    reg.write(val);
}

pub fn sla_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_sla(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

fn internal_sra(val: u8, freg: &mut Register<Flags>) -> u8 {
    freg.write(Flags::empty());
    if val == 0 {
        freg.write(ZeroFlag);
        return 0;
    }

    if val & 0x01 == 1 {
        freg.write(CarryFlag);
    }

    let bit_7 = val & 0x80;
    return (val >> 1) | bit_7;
}

pub fn sra(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_sra(reg.read(), freg);
    reg.write(val);
}

pub fn sra_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_sra(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

fn internal_swap(val: u8, freg: &mut Register<Flags>) -> u8 {
    freg.write(Flags::empty());

    if val == 0 {
        freg.write(ZeroFlag);
    }

    return (low_nibble(val) << 4) + high_nibble(val);
}

pub fn swap(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_swap(reg.read(), freg);
    reg.write(val);
}

pub fn swap_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_swap(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

fn internal_srl(val: u8, freg: &mut Register<Flags>) -> u8 {
    freg.write(Flags::empty());

    if val == 0 {
        freg.write(ZeroFlag);
    }

    if val & 0x01 == 1 {
        freg.write(CarryFlag);
    }

    return val >> 1;
}

pub fn srl(reg: &mut Register<u8>, freg: &mut Register<Flags>) {
    let val = internal_sra(reg.read(), freg);
    reg.write(val);
}

pub fn srl_at_address(mem: &mut Memory, addr: u16, freg: &mut Register<Flags>) {
    let val = internal_sra(mem.read_byte(addr), freg);
    mem.write_byte(addr, val);
}

pub fn disable_interrupts(ime: &mut bool) {
    debug!("disable interrupts");
    *ime = false;
}

pub fn write_value_to_u16_immediate(mem: &mut Memory, pc: &mut Register<u16>, val: u8) {
    let addr = u16_immediate(mem, pc);
    debug!("write value to u16 immediate val: {:X} addr: {:X}", val, addr);
    mem.write_byte(addr, val);
}

pub fn write_val_FF00_plus_immediate(mem: &mut Memory, pc: &mut Register<u16>, val: u8) {
    let lb = u8_immediate(mem, pc);
    let addr = 0xFF00 + lb as u16;
    debug!("write val: {:X} to addr: {:X}", val, addr);
    mem.write_byte(addr, val); 
}

pub fn pop_flags(mem: &Memory, sp: &mut Register<u16>, a: &mut Register<u8>, f: &mut Register<Flags>) {
    let val = pop_internal(mem, sp);
    debug!("POP AF val: {:X}", val);
    a.write(high_byte(val));
    f.write(Flags::from_bits_truncate(low_byte(val)));
}

pub fn load_val_FF00_plus_immediate(mem: &Memory, pc: &mut Register<u16>, reg: &mut Register<u8>) {
    let lb = u8_immediate(mem, pc);
    let addr = 0xFF00 + lb as u16;
    let val = mem.read_byte(addr); 
    debug!("load val: {:X} into a", val);
    reg.write(val);
}

#[test]
fn test_load_val_FF00_plus_immediate() {
}

#[test]
fn test_pop_flags() {
    let mut a = Register::new(0x00);
    let mut f = Register::new(HalfCarryFlag);
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut sp = Register::new(0xFFFC);
    mem.write_word(sp.read(), 0xBCDF);

    pop_flags(&mem, &mut sp, &mut a, &mut f);

    assert!(sp.read() == 0xFFFE);
    assert!(a.read() == 0xBC);
    assert!(f.read() == ZeroFlag | SubtractFlag | CarryFlag);
}

#[test]
fn test_write_val_FF00_plus_immediate() {
}

#[test]
fn test_write_value_to_u16_immediate() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x8284;
    let val = 0x92;
    let mut pc = Register::new(0x1234);

    mem.write_word(0x1234, addr);

    write_value_to_u16_immediate(&mut mem, &mut pc, val);
    assert!(pc.read() == 0x1236);
    assert!(mem.read_byte(addr) == val);
}

#[test]
fn test_disable_interrupts() {
    let mut a = true;
    
    disable_interrupts(&mut a);

    assert!(a == false);
}

#[test]
fn test_srl_at_address() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut freg = Register::new(HalfCarryFlag);
    let mut val = 0x00;
    let addr = 0x3727;
    mem.write_byte(addr, val);
    srl_at_address(&mut mem, addr, &mut freg);

    assert!(freg.read() == ZeroFlag);
    assert!(mem.read_byte(addr) == 0x00);

    val = 0b00110011;
    mem.write_byte(addr, val);
    srl_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0b00011001);
    assert!(freg.read() == CarryFlag);

    srl_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0b00001100);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_srl() {
    let mut reg = Register::new(0x00);
    let mut freg = Register::new(HalfCarryFlag);
    srl(&mut reg, &mut freg);

    assert!(freg.read() == ZeroFlag);
    assert!(reg.read() == 0x00);

    reg.write(0b00110011);
    srl(&mut reg, &mut freg);
    assert!(reg.read() == 0b00011001);
    assert!(freg.read() == CarryFlag);

    srl(&mut reg, &mut freg);
    assert!(reg.read() == 0b00001100);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_swap_at_address() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x2321;
    let mut freg = Register::new(Flags::empty());
    mem.write_byte(addr, 0xCB);
    swap_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0xBC);
}

#[test]
fn test_swap() {
    let mut reg = Register::new(0x00);
    let mut freg = Register::new(Flags::empty());

    swap(&mut reg, &mut freg);
    assert!(reg.read() == 0x00);
    assert!(freg.read() == ZeroFlag);

    reg.write(0xAC);
    swap(&mut reg, &mut freg);
    assert!(reg.read() == 0xCA);
    assert!(freg.read() == Flags::empty());
}

#[test]
fn test_sra_at_address() {
    let mut val = 0b10010001;
    let mut freg = Register::new(Flags::empty());
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x2372;

    mem.write_byte(addr, val);
    sra_at_address(&mut mem, addr, &mut freg);

    assert!(mem.read_byte(addr) == 0b11001000);
    assert!(freg.read() == CarryFlag);
    
    sra_at_address(&mut mem, addr, &mut freg);

    assert!(mem.read_byte(addr) == 0b11100100);
    assert!(freg.read() == Flags::empty());

    mem.write_byte(addr, 0x00);
    sra_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0x00);
    assert!(freg.read() == ZeroFlag);

    mem.write_byte(addr, 0b00001111);
    sra_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0b00000111);
    assert!(freg.read() == CarryFlag);

}

#[test]
fn test_sra() {
    let mut reg = Register::new(0b10010001);
    let mut freg = Register::new(Flags::empty());

    sra(&mut reg, &mut freg);

    assert!(reg.read() == 0b11001000);
    assert!(freg.read() == CarryFlag);
    
    sra(&mut reg, &mut freg);

    assert!(reg.read() == 0b11100100);
    assert!(freg.read() == Flags::empty());

    reg.write(0x00);
    sra(&mut reg, &mut freg);
    assert!(reg.read() == 0x00);
    assert!(freg.read() == ZeroFlag);

    reg.write(0b00001111);
    sra(&mut reg, &mut freg);
    assert!(reg.read() == 0b00000111);
    assert!(freg.read() == CarryFlag);

}

#[test]
fn test_sla_at_address() {
    let mut val = 0b00001111;
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1423;
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    mem.write_byte(addr, val);
    sla_at_address(&mut mem, addr, &mut freg);
    
    // Rotate should happen
    assert!(mem.read_byte(addr) == 0b00011110);
    assert!(freg.read() == Flags::empty());
    
    mem.write_byte(addr, 0x00);

    sla_at_address(&mut mem, addr, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(mem.read_byte(addr) == 0x00);
    assert!(freg.read() == ZeroFlag);
    
    mem.write_byte(addr, 0b11001100);

    sla_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b10011000);
    assert!(freg.read() == CarryFlag);
    
    freg.write(CarryFlag);
    mem.write_byte(addr, 0b11001100);

    sla_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b10011000);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_sla() {
    let mut reg = Register::new(0b00001111);
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    sla(&mut reg, &mut freg);
    
    // Rotate should happen
    assert!(reg.read() == 0b00011110);
    assert!(freg.read() == Flags::empty());
    
    let mut regb = Register::new(0x00);

    sla(&mut regb, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(regb.read() == 0x00);
    assert!(freg.read() == ZeroFlag);

    let mut regc = Register::new(0b11001100);
    freg.write(CarryFlag);
    sla(&mut regc, &mut freg);

    assert!(regc.read() == 0b10011000);
    assert!(freg.read() == CarryFlag);

    regc.write(0b00110011);
    sla(&mut regc, &mut freg);

    assert!(regc.read() == 0b01100110);
    assert!(freg.read() == Flags::empty());
}

#[test]
fn test_rotate_right_at_address() {
    let mut val = 0b00001111;
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1423;
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    mem.write_byte(addr, val);
    rotate_right_at_address(&mut mem, addr, &mut freg);
    
    // Rotate should happen
    assert!(mem.read_byte(addr) == 0b00000111);
    assert!(freg.read() == CarryFlag);
    
    mem.write_byte(addr, 0x00);
    freg.write(Flags::empty());

    rotate_right_at_address(&mut mem, addr, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(mem.read_byte(addr) == 0x00);
    assert!(freg.read() == ZeroFlag);
    
    mem.write_byte(addr, 0b11001101);

    rotate_right_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b01100110);
    assert!(freg.read() == CarryFlag);
    
    freg.write(CarryFlag);
    mem.write_byte(addr, 0b11001100);

    rotate_right_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b11100110);
    assert!(freg.read() == Flags::empty());
}

#[test]
fn test_rotate_right() {
    let mut reg = Register::new(0b10011001);
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    rotate_right(&mut reg, &mut freg);

    assert!(reg.read() == 0b01001100);
    assert!(freg.read() == CarryFlag);

    reg.write(0x0);
    freg.write(Flags::empty());
    rotate_right(&mut reg, &mut freg);

    assert!(reg.read() == 0x0);
    assert!(freg.read() == ZeroFlag);

    reg.write(0x01);
    freg.write(CarryFlag);
    rotate_right(&mut reg, &mut freg);
    assert!(reg.read() == 0x80);
    assert!(freg.read() == CarryFlag);

    reg.write(0x01);
    freg.write(Flags::empty());
    rotate_right(&mut reg, &mut freg);
    assert!(reg.read() == 0x00);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_rotate_left_at_address() {
    let mut val = 0b00001111;
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1423;
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    mem.write_byte(addr, val);
    rotate_left_at_address(&mut mem, addr, &mut freg);
    
    // Rotate should happen
    assert!(mem.read_byte(addr) == 0b00011110);
    assert!(freg.read() == Flags::empty());
    
    mem.write_byte(addr, 0x00);

    rotate_left_at_address(&mut mem, addr, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(mem.read_byte(addr) == 0x00);
    assert!(freg.read() == ZeroFlag);
    
    mem.write_byte(addr, 0b11001100);

    rotate_left_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b10011000);
    assert!(freg.read() == CarryFlag);
    
    freg.write(CarryFlag);
    mem.write_byte(addr, 0b11001100);

    rotate_left_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b10011001);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_rotate_left() {
    let mut reg = Register::new(0b00001111);
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    rotate_left(&mut reg, &mut freg);
    
    // Rotate should happen
    assert!(reg.read() == 0b00011110);
    assert!(freg.read() == Flags::empty());
    
    let mut regb = Register::new(0x00);

    rotate_left(&mut regb, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(regb.read() == 0x00);
    assert!(freg.read() == ZeroFlag);

    let mut regc = Register::new(0b11001100);
    freg.write(CarryFlag);
    rotate_left(&mut regc, &mut freg);

    assert!(regc.read() == 0b10011001);
    assert!(freg.read() == CarryFlag);

    regc.write(0b00110011);
    freg.write(Flags::empty());
    rotate_left(&mut regc, &mut freg);

    assert!(regc.read() == 0b01100110);
    assert!(freg.read() == Flags::empty());


}

#[test]
fn test_rotate_right_with_carry_at_address() {
    let mut val = 0b10011001;
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1237;
    mem.write_byte(addr, val);
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    rotate_right_with_carry_at_address(&mut mem, addr, &mut freg);

    assert!(mem.read_byte(addr) == 0b11001100);
    assert!(freg.read() == CarryFlag);

    mem.write_byte(addr, 0x0);
    rotate_right_with_carry_at_address(&mut mem, addr, &mut freg);

    assert!(mem.read_byte(addr) == 0x0);
    assert!(freg.read() == ZeroFlag);

    mem.write_byte(addr, 0x01);
    freg.write(CarryFlag);
    rotate_right_with_carry_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0x80);
    assert!(freg.read() == CarryFlag);
    
    mem.write_byte(addr, 0x01);
    rotate_right_with_carry_at_address(&mut mem, addr, &mut freg);
    assert!(mem.read_byte(addr) == 0x80);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_rotate_left_with_carry_at_address() {
    let mut val = 0b00001111;
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1423;
    let mut freg = Register::new(SubtractFlag | HalfCarryFlag);

    mem.write_byte(addr, val);
    rotate_left_with_carry_at_address(&mut mem, addr, &mut freg);
    
    // Rotate should happen
    assert!(mem.read_byte(addr) == 0b00011110);
    assert!(freg.read() == Flags::empty());
    
    mem.write_byte(addr, 0x00);

    rotate_left_with_carry_at_address(&mut mem, addr, &mut freg);

    // Zero should return zero with ZeroFlag
    assert!(mem.read_byte(addr) == 0x00);
    assert!(freg.read() == ZeroFlag);

    mem.write_byte(addr, 0b11001100);

    rotate_left_with_carry_at_address(&mut mem, addr, &mut freg);

    // Carry should get set
    assert!(mem.read_byte(addr) == 0b10011001);
    assert!(freg.read() == CarryFlag);
}

#[test]
fn test_set_at_addr() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1235;

    mem.write_byte(addr, 0b11001100); 

    set_at_addr(&mut mem, addr, 0);
    assert!(mem.read_byte(addr) == 0b11001101);
}

#[test]
fn test_set() {
    let mut reg = Register::new(0b00110011);

    set(&mut reg, 7);
    assert!(reg.read() == 0b10110011);

    set(&mut reg, 1);
    assert!(reg.read() == 0b10110011);

    set(&mut reg, 4);
    assert!(reg.read() == 0b10110011);

}

#[test]
fn test_res_at_addr() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1235;

    mem.write_byte(addr, 0b11001100); 

    res_at_addr(&mut mem, addr, 2);
    assert!(mem.read_byte(addr) == 0b11001000);
    
}

#[test]
fn test_res() {
    let mut reg = Register::new(0b00110011);

    res(&mut reg, 0);
    assert!(reg.read() == 0b00110010);

    res(&mut reg, 5);
    assert!(reg.read() == 0b00010010);

    res(&mut reg, 7);
    assert!(reg.read() == 0b00010010);
}

#[test]
fn test_byte_at_address() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let addr = 0x1234;

    mem.write_byte(addr, 0xFE);

    let val = byte_at_address(&mem, addr);
    assert!(val == 0xFE);
}

#[test]
fn test_bit() {
    let reg = 0b0001111;
    let mut freg = Register::new(SubtractFlag | CarryFlag);

    bit(reg, 3, &mut freg);

    assert!(freg.read() == HalfCarryFlag | CarryFlag);

    bit(reg, 7, &mut freg);

    assert!(freg.read() == HalfCarryFlag | CarryFlag | ZeroFlag);
}

#[test]
fn test_jp() {
    let mut pc = Register::new(0x2311);
    let addr = 0x1223;

    jp(&mut pc, addr);

    assert!(pc.read() == addr);
}

#[test]
fn test_u16_immediate() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x123);
    mem.write_word(pc.read(), 0xBEEF);

    let val = u16_immediate(&mem, &mut pc);

    assert!(val == 0xBEEF);
    assert!(pc.read() == 0x125);

}

#[test]
fn test_u8_immediate() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x123);
    mem.write_byte(pc.read(), 0x43);

    let val = u8_immediate(&mem, &mut pc);

    assert!(val == 0x43);
    assert!(pc.read() == 0x124);
}

#[test]
fn test_reti() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut ime = false;
    let mut pc = Register::new(0x123);
    let mut sp = Register::new(0xFFFC);
    mem.write_word(sp.read(), 0xADCD);

    reti(&mem, &mut pc, &mut sp, &mut ime);
    assert!(pc.read() == 0xADCD);
    assert!(ime == true);
    assert!(sp.read() == 0xFFFE);
}

#[test]
fn test_sub_u8_immediate() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x2736);
    let mut reg = Register::new(0x35);
    let mut freg = Register::new(Flags::empty());
    mem.write_byte(pc.read(), 0x11);

    sub_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, false);

    assert!(pc.read() == 0x2737);
    assert!(reg.read() == 0x24);
    assert!(freg.read() == SubtractFlag | HalfCarryFlag | CarryFlag);

    mem.write_byte(pc.read(), 0x0C);
    sub_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, false);

    assert!(pc.read() == 0x2738);
    assert!(reg.read() == 0x18);
    assert!(freg.read() == SubtractFlag | CarryFlag);

    mem.write_byte(pc.read(), 0x0F);
    freg.write(CarryFlag);
    sub_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, true);

    assert!(pc.read() == 0x2739);
    assert!(reg.read() == 0x08);
    assert!(freg.read() == SubtractFlag | CarryFlag | HalfCarryFlag);

}

#[test]
fn test_call() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x4324);
    let mut sp = Register::new(0xFFFE);
    let addr = 0x10;

    call(&mut mem, &mut pc, &mut sp, addr);

    assert!(pc.read() == 0x0010);
    assert!(sp.read() == 0xFFFC);
    assert!(mem.read_word(sp.read()) == 0x4324);
}

#[test]
fn test_add_u8_immediate() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x2736);
    let mut reg = Register::new(0x05);
    let mut freg = Register::new(Flags::empty());
    mem.write_byte(pc.read(), 0x11);

    add_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, false);

    assert!(pc.read() == 0x2737);
    assert!(reg.read() == 0x16);
    assert!(freg.read() == Flags::empty());

    mem.write_byte(pc.read(), 0x0C);
    add_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, false);

    assert!(pc.read() == 0x2738);
    assert!(reg.read() == 0x22);
    assert!(freg.read() == HalfCarryFlag);

    mem.write_byte(pc.read(), 0x0F);
    freg.write(CarryFlag);
    add_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, true);

    assert!(pc.read() == 0x2739);
    assert!(reg.read() == 0x32);
    assert!(freg.read() == HalfCarryFlag);

    mem.write_byte(pc.read(), 0x01);
    reg.write(0xFF);
    add_u8_immediate(&mem, &mut pc, &mut reg, &mut freg, false);
    assert!(reg.read() == 0x00);
    assert!(freg.read() == ZeroFlag);

}

#[test]
fn test_push() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut sp = Register::new(0xFFAB);
     
    push(&mut mem, &mut sp, 0x8735);
    assert!(sp.read() == 0xFFA9);
    assert!(mem.read_word(sp.read()) == 0x8735);
}

#[test]
fn test_call_immediate_if_true() {
    let mut mem = EmptyMemory::new(0xFFFF);
    let mut pc = Register::new(0x6542);
{'fraction_non_alphanumeric': 0.16164303321699655, 'fraction_numerical': 0.037305054548117056, 'mean_word_length': 3.415723677120379, 'pattern_counts': {'":': 0, '<': 337, '<?xml version=': 0, '>': 359, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3157589', 'n_tokens_mistral': 33077, 'n_tokens_neox': 30912, 'n_words': 7194}