use regs::{GeneralPurposeRegister, GeneralPurposeRegisterPair, IORegister, StatusFlag, PC};

use std::fmt;

#[derive(Debug)]
pub enum DecodingError {
    UndefinedInstruction(u16),
    TruncatedInstruction,
}

#[derive(Debug)]
pub enum IncrementMode {
    None,               // [Z]
    PreDecrement,       // [-Z]
    PostIncrement,      // [Z+]
    ConstantOffset(u8), // e.g. [Z+q]
}

#[derive(Debug)]
#[allow(non_camel_case_types)]
pub enum Instruction {
    ADC(GeneralPurposeRegister, GeneralPurposeRegister),
    ADD(GeneralPurposeRegister, GeneralPurposeRegister),
    ADIW(GeneralPurposeRegisterPair, u16),
    AND(GeneralPurposeRegister, GeneralPurposeRegister),
    ANDI(GeneralPurposeRegister, u8),
    ASR(GeneralPurposeRegister),
    BLD(GeneralPurposeRegister, u8),
    BREAK,
    BR_IF(i8, StatusFlag, bool),
    BST(GeneralPurposeRegister, u8),
    CALL(PC),
    CBI(IORegister, u8),
    CLR_FLAG(StatusFlag),
    CLR(GeneralPurposeRegister), // TODO: Find CLR flag thingy
    COM(GeneralPurposeRegister),
    CP(GeneralPurposeRegister, GeneralPurposeRegister),
    CPC(GeneralPurposeRegister, GeneralPurposeRegister),
    CPI(GeneralPurposeRegister, u8),
    CPSE(GeneralPurposeRegister, GeneralPurposeRegister),
    DEC(GeneralPurposeRegister),
    EICALL,
    EIJMP,
    ELPM(GeneralPurposeRegister, IncrementMode), // Only supports PostIncrement
    EOR(GeneralPurposeRegister, GeneralPurposeRegister),
    FMUL(GeneralPurposeRegister, GeneralPurposeRegister),
    FMULS(GeneralPurposeRegister, GeneralPurposeRegister),
    FMULSU(GeneralPurposeRegister, GeneralPurposeRegister),
    ICALL,
    IJMP,
    IN(GeneralPurposeRegister, IORegister),
    INC(GeneralPurposeRegister),
    JMP(PC),
    LAC(GeneralPurposeRegister),
    LAS(GeneralPurposeRegister),
    LAT(GeneralPurposeRegister),
    LD(
        GeneralPurposeRegister,
        GeneralPurposeRegisterPair,
        IncrementMode,
    ),
    LDI(GeneralPurposeRegister, u8),
    LDS8(GeneralPurposeRegister, u8), // Two variants, with u8 and u16
    LDS16(GeneralPurposeRegister, u16),
    LPM(GeneralPurposeRegister, IncrementMode), // Only supports PostIncrement
    LSL(GeneralPurposeRegister),
    LSR(GeneralPurposeRegister),
    MOV(GeneralPurposeRegister, GeneralPurposeRegister),
    MOVW(GeneralPurposeRegisterPair, GeneralPurposeRegisterPair),
    MUL(GeneralPurposeRegister, GeneralPurposeRegister),
    MULS(GeneralPurposeRegister, GeneralPurposeRegister),
    MULSU(GeneralPurposeRegister, GeneralPurposeRegister),
    NEG(GeneralPurposeRegister),
    NOP,
    OR(GeneralPurposeRegister, GeneralPurposeRegister),
    ORI(GeneralPurposeRegister, u8),
    OUT(IORegister, GeneralPurposeRegister),
    POP(GeneralPurposeRegister),
    PUSH(GeneralPurposeRegister),
    RCALL(i16), // These are relative and have only 12 bits.
    RET,
    RETI,
    RJMP(i16), // These are relative and have only 12 bits.
    ROL(GeneralPurposeRegister),
    ROR(GeneralPurposeRegister),
    SBC(GeneralPurposeRegister, GeneralPurposeRegister),
    SBCI(GeneralPurposeRegister, u8),
    SBI(IORegister, u8),
    SBIC(IORegister, u8),
    SBIS(IORegister, u8),
    SBIW(GeneralPurposeRegisterPair, u16),
    // SBR(GeneralPurposeRegister, u8), => is the same as ORI, so ignore this :)
    SBRC(GeneralPurposeRegister, u8),
    SBRS(GeneralPurposeRegister, u8),
    SER(GeneralPurposeRegister),
    SET_FLAG(StatusFlag), // TODO how is it really called? (CLR)
    SLEEP,
    SPM(IncrementMode), // Only supports PostIncrement
    ST(
        GeneralPurposeRegisterPair,
        GeneralPurposeRegister,
        IncrementMode,
    ),
    STS8(u8, GeneralPurposeRegister), // Two variants, u8/u16
    STS16(u16, GeneralPurposeRegister),
    SUB(GeneralPurposeRegister, GeneralPurposeRegister),
    SUBI(GeneralPurposeRegister, u8),
    SWAP(GeneralPurposeRegister),
    TST(GeneralPurposeRegister),
    WDR,
    XCH(GeneralPurposeRegister),
}

impl fmt::Display for Instruction {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        match *self {
            Instruction::ADC(ref r, ref d) => write!(f, "ADC {}, {}", r, d),
            Instruction::ADD(ref r, ref d) => write!(f, "ADD {}, {}", r, d),
            Instruction::ADIW(ref r, v) => write!(f, "ADIW {}, {}", r, v),
            Instruction::AND(ref r, ref d) => write!(f, "AND {}, {}", r, d),
            Instruction::ANDI(ref r, v) => write!(f, "ANDI {}, {}", r, v),
            Instruction::ASR(ref r) => write!(f, "ASR {}", r),
            Instruction::BLD(ref r, v) => write!(f, "BLD {}, {}", r, v),
            Instruction::BREAK => write!(f, "BREAK"),
            Instruction::BR_IF(rel, ref flag, tgt) => {
                write!(f, "BR_IF {}, {:?}=={}", rel, flag, tgt)
            }
            Instruction::BST(ref r, v) => write!(f, "BST {}, {}", r, v),
            Instruction::CALL(pc) => write!(f, "CALL {:06X}", pc),
            Instruction::CBI(ref ior, v) => write!(f, "CBI {}, {}", ior, v),
            Instruction::CLR_FLAG(flag) => write!(f, "CLR_FLAG {:?}", flag),
            Instruction::CLR(ref r) => write!(f, "CLR {}", r),
            Instruction::COM(ref r) => write!(f, "COM {}", r),
            Instruction::CP(ref r, ref d) => write!(f, "CP {}, {}", r, d),
            Instruction::CPC(ref r, ref d) => write!(f, "CPC {}, {}", r, d),
            Instruction::CPI(ref r, v) => write!(f, "CPI {} {}", r, v),
            Instruction::CPSE(ref r, ref d) => write!(f, "CPSE {} {}", r, d),
            Instruction::DEC(ref r) => write!(f, "DEC {}", r),
            Instruction::EICALL => write!(f, "EICALL"),
            Instruction::EIJMP => write!(f, "EIJMP"),
            Instruction::ELPM(ref r, ref mode) => write!(f, "ELPM[{:?}] {}", mode, r),
            Instruction::EOR(ref r, ref d) => write!(f, "EOR {}, {}", r, d),
            Instruction::FMUL(ref r, ref d) => write!(f, "FMUL {}, {}", r, d),
            Instruction::FMULS(ref r, ref d) => write!(f, "FMULS {}, {}", r, d),
            Instruction::FMULSU(ref r, ref d) => write!(f, "FMULSU {}, {}", r, d),
            Instruction::ICALL => write!(f, "ICALL"),
            Instruction::IJMP => write!(f, "IJMP"),
            Instruction::IN(ref r, ref ior) => write!(f, "IN {}, {}", r, ior),
            Instruction::INC(ref r) => write!(f, "INC {}", r),
            Instruction::JMP(pc) => write!(f, "JMP {:06X}", pc),
            Instruction::LAC(ref r) => write!(f, "LAC {}", r),
            Instruction::LAS(ref r) => write!(f, "LAS {}", r),
            Instruction::LAT(ref r) => write!(f, "LAT {}", r),
            Instruction::LD(ref r, ref d, ref mode) => write!(f, "LD {}, [{} {:?}]", r, d, mode),
            Instruction::LDI(ref r, v) => write!(f, "LDI {}, {}", r, v),
            Instruction::LDS8(ref r, v) => write!(f, "LDS8 {}, {}", r, v),
            Instruction::LDS16(ref r, v) => write!(f, "LDS16 {}, {}", r, v),
            Instruction::LPM(ref r, ref mode) => write!(f, "LPM {} {:?}", r, mode),
            Instruction::LSL(ref r) => write!(f, "LSL {}", r),
            Instruction::LSR(ref r) => write!(f, "LSR {}", r),
            Instruction::MOV(ref r, ref d) => write!(f, "MOV {}, {}", r, d),
            Instruction::MOVW(ref r, ref d) => write!(f, "MOVW {}, {}", r, d),
            Instruction::MUL(ref r, ref d) => write!(f, "MUL {}, {}", r, d),
            Instruction::MULS(ref r, ref d) => write!(f, "MULS {}, {}", r, d),
            Instruction::MULSU(ref r, ref d) => write!(f, "MULSU {}, {}", r, d),
            Instruction::NEG(ref r) => write!(f, "NEG {}", r),
            Instruction::NOP => write!(f, "NOP"),
            Instruction::OR(ref r, ref d) => write!(f, "OR {} {}", r, d),
            Instruction::ORI(ref r, v) => write!(f, "ORI {} {}", r, v),
            Instruction::OUT(ref ior, ref d) => write!(f, "OUT {}, {}", ior, d),
            Instruction::POP(ref r) => write!(f, "POP {}", r),
            Instruction::PUSH(ref r) => write!(f, "PUSH {}", r),
            Instruction::RCALL(rv) => write!(f, "RCALL {:X}", rv),
            Instruction::RET => write!(f, "RET"),
            Instruction::RETI => write!(f, "RETI"),
            Instruction::RJMP(rv) => write!(f, "RJMP {:X}", rv),
            Instruction::ROL(ref r) => write!(f, "ROL {}", r),
            Instruction::ROR(ref r) => write!(f, "ROR {}", r),
            Instruction::SBC(ref r, ref d) => write!(f, "SBC {}, {}", r, d),
            Instruction::SBCI(ref r, v) => write!(f, "SBCI {}, {}", r, v),
            Instruction::SBI(ref ior, v) => write!(f, "SBI {}, {}", ior, v),
            Instruction::SBIC(ref ior, v) => write!(f, "SBIC {}, {}", ior, v),
            Instruction::SBIS(ref ior, v) => write!(f, "SBIS {}, {}", ior, v),
            Instruction::SBIW(ref r, v) => write!(f, "SBIW {}, {}", r, v),
            Instruction::SBRC(ref r, v) => write!(f, "SBRC {}, {}", r, v),
            Instruction::SBRS(ref r, v) => write!(f, "SBRS {}, {}", r, v),
            Instruction::SER(ref r) => write!(f, "SER {}", r),
            Instruction::SET_FLAG(flag) => write!(f, "SET_FLAG {:?}", flag),
            Instruction::SLEEP => write!(f, "SLEEP"),
            Instruction::SPM(ref mode) => write!(f, "SPM {:?}", mode),
            Instruction::ST(ref r, ref d, ref mode) => write!(f, "ST [{} {:?}], {}", r, mode, d),
            Instruction::STS8(v, ref r) => write!(f, "STS8 {}, {}", v, r),
            Instruction::STS16(v, ref r) => write!(f, "STS16 {}, {}", v, r),
            Instruction::SUB(ref r, ref d) => write!(f, "SUB {}, {}", r, d),
            Instruction::SUBI(ref r, v) => write!(f, "SUBI {}, {}", r, v),
            Instruction::SWAP(ref r) => write!(f, "SWAP {}", r),
            Instruction::TST(ref r) => write!(f, "TST {}", r),
            Instruction::WDR => write!(f, "WDR"),
            Instruction::XCH(ref r) => write!(f, "XCH {}", r),
        }
    }
}

impl Instruction {
    // TODO: There are more
    // LDS32 STS32
    pub fn size(&self) -> usize {
        match *self {
            Instruction::JMP(_) | Instruction::CALL(_) |
            Instruction::STS16(_, _) | Instruction::LDS16(_, _) => 4,
            _ => 2,
        }
    }

    pub fn cycles(&self) -> usize {
        // TODO: Implement this correctly.
        2
    }
}

pub fn decode(data: &[u8]) -> Result<Instruction, DecodingError> {
    if data.len() < 2 {
        return Err(DecodingError::TruncatedInstruction);
    }
    // Try to match 2b instructions without any parameters first.
    let v: u16 = (u16::from(data[1]) << 8) | u16::from(data[0]);

    // Load second u16 as well if possible
    let v2: Option<u16> = if data.len() >= 4 {
        Some((u16::from(data[3]) << 8) | u16::from(data[2]))
    } else {
        None
    };

    // Status reg operations (set and clear bits).
    match v & 0b1111_1111_1000_1111 {
        0b1001_0100_1000_1000 => {
            // CLR_FLAG TODO HOW IS IT CALLED
            let idx = ((v & 0b0000_0000_0111_0000) >> 4) as u8;
            return Ok(Instruction::CLR_FLAG(
                StatusFlag::try_from_idx(idx).unwrap(),
            ));
        }
        0b1001_0100_0000_1000 => {
            // SET_FLAG TODO HOW IS IT CALLED
            let idx = ((v & 0b0000_0000_0111_0000) >> 4) as u8;
            return Ok(Instruction::SET_FLAG(
                StatusFlag::try_from_idx(idx).unwrap(),
            ));
        }
        _ => {}
    }

    // Other instructions with no parameters.
    match v {
        0b1001_0101_1001_1000 => return Ok(Instruction::BREAK),
        0b1001_0101_0001_1001 => return Ok(Instruction::EICALL),
        0b1001_0100_0001_1001 => return Ok(Instruction::EIJMP),
        0b1001_0101_1101_1000 => return Ok(Instruction::ELPM(0.into(), IncrementMode::None)),
        0b1001_0101_0000_1001 => return Ok(Instruction::ICALL),
        0b1001_0100_0000_1001 => return Ok(Instruction::IJMP),
        0b1001_0101_1100_1000 => return Ok(Instruction::LPM(0.into(), IncrementMode::None)),
        0b0000_0000_0000_0000 => return Ok(Instruction::NOP),
        0b1001_0101_0000_1000 => return Ok(Instruction::RET),
        0b1001_0101_0001_1000 => return Ok(Instruction::RETI),
        0b1001_0101_1000_1000 => return Ok(Instruction::SLEEP),
        0b1001_0101_1110_1000 => return Ok(Instruction::SPM(IncrementMode::None)),
        0b1001_0101_1111_1000 => return Ok(Instruction::SPM(IncrementMode::PostIncrement)),
        0b1001_0101_1010_1000 => return Ok(Instruction::WDR),
        _ => {}
    }

    // Now things will get ugly, we have a lot of different prefix sizes for
    // different instructions.

    /*
    0000_0011_1: <class 'FMULS'> <class 'FMULSU'>
    0000_0011_0: <class 'FMUL'> <class 'MULSU'>
    */
    if v & 0b1111_1111_0000_0000 == 0b0000_0011_0000_0000 {
        // FMUL/FMULS/FMULSU/MULSU
        let d = ((v & 0b0111_0000) >> 4) as u8;
        let r = ((v & 0b0111)) as u8;
        let mode = v & 0b1000_1000;
        match mode {
            0b0000_0000 => return Ok(Instruction::MULSU((d + 16).into(), (r + 16).into())),
            0b0000_1000 => return Ok(Instruction::FMUL((d + 16).into(), (r + 16).into())),
            0b1000_0000 => return Ok(Instruction::FMULS((d + 16).into(), (r + 16).into())),
            0b1000_1000 => return Ok(Instruction::FMULSU((d + 16).into(), (r + 16).into())),
            _ => unreachable!(),
        }
    }

    /*
    10010110: <class 'ADIW'> '1001 0110 KKdd KKKK'
    00000001: <class 'MOVW'> '0000 0001 dddd rrrr'
    00000010: <class 'MULS'> '0000 0010 dddd rrrr'
    10010111: <class 'SBIW'> '1001 0111 KKdd KKKK'
    11101111: <class 'SER'>  '1110 1111 dddd 1111'
    10011010: <class 'SBI'>  '1001 1010 AAAA Abbb'
    10011011: <class 'SBIS'> '1001 1011 AAAA Abbb'
    10011001: <class 'SBIC'> '1001 1001 AAAA Abbb'
    10011000: <class 'CBI'>  '1001 1000 AAAA Abbb'
     */
    {
        let K = (((v & 0b1100_0000) >> 2) | (v & 0b1111)) as u8;
        let d = ((v & 0b1111_0000) >> 4) as u8;
        let r = (v & 0b1111) as u8;
        let A = u16::from((v & 0b1111_1000) >> 3);
        let b = (v & 0b0111) as u8;
        match v & 0b1111_1111_0000_0000 {
            0b1001_0110_0000_0000 => {
                return Ok(Instruction::ADIW(((d & 0b11) * 2 + 24).into(), u16::from(K)))
            }
            0b0000_0001_0000_0000 => return Ok(Instruction::MOVW((d * 2).into(), (r * 2).into())),
            0b0000_0010_0000_0000 => return Ok(Instruction::MULS((d + 16).into(), (r + 16).into())),
            0b1001_0111_0000_0000 => {
                return Ok(Instruction::SBIW(((d & 0b11) * 2 + 24).into(), u16::from(K)))
            }
            0b1110_1111_0000_0000 => if r == 0b1111 {
                return Ok(Instruction::SER((d + 16).into()));
            },
            0b1001_1010_0000_0000 => return Ok(Instruction::SBI(A, b)),
            0b1001_1011_0000_0000 => return Ok(Instruction::SBIS(A, b)),
            0b1001_1001_0000_0000 => return Ok(Instruction::SBIC(A, b)),
            0b1001_1000_0000_0000 => return Ok(Instruction::CBI(A, b)),
            _ => {}
        }
    }

    /*
    1001001: <class 'STS_32'>
    1001000: <class 'LDS_32'>
    */
    // Handle LD{X,Y,Z}'s (none, incre and decre, just no offset)
    {
        if v & 0b1110_1100_0000_0000 == 0b1000_0000_0000_0000 {
            let dr = (((v & 0b1_1111_0000) >> 4) as u8).into();
            // mode: 00 -> None, 01 -> PostIncrement, 10 -> PreDecrement
            let mode = v & 0b11;
            let do_store = v & 0b0010_0000_0000 != 0;
            match (
                do_store,
                v & 0b0001_0000_0000_0000 > 0,
                (v >> 2) & 0b11,
                mode,
            ) {
                // LD X
                (false, true, 0b11, 0b00) => {
                    return Ok(Instruction::LD(dr, 26.into(), IncrementMode::None))
                }
                (false, true, 0b11, 0b01) => {
                    return Ok(Instruction::LD(dr, 26.into(), IncrementMode::PostIncrement))
                }
                (false, true, 0b11, 0b10) => {
                    return Ok(Instruction::LD(dr, 26.into(), IncrementMode::PreDecrement))
                }
                // LD Y
                (false, false, 0b10, 0b00) => {
                    return Ok(Instruction::LD(dr, 28.into(), IncrementMode::None))
                }
                (false, true, 0b10, 0b01) => {
                    return Ok(Instruction::LD(dr, 28.into(), IncrementMode::PostIncrement))
                }
                (false, true, 0b10, 0b10) => {
                    return Ok(Instruction::LD(dr, 28.into(), IncrementMode::PreDecrement))
                }
                // LD Z
                (false, false, 0b00, 0b00) => {
                    return Ok(Instruction::LD(dr, 30.into(), IncrementMode::None))
                }
                (false, true, 0b00, 0b01) => {
                    return Ok(Instruction::LD(dr, 30.into(), IncrementMode::PostIncrement))
                }
                (false, true, 0b00, 0b10) => {
                    return Ok(Instruction::LD(dr, 30.into(), IncrementMode::PreDecrement))
                }
                // ST X
                (true, true, 0b11, 0b00) => {
                    return Ok(Instruction::ST(26.into(), dr, IncrementMode::None))
                }
                (true, true, 0b11, 0b01) => {
                    return Ok(Instruction::ST(26.into(), dr, IncrementMode::PostIncrement))
                }
                (true, true, 0b11, 0b10) => {
                    return Ok(Instruction::ST(26.into(), dr, IncrementMode::PreDecrement))
                }
                // ST Y
                (true, false, 0b10, 0b00) => {
                    return Ok(Instruction::ST(28.into(), dr, IncrementMode::None))
                }
                (true, true, 0b10, 0b01) => {
                    return Ok(Instruction::ST(28.into(), dr, IncrementMode::PostIncrement))
                }
                (true, true, 0b10, 0b10) => {
                    return Ok(Instruction::ST(28.into(), dr, IncrementMode::PreDecrement))
                }
                // ST Z
                (true, false, 0b00, 0b00) => {
                    return Ok(Instruction::ST(30.into(), dr, IncrementMode::None))
                }
                (true, true, 0b00, 0b01) => {
                    return Ok(Instruction::ST(30.into(), dr, IncrementMode::PostIncrement))
                }
                (true, true, 0b00, 0b10) => {
                    return Ok(Instruction::ST(30.into(), dr, IncrementMode::PreDecrement))
                }

                // POP: 1001 000d dddd 1111
                (false, true, 0b11, 0b11) => return Ok(Instruction::POP(dr)),
                // PUSH: 1001 001d dddd 1111
                (true, true, 0b11, 0b11) => return Ok(Instruction::PUSH(dr)),
                // LAC: 1001 001r rrrr 0110
                (true, true, 0b01, 0b10) => return Ok(Instruction::LAC(dr)),
                // LAS: 1001 001r rrrr 0101
                (true, true, 0b01, 0b01) => return Ok(Instruction::LAS(dr)),
                // LAT: 1001 001r rrrr 0111
                (true, true, 0b01, 0b11) => return Ok(Instruction::LAT(dr)),
                // XCH: 1001 001r rrrr 0100
                (true, true, 0b01, 0b00) => return Ok(Instruction::XCH(dr)),
                // ELPM (2) 1001 000d dddd 0110
                (false, true, 0b01, 0b10) => return Ok(Instruction::ELPM(dr, IncrementMode::None)),
                // ELPM (3) 1001 000d dddd 0111
                (false, true, 0b01, 0b11) => {
                    return Ok(Instruction::ELPM(dr, IncrementMode::PostIncrement))
                }
                // LPM (2): 1001 000d dddd 0100
                (false, true, 0b01, 0b00) => return Ok(Instruction::LPM(dr, IncrementMode::None)),
                // LPM (3): 1001 000d dddd 0101
                (false, true, 0b01, 0b01) => {
                    return Ok(Instruction::LPM(dr, IncrementMode::PostIncrement))
                }
                // TODO: LDS32 STS32
                // STS32 1001 001r rrrr 0000 kkkk kkkk kkkk kkkk
                (true, true, 0, 0) => {
                    //STS32
                    match v2 {
                        Some(k) => return Ok(Instruction::STS16(k, dr)),
                        None => return Err(DecodingError::TruncatedInstruction),
                    }
                }
                // LDS32 1001 000d dddd 0000 kkkk kkkk kkkk kkkk
                (false, true, 0, 0) => {
                    //LDS32
                    match v2 {
                        Some(k) => return Ok(Instruction::LDS16(dr, k)),
                        None => return Err(DecodingError::TruncatedInstruction),
                    }
                }
                _ => {}
            }
        }
    }
    //LDX1 1001 000d dddd 1100
    //LDX2 1001 000d dddd 1101
    //LDX3 1001 000d dddd 1110

    //LDY1 1000 000d dddd 1000
    //LDY2 1001 000d dddd 1001
    //LDY3 1001 000d dddd 1010

    //LDZ1 1000 000d dddd 0000
    //LDZ2 1001 000d dddd 0001
    //LDZ3 1001 000d dddd 0010

    //STY1 1000 001r rrrr 1000
    //STY2 1001 001r rrrr 1001
    //STY3 1001 001r rrrr 1010

    match v & 0b1111_1110_0000_0000 {
        0b1111_1100_0000_0000 | 0b1111_1110_0000_0000 => {
            // 1111110: <class 'SBRC'>
            // 1111111: <class 'SBRS'>
            // 1111 110r rrrr 0bbb
            let r = (((v >> 4) & 0b1_1111) as u8).into();
            let b = (v & 0b111) as u8;
            if (v & 0b1000) == 0 {
                if ((v >> 9) & 1) == 0 {
                    return Ok(Instruction::SBRC(r, b));
                } else {
                    return Ok(Instruction::SBRS(r, b));
                }
            }
        }
        // BLD '1111 100d dddd 0bbb'
        // BST '1111 101d dddd 0bbb'
        0b1111_1000_0000_0000 | 0b1111_1010_0000_0000 => {
            let d = (((v >> 4) & 0b1_1111) as u8).into();
            let b = (v & 0b111) as u8;
            if (v & 0b1000) == 0 {
                if ((v >> 9) & 1) == 1 {
                    return Ok(Instruction::BLD(d, b));
                } else {
                    return Ok(Instruction::BST(d, b));
                }
            }
        }
        0b1001_0100_0000_0000 => {
            /* 1001010: <class 'ASR'> <class 'CALL'> <class 'COM'> <class 'DEC'>
                <class 'INC'> <class 'JMP'> <class 'LSR'> <class 'NEG'>
                <class 'ROR'> <class 'SWAP'>
            */
            let d = (((v & 0b0000_0001_1111_0000) >> 4) as u8).into();
            // ASR '1001 010d dddd 0101
            // COM '1001 010d dddd 0000'
            // DEC '1001 010d dddd 1010'
            // INC '1001 010d dddd 0011'
            // LSR '1001 010d dddd 0110'
            // NEG '1001 010d dddd 0001'
            // ROR '1001 010d dddd 0111'
            // SWP '1001 010d dddd 0010'
            match v & 0b1111 {
                0b0101 => return Ok(Instruction::ASR(d)),
                0b0000 => return Ok(Instruction::COM(d)),
                0b1010 => return Ok(Instruction::DEC(d)),
                0b0011 => return Ok(Instruction::INC(d)),
                0b0110 => return Ok(Instruction::LSR(d)),
                0b0001 => return Ok(Instruction::NEG(d)),
                0b0111 => return Ok(Instruction::ROR(d)),
                0b0010 => return Ok(Instruction::SWAP(d)),
                // CALL 1001 010k kkkk 111k kkkk kkkk kkkk kkkk'
                // JMP  '1001 010k kkkk 110k kkkk kkkk kkkk kkkk'
                0b1100 | 0b1101 | 0b1110 | 0b1111 => {
                    // u32 instruction
                    match v2 {
                        None => return Err(DecodingError::TruncatedInstruction),
                        Some(kl) => {
                            let k = (u32::from(v & 0b1111_0000) << 12) | (u32::from(kl));
                            if v & 0b10 == 0 {
                                return Ok(Instruction::JMP(k));
                            } else {
                                return Ok(Instruction::CALL(k));
                            }
                        }
                    }
                }
                _ => {}
            }
        }
        _ => {}
    }

    {
        let d5 = (((v >> 4) & 0b1_1111) as u8).into();
        let r5 = (((v & 0b1111) | ((v >> 5) & 0b1_0000)) as u8).into();
        match v & 0b1111_1100_0000_0000 {
            0b0001_1000_0000_0000 => return Ok(Instruction::SUB(d5, r5)),
            0b0001_1100_0000_0000 => {
                if d5 == r5 {
                    return Ok(Instruction::ROL(d5));
                } else {
                    return Ok(Instruction::ADC(d5, r5));
                }
            }
            0b0000_1100_0000_0000 => {
                if d5 == r5 {
                    return Ok(Instruction::LSL(d5));
                } else {
                    return Ok(Instruction::ADD(d5, r5));
                }
            }
            0b0000_1000_0000_0000 => return Ok(Instruction::SBC(d5, r5)),
            0b0000_0100_0000_0000 => return Ok(Instruction::CPC(d5, r5)),
            0b1001_1100_0000_0000 => return Ok(Instruction::MUL(d5, r5)),
            0b0001_0100_0000_0000 => return Ok(Instruction::CP(d5, r5)),
            0b0001_0000_0000_0000 => return Ok(Instruction::CPSE(d5, r5)),
            0b0010_0100_0000_0000 => {
                if d5 == r5 {
                    return Ok(Instruction::CLR(d5));
                } else {
                    return Ok(Instruction::EOR(d5, r5));
                }
            }
            0b0010_0000_0000_0000 => {
                if d5 == r5 {
                    return Ok(Instruction::TST(d5));
                } else {
                    return Ok(Instruction::AND(d5, r5));
                }
            }
            0b0010_1000_0000_0000 => return Ok(Instruction::OR(d5, r5)),
            0b0010_1100_0000_0000 => return Ok(Instruction::MOV(d5, r5)),

            // Branch instructions
            // 0b1111_0000_0000_0000 => <class 'BRCS'> <class 'BREQ'> <class 'BRHS'> <class 'BRIE'> <class 'BRLO'> <class 'BRLT'> <class 'BRMI'> <class 'BRTS'> <class 'BRVS'>
            // 0b1111_0100_0000_0000 => <class 'BRCC'> <class 'BRGE'> <class 'BRHC'> <class 'BRID'> <class 'BRNE'> <class 'BRPL'> <class 'BRSH'> <class 'BRTC'> <class 'BRVC'>
            0b1111_0100_0000_0000 | 0b1111_0000_0000_0000 => {
                let flag_should_be_set = ((v >> 10) & 1) == 0;
                let flag = StatusFlag::try_from_idx((v & 0b111) as u8).unwrap();
                let mut k = ((v >> 3) & 0b0111_1111) as i8;

                if k >= (1 << 6) {
                    k = k.wrapping_sub(1 << 7);
                }
                return Ok(Instruction::BR_IF(k, flag, flag_should_be_set));
            }
            _ => {}
        }
    }

    {
        // OUT 1011 1AAr rrrr AAAA
        // IN '1011 0AAd dddd AAAA
        let r = (((v >> 4) & 0b1_1111) as u8).into();
        let A = ((((v >> 5) & 0b11_0000) | (v & 0b1111)) as u8).into();

        // STS return '1010 1kkk rrrr kkkk'.replace(' ', '')
        // LDS return '1010 0kkk dddd kkkk'.replace(' ', '')
        //             1010 0kkk dddd kkkk
        // ST_ return "10q0 qq1r rrrr 0qqq".replace(' ', '')
        //             10q0 qq1r rrrr 0qqq
        let r16 = (((v >> 4) & 0b1111) as u8).into();
        let k = (((v >> 4) & 0b111_0000) | (v & 0b1111)) as u8;
        match v & 0b1111_1000_0000_0000 {
            0b1011_1000_0000_0000 => return Ok(Instruction::OUT(A, r)),
            0b1011_0000_0000_0000 => return Ok(Instruction::IN(r, A)),
            0b1010_1000_0000_0001 => return Ok(Instruction::STS8(k + 0x40, r16)), // Disabled, can't be used
            0b1010_0000_0000_0001 => return Ok(Instruction::LDS8(r16, k + 0x40)), // Disabled, can't be used
            _ => {}
        }
    }

    {
        // Rcall/jmp: 1101 kkkk kkkk kkkk
        // SBR  0110 KKKK dddd KKKK
        // ORI: 0110 KKKK dddd KKKK
        // ANDI 0111 KKKK dddd KKKK
        // CPI  0011 KKKK dddd KKKK
        // SUBI 0101 KKKK dddd KKKK
        // SBCI 0100 KKKK dddd KKKK
        // LDI  1110 KKKK dddd KKKK
        // 0110: <class 'ORI'> <class 'SBR'>
        // 0111: <class 'ANDI'>
        // 0011: <class 'CPI'>
        // 0101: <class 'SUBI'>
        // 0100: <class 'SBCI'>
        // 1110: <class 'LDI'>
        // 1100: <class 'RJMP'>
        // 1101: <class 'RCALL'>
        let k = (v & 0b0000_1111_1111_1111) as i16;
        let k: i16 = if (k & 0b1000_0000_0000) == 0b1000_0000_0000 {
            k - 0b1_0000_0000_0000
        } else {
            k
        };
        let K = (((v >> 4) & 0b1111_0000) | (v & 0b1111)) as u8;
        let d = (((v >> 4) & 0b1111) as u8 + 16u8).into();
        match v & 0b1111_0000_0000_0000 {
            0b0110_0000_0000_0000 => return Ok(Instruction::ORI(d, K)),
            0b0111_0000_0000_0000 => return Ok(Instruction::ANDI(d, K)),
            0b0011_0000_0000_0000 => return Ok(Instruction::CPI(d, K)),
            0b0101_0000_0000_0000 => return Ok(Instruction::SUBI(d, K)),
            0b0100_0000_0000_0000 => return Ok(Instruction::SBCI(d, K)),
            0b1110_0000_0000_0000 => return Ok(Instruction::LDI(d, K)),
            0b1100_0000_0000_0000 => return Ok(Instruction::RJMP(k)),
            0b1101_0000_0000_0000 => return Ok(Instruction::RCALL(k)),
            _ => {}
        }
    }

    // return '10q0 qq0d dddd 1qqq'.replace(' ', '') LD Y IV
    // return '10q0 qq0d dddd 0qqq'.replace(' ', '') LD Z IV
    // return "10q0 qq1r rrrr 1qqq".replace(' ', '') ST Y IV
    // return "10q0 qq1r rrrr 0qqq".replace(' ', '') ST Z IV
    if (v >> 12) & 0b1101 == 0b1000 {
        let w = ((v >> 9) & 1) == 1;
        let dr = (((v >> 4) & 0b11111) as u8).into();
        let q = (((v >> 8) & 0b10_0000) | ((v >> 7) & 0b1_1000) | (v & 0b111)) as u8;
        match (v & 0b1000, w) {
            (0b1000, false) => {
                return Ok(Instruction::LD(
                    dr,
                    28.into(),
                    IncrementMode::ConstantOffset(q),
                ))
            }
            (0b1000, true) => {
                return Ok(Instruction::ST(
                    28.into(),
                    dr,
                    IncrementMode::ConstantOffset(q),
                ))
            }
            (0b0000, false) => {
                return Ok(Instruction::LD(
                    dr,
                    30.into(),
                    IncrementMode::ConstantOffset(q),
                ))
            }
            (0b0000, true) => {
                return Ok(Instruction::ST(
                    30.into(),
                    dr,
                    IncrementMode::ConstantOffset(q),
                ))
            }
            _ => {}
        }
    }
    Err(DecodingError::UndefinedInstruction(v))
}