mod opcode;
mod opcode_table;

use std::{
    fmt::Display,
    ops::{Index, IndexMut},
};

use bitflags::bitflags;
use opcode_table::OPCODE_TABLE;
use ux::{u4, u5};

use self::opcode::{Condition, Opcode, Register, RegisterPair};

bitflags! {
    #[derive(Clone, Copy, Debug, PartialEq, Eq)]
    pub struct Status: u8 {
        const INTA = 0x1;
        const WO = 0x2;
        const STACK = 0x4;
        const HLTA = 0x8;
        const OUT = 0x10;
        const M1 = 0x20;
        const INP = 0x40;
        const MEMR = 0x80;
    }
}

#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum MemCycle {
    Fetch(u16),
    Read(u16),
    Write(u16, u8),
    StackRead(u16),
    StackWrite(u16, u8),
    In(u16),
    Out(u16, u8),
    #[allow(unused)]
    Inta(u16),
    Hlta(u16),
    #[allow(unused)]
    IntaHlt(u16),
}

impl MemCycle {
    pub fn get_status(self) -> Status {
        match self {
            Self::Fetch(_) => Status::WO | Status::M1 | Status::MEMR,
            Self::Read(_) => Status::WO | Status::MEMR,
            Self::Write(_, _) => Status::empty(),
            Self::StackRead(_) => Status::WO | Status::MEMR | Status::STACK,
            Self::StackWrite(_, _) => Status::STACK,
            Self::In(_) => Status::WO | Status::INP,
            Self::Out(_, _) => Status::OUT,
            Self::Inta(_) => Status::INTA | Status::WO | Status::M1,
            Self::Hlta(_) => Status::HLTA | Status::WO | Status::M1,
            Self::IntaHlt(_) => Status::INTA | Status::HLTA | Status::WO | Status::M1,
        }
    }

    pub fn address(self) -> u16 {
        match self {
            Self::Fetch(a) => a,
            Self::Read(a) => a,
            Self::Write(a, _) => a,
            Self::StackRead(a) => a,
            Self::StackWrite(a, _) => a,
            Self::In(a) => a,
            Self::Out(a, _) => a,
            Self::Inta(a) => a,
            Self::Hlta(a) => a,
            Self::IntaHlt(a) => a,
        }
    }
}

#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum MCycle {
    M1,
    M2,
    M3,
    M4,
    M5,
}

impl MCycle {
    fn next(self) -> Self {
        match self {
            Self::M1 => Self::M2,
            Self::M2 => Self::M3,
            Self::M3 => Self::M4,
            Self::M4 => Self::M5,
            Self::M5 => panic!(),
        }
    }
}

impl Display for MCycle {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            Self::M1 => f.write_str("M1"),
            Self::M2 => f.write_str("M2"),
            Self::M3 => f.write_str("M3"),
            Self::M4 => f.write_str("M4"),
            Self::M5 => f.write_str("M5"),
        }
    }
}

#[derive(Clone, Debug)]
struct RegisterFile {
    b: u8,
    c: u8,
    d: u8,
    e: u8,
    h: u8,
    l: u8,
    a: u8,
}

impl RegisterFile {
    fn new() -> Self {
        Self {
            b: rand::random(),
            c: rand::random(),
            d: rand::random(),
            e: rand::random(),
            h: rand::random(),
            l: rand::random(),
            a: rand::random(),
        }
    }
}

impl Index<Register> for RegisterFile {
    type Output = u8;

    fn index(&self, index: Register) -> &Self::Output {
        match index {
            Register::B => &self.b,
            Register::C => &self.c,
            Register::D => &self.d,
            Register::E => &self.e,
            Register::H => &self.h,
            Register::L => &self.l,
            Register::A => &self.a,
        }
    }
}

impl IndexMut<Register> for RegisterFile {
    fn index_mut(&mut self, index: Register) -> &mut Self::Output {
        match index {
            Register::B => &mut self.b,
            Register::C => &mut self.c,
            Register::D => &mut self.d,
            Register::E => &mut self.e,
            Register::H => &mut self.h,
            Register::L => &mut self.l,
            Register::A => &mut self.a,
        }
    }
}

impl Default for RegisterFile {
    fn default() -> Self {
        Self::new()
    }
}

#[derive(Clone, Debug)]
pub struct I8080 {
    pc: u16,
    regs: RegisterFile,
    sp: u16,
    #[allow(unused)]
    sign: bool,
    #[allow(unused)]
    zero: bool,
    #[allow(unused)]
    parity: bool,
    #[allow(unused)]
    carry: bool,
    #[allow(unused)]
    aux_carry: bool,
    cycle: MCycle,
    opcode: Opcode,
    w: u8,
    z: u8,
    tmp: u8,
    halted: bool,
    inte: bool,
}

impl Display for I8080 {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        f.write_fmt(format_args!(
            "PC: {:#x} A: {:#x}, B: {:#x}, C: {:#x}, D: {:#x}, E: {:#x}, H: {:#x}, L: {:#x}, SP: {:#x}",
            self.pc,
            self.regs.a,
            self.regs.b,
            self.regs.c,
            self.regs.d,
            self.regs.e,
            self.regs.h,
            self.regs.l,
            self.sp
        ))
    }
}

impl I8080 {
    pub fn new() -> Self {
        Self {
            pc: rand::random(),
            regs: RegisterFile::new(),
            sp: rand::random(),
            sign: rand::random(),
            zero: rand::random(),
            parity: rand::random(),
            carry: rand::random(),
            aux_carry: rand::random(),
            cycle: MCycle::M1,
            opcode: Opcode::Nop,
            w: rand::random(),
            z: rand::random(),
            tmp: rand::random(),
            halted: rand::random(),
            inte: rand::random(),
        }
    }

    pub fn reset(&mut self) {
        self.pc = 0;
        self.halted = false;
        self.inte = false;
    }

    pub fn get_mem_cycle(&self) -> MemCycle {
        if self.halted {
            return MemCycle::Hlta(self.pc);
        };
        match self.cycle {
            MCycle::M1 => MemCycle::Fetch(self.pc),
            MCycle::M2 => match self.opcode {
                Opcode::MovMR(src) => {
                    MemCycle::Write(self.get_pair(RegisterPair::HL), self.regs[src])
                }
                Opcode::MovRM(_) => MemCycle::Read(self.get_pair(RegisterPair::HL)),
                Opcode::Mvi(_) => MemCycle::Read(self.pc),
                Opcode::Lxi(_) => MemCycle::Read(self.pc),
                Opcode::Lda => MemCycle::Read(self.pc),
                Opcode::Sta => MemCycle::Read(self.pc),
                Opcode::Lhld => MemCycle::Read(self.pc),
                Opcode::Shld => MemCycle::Read(self.pc),
                Opcode::Ldax(rp) => MemCycle::Read(self.get_pair(rp)),
                Opcode::Stax(rp) => MemCycle::Write(self.get_pair(rp), self.regs.a),
                Opcode::Add(_) => MemCycle::Read(self.get_pair(RegisterPair::HL)),
                Opcode::Adi => MemCycle::Read(self.pc),
                Opcode::Adc(_) => MemCycle::Read(self.get_pair(RegisterPair::HL)),
                Opcode::Aci => MemCycle::Read(self.pc),
                Opcode::Sub(_) => MemCycle::Read(self.get_pair(RegisterPair::HL)),
                Opcode::Sui => MemCycle::Read(self.pc),
                Opcode::Sbb(_) => MemCycle::Read(self.get_pair(RegisterPair::HL)),
                Opcode::Sbi => MemCycle::Read(self.pc),
                Opcode::InrM => MemCycle::Read(self.get_pair(RegisterPair::HL)),
                Opcode::DcrM => MemCycle::Read(self.get_pair(RegisterPair::HL)),
                Opcode::Ana(_) => MemCycle::Read(self.get_pair(RegisterPair::HL)),
                Opcode::Ani => MemCycle::Read(self.pc),
                Opcode::Xra(_) => MemCycle::Read(self.get_pair(RegisterPair::HL)),
                Opcode::Xri => MemCycle::Read(self.pc),
                Opcode::Ora(_) => MemCycle::Read(self.get_pair(RegisterPair::HL)),
                Opcode::Ori => MemCycle::Read(self.pc),
                Opcode::Cmp(_) => MemCycle::Read(self.get_pair(RegisterPair::HL)),
                Opcode::Cpi => MemCycle::Read(self.pc),
                Opcode::Jmp => MemCycle::Read(self.pc),
                Opcode::Call => MemCycle::Read(self.pc),
                Opcode::Ret => MemCycle::Read(self.sp),
                Opcode::Rst(_) => MemCycle::StackWrite(self.sp, (self.pc >> 8) as u8),
                Opcode::Push(rp) => MemCycle::StackWrite(self.sp, (self.get_pair(rp) >> 8) as u8),
                Opcode::PushPsw => MemCycle::StackWrite(self.sp, self.regs.a),
                Opcode::Pop(_) => MemCycle::StackRead(self.sp),
                Opcode::PopPsw => MemCycle::StackRead(self.sp),
                Opcode::Xthl => MemCycle::StackRead(self.sp),
                Opcode::In => MemCycle::Read(self.pc),
                Opcode::Out => MemCycle::Read(self.pc),
                _ => unreachable!(),
            },
            MCycle::M3 => match self.opcode {
                Opcode::Mvi(_) => MemCycle::Write(self.get_pair(RegisterPair::HL), self.tmp),
                Opcode::Lxi(_) => MemCycle::Read(self.pc),
                Opcode::Lda => MemCycle::Read(self.pc),
                Opcode::Sta => MemCycle::Read(self.pc),
                Opcode::Lhld => MemCycle::Read(self.pc),
                Opcode::Shld => MemCycle::Read(self.pc),
                Opcode::InrM => MemCycle::Write(self.get_pair(RegisterPair::HL), self.tmp),
                Opcode::DcrM => MemCycle::Write(self.get_pair(RegisterPair::HL), self.tmp),
                Opcode::Jmp => MemCycle::Read(self.pc),
                Opcode::Call => MemCycle::Read(self.pc),
                Opcode::Ret => MemCycle::StackRead(self.sp),
                Opcode::Rst(_) => MemCycle::StackWrite(self.sp, 0),
                Opcode::Push(rp) => MemCycle::StackWrite(self.sp, self.get_pair(rp) as u8),
                Opcode::PushPsw => MemCycle::StackWrite(self.sp, self.get_flags()),
                Opcode::Pop(_) => MemCycle::StackRead(self.sp),
                Opcode::PopPsw => MemCycle::StackRead(self.sp),
                Opcode::Xthl => MemCycle::StackRead(self.sp),
                Opcode::In => MemCycle::In(self.get_wz()),
                Opcode::Out => MemCycle::Out(self.get_wz(), self.regs.a),
                _ => unreachable!(),
            },
            MCycle::M4 => match self.opcode {
                Opcode::Lda => MemCycle::Read(self.get_wz()),
                Opcode::Sta => MemCycle::Write(self.get_wz(), self.regs.a),
                Opcode::Lhld => MemCycle::Read(self.get_wz()),
                Opcode::Shld => MemCycle::Write(self.get_wz(), self.regs.l),
                Opcode::Call => MemCycle::StackWrite(self.sp, (self.pc >> 8) as u8),
                Opcode::Xthl => MemCycle::StackWrite(self.sp, self.regs.h),
                _ => unreachable!(),
            },
            MCycle::M5 => match self.opcode {
                Opcode::Lhld => MemCycle::Read(self.get_wz()),
                Opcode::Shld => MemCycle::Write(self.get_wz(), self.regs.h),
                Opcode::Call => MemCycle::StackWrite(self.sp, self.pc as u8),
                Opcode::Ccc(_) => MemCycle::StackWrite(self.sp, self.pc as u8),
                Opcode::Xthl => MemCycle::StackWrite(self.sp, self.regs.l),
                _ => unreachable!(),
            },
        }
    }

    pub fn finish_m_cycle(&mut self, data: u8) {
        if self.halted {
            return;
        }
        println!(
            "Finish {} ({:?}), got data {}",
            self.cycle,
            self.get_mem_cycle(),
            data
        );
        let mut cond_failed = false;
        match self.cycle {
            MCycle::M1 => {
                self.pc += 1;
                self.opcode = OPCODE_TABLE[data as usize];
                dbg!(self.opcode);
                match self.opcode {
                    Opcode::MovMR(_) => (),
                    Opcode::MovRM(_) => (),
                    Opcode::Mov(dst, src) => {
                        self.regs[dst] = self.regs[src];
                    }
                    Opcode::Sphl => {
                        self.sp = ((self.regs.h as u16) << 8) | (self.regs.l as u16);
                    }
                    Opcode::MviM => (),
                    Opcode::Mvi(_) => (),
                    Opcode::Lxi(_) => {}
                    Opcode::Lda => (),
                    Opcode::Sta => (),
                    Opcode::Lhld => (),
                    Opcode::Shld => (),
                    Opcode::Ldax(_) => (),
                    Opcode::Stax(_) => (),
                    Opcode::Xchg => {
                        (self.regs.d, self.regs.e, self.regs.h, self.regs.l) =
                            (self.regs.h, self.regs.l, self.regs.d, self.regs.e);
                    }
                    Opcode::AddM => (),
                    Opcode::Add(src) => {
                        let (ac, cy, res) = Self::add_8bit(self.regs.a, self.regs[src]);
                        self.update_arith_flags(ac, cy, res);
                        self.regs.a = res;
                    }
                    Opcode::Adi => (),
                    Opcode::AdcM => (),
                    Opcode::Adc(src) => {
                        let (ac, cy, res) = Self::adc_8bit(self.regs.a, self.regs[src], self.carry);
                        self.update_arith_flags(ac, cy, res);
                        self.regs.a = res;
                    }
                    Opcode::Aci => (),
                    Opcode::SubM => (),
                    Opcode::Sub(src) => {
                        let (ac, cy, res) = Self::sub_8bit(self.regs.a, self.regs[src]);
                        self.update_arith_flags(ac, cy, res);
                        self.regs.a = res;
                    }
                    Opcode::Sui => (),
                    Opcode::SbbM => (),
                    Opcode::Sbb(src) => {
                        let (ac, cy, res) = Self::sbb_8bit(self.regs.a, self.regs[src], self.carry);
                        self.update_arith_flags(ac, cy, res);
                        self.regs.a = res;
                    }
                    Opcode::Sbi => (),
                    Opcode::InrM => (),
                    Opcode::Inr(src) => {
                        let (ac, _cy, res) = Self::add_8bit(self.regs[src], 1);
                        self.update_arith_flags(ac, self.carry, res);
                        self.regs[src] = res;
                    }
                    Opcode::DcrM => (),
                    Opcode::Dcr(src) => {
                        let (ac, _cy, res) = Self::sub_8bit(self.regs[src], 1);
                        self.update_arith_flags(ac, self.carry, res);
                        self.regs[src] = res;
                    }
                    Opcode::Inx(dst) => self.set_pair(dst, self.get_pair(dst) + 1),
                    Opcode::Dcx(dst) => self.set_pair(dst, self.get_pair(dst) + 2),
                    Opcode::Dad(src) => {
                        let a = self.get_pair(RegisterPair::HL) as u32;
                        let b = self.get_pair(src) as u32;
                        let res = a + b;
                        self.carry = res > 0xffff;
                        self.set_pair(RegisterPair::HL, res as u16);
                    }
                    Opcode::Daa => todo!(),
                    Opcode::AnaM => (),
                    Opcode::Ana(src) => {
                        self.regs.a &= self.regs[src];
                        self.update_logic_flags(self.regs.a);
                    }
                    Opcode::Ani => (),
                    Opcode::XraM => (),
                    Opcode::Xra(src) => {
                        self.regs.a ^= self.regs[src];
                        self.update_logic_flags(self.regs.a);
                    }
                    Opcode::Xri => (),
                    Opcode::OraM => (),
                    Opcode::Ora(src) => {
                        self.regs.a |= self.regs[src];
                        self.update_logic_flags(self.regs.a);
                    }
                    Opcode::Ori => (),
                    Opcode::CmpM => (),
                    Opcode::Cmp(src) => {
                        let (ac, cy, res) = Self::sub_8bit(self.regs.a, self.regs[src]);
                        self.update_arith_flags(ac, cy, res);
                    }
                    Opcode::Cpi => (),
                    Opcode::Rlc => todo!(),
                    Opcode::Rrc => todo!(),
                    Opcode::Ral => todo!(),
                    Opcode::Rar => todo!(),
                    Opcode::Cma => todo!(),
                    Opcode::Cmc => todo!(),
                    Opcode::Stc => todo!(),
                    Opcode::Jmp => (),
                    Opcode::Jcc(cc) => {
                        cond_failed = !self.check_cond(cc);
                        if cond_failed {
                            self.pc += 2;
                        }
                        self.opcode = Opcode::Jmp; // Identical after M1
                    }
                    Opcode::Call => {
                        self.sp -= 1;
                    }
                    Opcode::Ccc(cc) => {
                        cond_failed = !self.check_cond(cc);
                        if cond_failed {
                            self.pc += 2;
                        } else {
                            self.sp -= 1;
                        }
                        self.opcode = Opcode::Call; // Identical after M1
                    }
                    Opcode::Ret => (),
                    Opcode::Rcc(cc) => {
                        cond_failed = !self.check_cond(cc);
                        self.opcode = Opcode::Ret; // Identical after M1
                    }
                    Opcode::Rst(_) => {
                        self.w = 0;
                        self.sp -= 1;
                    }
                    Opcode::Pchl => {
                        self.pc = self.get_pair(RegisterPair::HL);
                    }
                    Opcode::Push(_) => {
                        self.sp -= 1;
                    }
                    Opcode::PushPsw => {
                        self.sp -= 1;
                    }
                    Opcode::Pop(_) => (),
                    Opcode::PopPsw => (),
                    Opcode::Xthl => (),
                    Opcode::In => (),
                    Opcode::Out => (),
                    Opcode::Ei => {
                        self.inte = false;
                    }
                    Opcode::Di => {
                        self.inte = false;
                    }
                    Opcode::Hlt => self.halted = true,
                    Opcode::Nop => (),
                }
            }
            MCycle::M2 => match self.opcode {
                Opcode::MovMR(_) => (),
                Opcode::MovRM(dst) => self.regs[dst] = data,
                Opcode::MviM => {
                    self.pc += 1;
                    self.tmp = data;
                }
                Opcode::Mvi(dst) => {
                    self.regs[dst] = data;
                }
                Opcode::Lxi(_) => {
                    self.z = data;
                    self.pc += 1;
                }
                Opcode::Lda => {
                    self.z = data;
                    self.pc += 1;
                }
                Opcode::Sta => {
                    self.z = data;
                    self.pc += 1;
                }
                Opcode::Lhld => {
                    self.z = data;
                    self.pc += 1;
                }
                Opcode::Shld => {
                    self.z = data;
                    self.pc += 1;
                }
                Opcode::Ldax(_) => {
                    self.regs.a = data;
                }
                Opcode::Stax(_) => (),
                Opcode::AddM => {
                    let (ac, cy, res) = Self::add_8bit(self.regs.a, data);
                    self.update_arith_flags(ac, cy, res);
                    self.regs.a = res;
                }
                Opcode::Adi => {
                    let (ac, cy, res) = Self::add_8bit(self.regs.a, data);
                    self.update_arith_flags(ac, cy, res);
                    self.regs.a = res;
                    self.pc += 1;
                }
                Opcode::AdcM => {
                    let (ac, cy, res) = Self::adc_8bit(self.regs.a, data, self.carry);
                    self.update_arith_flags(ac, cy, res);
                    self.regs.a = res;
                }
                Opcode::Aci => {
                    let (ac, cy, res) = Self::adc_8bit(self.regs.a, data, self.carry);
                    self.update_arith_flags(ac, cy, res);
                    self.regs.a = res;
                    self.pc += 1;
                }
                Opcode::SubM => {
                    let (ac, cy, res) = Self::sub_8bit(self.regs.a, data);
                    self.update_arith_flags(ac, cy, res);
                    self.regs.a = res;
                }
                Opcode::Sui => {
                    let (ac, cy, res) = Self::sub_8bit(self.regs.a, data);
                    self.update_arith_flags(ac, cy, res);
                    self.regs.a = res;
                    self.pc += 1;
                }
                Opcode::SbbM => {
                    let (ac, cy, res) = Self::sbb_8bit(self.regs.a, data, self.carry);
                    self.update_arith_flags(ac, cy, res);
                    self.regs.a = res;
                }
                Opcode::Sbi => {
                    let (ac, cy, res) = Self::sbb_8bit(self.regs.a, data, self.carry);
                    self.update_arith_flags(ac, cy, res);
                    self.regs.a = res;
                    self.pc += 1;
                }
                Opcode::InrM => {
                    let (ac, _cy, res) = Self::add_8bit(data, 1);
                    self.update_arith_flags(ac, self.carry, self.tmp);
                    self.tmp = res;
                }
                Opcode::DcrM => {
                    let (ac, _cy, res) = Self::sub_8bit(data, 1);
                    self.update_arith_flags(ac, self.carry, self.tmp);
                    self.tmp = res;
                }
                Opcode::AnaM => {
                    self.regs.a &= data;
                    self.update_logic_flags(self.regs.a);
                }
                Opcode::Ani => {
                    self.regs.a &= data;
                    self.update_logic_flags(self.regs.a);
                    self.pc += 1;
                }
                Opcode::XraM => {
                    self.regs.a ^= data;
                    self.update_logic_flags(self.regs.a);
                }
                Opcode::Xri => {
                    self.regs.a ^= data;
                    self.update_logic_flags(self.regs.a);
                    self.pc += 1;
                }
                Opcode::OraM => {
                    self.regs.a |= data;
                    self.update_logic_flags(self.regs.a);
                }
                Opcode::Ori => {
                    self.regs.a ^= data;
                    self.update_logic_flags(self.regs.a);
                    self.pc += 1;
                }
                Opcode::CmpM => {
                    let (ac, cy, res) = Self::sub_8bit(self.regs.a, data);
                    self.update_arith_flags(ac, cy, res);
                }
                Opcode::Cpi => {
                    let (ac, cy, res) = Self::sub_8bit(self.regs.a, data);
                    self.update_arith_flags(ac, cy, res);
                    self.pc += 1;
                }
                Opcode::Jmp => {
                    self.z = data;
                    self.pc += 1;
                }
                Opcode::Call => {
                    self.z = data;
                    self.pc += 1;
                }
                Opcode::Ret => {
                    self.z = data;
                    self.pc += 1;
                }
                Opcode::Rst(_) => {
                    self.sp -= 1;
                }
                Opcode::Push(_) => {
                    self.sp -= 1;
                }
                Opcode::PushPsw => {
                    self.sp -= 1;
                }
                Opcode::Pop(_) => {
                    self.sp += 1;
                    self.z = data;
                }
                Opcode::PopPsw => {
                    self.sp += 1;
                    self.set_flags(data);
                }
                Opcode::Xthl => {
                    self.sp += 1;
                    self.z = data;
                }
                Opcode::In => {
                    self.w = data;
                    self.z = data;
                    self.pc += 1;
                }
                Opcode::Out => {
                    self.w = data;
                    self.z = data;
                    self.pc += 1;
                }
                _ => unreachable!(),
            },
            MCycle::M3 => match self.opcode {
                Opcode::MviM => (),
                Opcode::Lxi(rp) => {
                    self.w = data;
                    self.set_pair(rp, self.get_wz());
                }
                Opcode::Lda => {
                    self.w = data;
                    self.pc += 1;
                }
                Opcode::Sta => {
                    self.w = data;
                    self.pc += 1;
                }
                Opcode::Lhld => {
                    self.w = data;
                    self.pc += 1;
                }
                Opcode::Shld => {
                    self.w = data;
                    self.pc += 1;
                }
                Opcode::InrM => (),
                Opcode::DcrM => (),
                Opcode::Jmp => {
                    self.w = data;
                    self.pc = self.get_wz();
                }
                Opcode::Call => {
                    self.w = data;
                    self.pc += 1;
                }
                Opcode::Ret => {
                    self.w = data;
                    self.pc += 1;
                }
                Opcode::Rst(n) => {
                    self.z = n << 3;
                    self.pc = self.get_wz();
                }
                Opcode::Push(_) => (),
                Opcode::PushPsw => (),
                Opcode::Pop(rp) => {
                    self.sp += 1;
                    self.z = data;
                    self.set_pair(rp, self.get_wz());
                }
                Opcode::PopPsw => {
                    self.sp += 1;
                    self.regs.a = data;
                }
                Opcode::Xthl => {
                    self.w = data;
                }
                _ => unreachable!(),
            },
            MCycle::M4 => match self.opcode {
                Opcode::Lda => {
                    self.regs.a = data;
                }
                Opcode::Sta => (),
                Opcode::Lhld => {
                    self.set_wz(self.get_wz() + 1);
                    self.regs.l = data;
                }
                Opcode::Shld => {
                    self.set_wz(self.get_wz() + 1);
                }
                Opcode::Call => {
                    self.sp -= 1;
                }
                Opcode::Xthl => {
                    self.sp -= 1;
                }
                _ => unreachable!(),
            },
            MCycle::M5 => match self.opcode {
                Opcode::Lhld => {
                    self.regs.h = data;
                }
                Opcode::Shld => (),
                Opcode::Call => (),
                Opcode::Xthl => {
                    self.set_pair(RegisterPair::HL, self.get_wz());
                }
                _ => unreachable!(),
            },
        }
        if self.cycle == self.opcode.max_m_cycle() || cond_failed {
            println!("Instruction done");
            self.cycle = MCycle::M1;
        } else {
            self.cycle = self.cycle.next();
        }
    }

    fn nibblize(n: u8) -> (u4, u4) {
        (u4::new(n >> 4), u4::new(n & 0xF))
    }

    fn join_nibbles(nh: u4, nl: u4) -> u8 {
        (u8::from(nh) << 4) | u8::from(nl)
    }

    fn add_4bit(a: u4, b: u4, cy: bool) -> (bool, u4) {
        let res: u5 = u5::from(a) + u5::from(b) + u5::new(cy as u8);
        (
            (res >> 4) > u5::new(0),
            (res & u5::new(0xF)).try_into().unwrap(),
        )
    }

    fn add_8bit(a: u8, b: u8) -> (bool, bool, u8) {
        Self::adc_8bit(a, b, false)
    }

    fn adc_8bit(a: u8, b: u8, cy: bool) -> (bool, bool, u8) {
        let (ah, al) = Self::nibblize(a);
        let (bh, bl) = Self::nibblize(b);
        let (ac, sl) = Self::add_4bit(al, bl, cy);
        let (cy, sh) = Self::add_4bit(ah, bh, ac);
        let sum = Self::join_nibbles(sh, sl);
        (ac, cy, sum)
    }

    fn sub_8bit(a: u8, b: u8) -> (bool, bool, u8) {
        Self::sbb_8bit(a, b, true)
    }

    fn sbb_8bit(a: u8, b: u8, cy: bool) -> (bool, bool, u8) {
        let (ah, al) = Self::nibblize(a);
        let (bh, bl) = Self::nibblize(!b);
        let (ac, sl) = Self::add_4bit(al, bl, cy);
        let (cy, sh) = Self::add_4bit(ah, bh, ac);
        let sum = Self::join_nibbles(sh, sl);
        (ac, cy, sum)
    }

    fn update_logic_flags(&mut self, res: u8) {
        self.update_arith_flags(false, false, res);
    }

    fn update_arith_flags(&mut self, ac: bool, cy: bool, res: u8) {
        self.sign = (res & 0x80) > 0;
        self.zero = res == 0;
        self.aux_carry = ac;
        self.parity = res.count_ones() % 2 == 0;
        self.carry = cy;
    }

    fn check_cond(&self, cond: Condition) -> bool {
        match cond {
            Condition::NZ => !self.zero,
            Condition::Z => self.zero,
            Condition::NC => !self.carry,
            Condition::C => self.carry,
            Condition::PO => !self.parity,
            Condition::PE => self.parity,
            Condition::P => !self.sign,
            Condition::M => self.sign,
        }
    }

    fn get_pair(&self, pair: RegisterPair) -> u16 {
        match pair {
            RegisterPair::BC => ((self.regs.b as u16) << 8) | (self.regs.c as u16),
            RegisterPair::DE => ((self.regs.d as u16) << 8) | (self.regs.e as u16),
            RegisterPair::HL => ((self.regs.h as u16) << 8) | (self.regs.l as u16),
            RegisterPair::SP => self.sp,
        }
    }

    fn get_wz(&self) -> u16 {
        ((self.w as u16) << 8) | (self.z as u16)
    }

    fn set_pair(&mut self, pair: RegisterPair, val: u16) {
        match pair {
            RegisterPair::BC => {
                self.regs.b = (val >> 8) as u8;
                self.regs.c = val as u8;
            }
            RegisterPair::DE => {
                self.regs.d = (val >> 8) as u8;
                self.regs.e = val as u8;
            }
            RegisterPair::HL => {
                self.regs.h = (val >> 8) as u8;
                self.regs.l = val as u8;
            }
            RegisterPair::SP => self.sp = val,
        }
    }

    fn set_wz(&mut self, val: u16) {
        self.w = (val >> 8) as u8;
        self.z = val as u8;
    }

    fn get_flags(&self) -> u8 {
        0b10 | self.carry as u8
            | (self.parity as u8) << 2
            | (self.aux_carry as u8) << 4
            | (self.zero as u8) << 6
            | (self.sign as u8) << 7
    }

    fn set_flags(&mut self, flags: u8) {
        self.carry = (flags & (1 << 0)) > 0;
        self.parity = (flags & (1 << 2)) > 0;
        self.aux_carry = (flags & (1 << 4)) > 0;
        self.zero = (flags & (1 << 6)) > 0;
        self.sign = (flags & (1 << 7)) > 0;
    }
}