/// A compiled CORDIC instruction operating on indexed slots.
///
/// Slot i holds the result of instruction i. All operand references
/// point to earlier slots (j < i), matching the TrigGraph's topological order.
///
/// CORDIC mapping:
///   Sin, Cos    → rotation mode (angle → sin/cos via shift-and-add)
///   Hypot       → vectoring mode (magnitude via shift-and-add)
///   Atan2       → vectoring mode (angle via shift-and-add)
///   Mul         → fixed-point multiply (shift-and-add)
///   Add, Sub, Neg, Abs, Min, Max, Clamp → direct binary ops
#[derive(Debug, Clone)]
pub enum CORDICInstr {
    InputX,
    InputY,
    InputZ,
    LoadImm(i64),

    Add(u32, u32),
    Sub(u32, u32),
    Mul(u32, u32),
    Div(u32, u32),
    Neg(u32),
    Abs(u32),

    Sin(u32),
    Cos(u32),
    Tan(u32),

    Asin(u32),
    Acos(u32),
    Atan(u32),

    Sinh(u32),
    Cosh(u32),
    Tanh(u32),

    Asinh(u32),
    Acosh(u32),
    Atanh(u32),

    Sqrt(u32),
    Exp(u32),
    Ln(u32),

    Hypot(u32, u32),
    Atan2(u32, u32),

    Min(u32, u32),
    Max(u32, u32),
    Clamp { val: u32, lo: u32, hi: u32 },
}

/// Precomputed arctan table for CORDIC iterations.
/// atan(2^-i) in fixed-point with the given number of fractional bits.
pub fn atan_table(word_bits: u8) -> Vec<i64> {
    let frac_bits = word_bits - 1;
    (0..word_bits)
        .map(|i| {
            let angle = (2.0f64).powi(-(i as i32)).atan();
            (angle * (1i64 << frac_bits) as f64).round() as i64
        })
        .collect()
}

/// CORDIC gain constant K_n = prod(1/sqrt(1 + 2^{-2i})) for n iterations.
pub fn cordic_gain(iterations: u8, frac_bits: u8) -> i64 {
    let mut k = 1.0f64;
    for i in 0..iterations {
        k *= 1.0 / (1.0 + (2.0f64).powi(-2 * i as i32)).sqrt();
    }
    (k * (1i64 << frac_bits) as f64).round() as i64
}

// Binary encoding

const OP_INPUT_X: u8 = 0x00;
const OP_INPUT_Y: u8 = 0x01;
const OP_INPUT_Z: u8 = 0x02;
const OP_LOAD_IMM: u8 = 0x03;
const OP_ADD: u8 = 0x04;
const OP_SUB: u8 = 0x05;
const OP_MUL: u8 = 0x06;
const OP_DIV: u8 = 0x10;
const OP_NEG: u8 = 0x07;
const OP_ABS: u8 = 0x08;
const OP_SIN: u8 = 0x09;
const OP_COS: u8 = 0x0A;
const OP_HYPOT: u8 = 0x0B;
const OP_ATAN2: u8 = 0x0C;
const OP_MIN: u8 = 0x0D;
const OP_MAX: u8 = 0x0E;
const OP_CLAMP: u8 = 0x0F;
const OP_TAN: u8 = 0x11;
const OP_ASIN: u8 = 0x12;
const OP_ACOS: u8 = 0x13;
const OP_ATAN: u8 = 0x14;
const OP_SINH: u8 = 0x15;
const OP_COSH: u8 = 0x16;
const OP_TANH: u8 = 0x17;
const OP_ASINH: u8 = 0x18;
const OP_ACOSH: u8 = 0x19;
const OP_ATANH: u8 = 0x1A;
const OP_SQRT: u8 = 0x1B;
const OP_EXP: u8 = 0x1C;
const OP_LN: u8 = 0x1D;

pub fn encode_instruction(buf: &mut Vec<u8>, instr: &CORDICInstr) {
    match instr {
        CORDICInstr::InputX => buf.push(OP_INPUT_X),
        CORDICInstr::InputY => buf.push(OP_INPUT_Y),
        CORDICInstr::InputZ => buf.push(OP_INPUT_Z),
        CORDICInstr::LoadImm(v) => {
            buf.push(OP_LOAD_IMM);
            buf.extend_from_slice(&v.to_le_bytes());
        }
        CORDICInstr::Add(a, b) => {
            buf.push(OP_ADD);
            buf.extend_from_slice(&a.to_le_bytes());
            buf.extend_from_slice(&b.to_le_bytes());
        }
        CORDICInstr::Sub(a, b) => {
            buf.push(OP_SUB);
            buf.extend_from_slice(&a.to_le_bytes());
            buf.extend_from_slice(&b.to_le_bytes());
        }
        CORDICInstr::Mul(a, b) => {
            buf.push(OP_MUL);
            buf.extend_from_slice(&a.to_le_bytes());
            buf.extend_from_slice(&b.to_le_bytes());
        }
        CORDICInstr::Div(a, b) => {
            buf.push(OP_DIV);
            buf.extend_from_slice(&a.to_le_bytes());
            buf.extend_from_slice(&b.to_le_bytes());
        }
        CORDICInstr::Neg(a) => {
            buf.push(OP_NEG);
            buf.extend_from_slice(&a.to_le_bytes());
        }
        CORDICInstr::Abs(a) => {
            buf.push(OP_ABS);
            buf.extend_from_slice(&a.to_le_bytes());
        }
        CORDICInstr::Sin(a) => {
            buf.push(OP_SIN);
            buf.extend_from_slice(&a.to_le_bytes());
        }
        CORDICInstr::Cos(a) => {
            buf.push(OP_COS);
            buf.extend_from_slice(&a.to_le_bytes());
        }
        CORDICInstr::Tan(a) => { buf.push(OP_TAN); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Asin(a) => { buf.push(OP_ASIN); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Acos(a) => { buf.push(OP_ACOS); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Atan(a) => { buf.push(OP_ATAN); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Sinh(a) => { buf.push(OP_SINH); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Cosh(a) => { buf.push(OP_COSH); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Tanh(a) => { buf.push(OP_TANH); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Asinh(a) => { buf.push(OP_ASINH); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Acosh(a) => { buf.push(OP_ACOSH); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Atanh(a) => { buf.push(OP_ATANH); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Sqrt(a) => { buf.push(OP_SQRT); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Exp(a) => { buf.push(OP_EXP); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Ln(a) => { buf.push(OP_LN); buf.extend_from_slice(&a.to_le_bytes()); }
        CORDICInstr::Hypot(a, b) => {
            buf.push(OP_HYPOT);
            buf.extend_from_slice(&a.to_le_bytes());
            buf.extend_from_slice(&b.to_le_bytes());
        }
        CORDICInstr::Atan2(a, b) => {
            buf.push(OP_ATAN2);
            buf.extend_from_slice(&a.to_le_bytes());
            buf.extend_from_slice(&b.to_le_bytes());
        }
        CORDICInstr::Min(a, b) => {
            buf.push(OP_MIN);
            buf.extend_from_slice(&a.to_le_bytes());
            buf.extend_from_slice(&b.to_le_bytes());
        }
        CORDICInstr::Max(a, b) => {
            buf.push(OP_MAX);
            buf.extend_from_slice(&a.to_le_bytes());
            buf.extend_from_slice(&b.to_le_bytes());
        }
        CORDICInstr::Clamp { val, lo, hi } => {
            buf.push(OP_CLAMP);
            buf.extend_from_slice(&val.to_le_bytes());
            buf.extend_from_slice(&lo.to_le_bytes());
            buf.extend_from_slice(&hi.to_le_bytes());
        }
    }
}

fn read_u32(data: &[u8], pos: usize) -> Option<u32> {
    Some(u32::from_le_bytes(data.get(pos..pos + 4)?.try_into().ok()?))
}

fn read_i64(data: &[u8], pos: usize) -> Option<i64> {
    Some(i64::from_le_bytes(data.get(pos..pos + 8)?.try_into().ok()?))
}

pub fn decode_instruction(data: &[u8]) -> Option<(CORDICInstr, usize)> {
    let op = *data.first()?;
    match op {
        OP_INPUT_X => Some((CORDICInstr::InputX, 1)),
        OP_INPUT_Y => Some((CORDICInstr::InputY, 1)),
        OP_INPUT_Z => Some((CORDICInstr::InputZ, 1)),
        OP_LOAD_IMM => {
            let v = read_i64(data, 1)?;
            Some((CORDICInstr::LoadImm(v), 9))
        }
        OP_ADD => {
            let a = read_u32(data, 1)?;
            let b = read_u32(data, 5)?;
            Some((CORDICInstr::Add(a, b), 9))
        }
        OP_SUB => {
            let a = read_u32(data, 1)?;
            let b = read_u32(data, 5)?;
            Some((CORDICInstr::Sub(a, b), 9))
        }
        OP_MUL => {
            let a = read_u32(data, 1)?;
            let b = read_u32(data, 5)?;
            Some((CORDICInstr::Mul(a, b), 9))
        }
        OP_DIV => {
            let a = read_u32(data, 1)?;
            let b = read_u32(data, 5)?;
            Some((CORDICInstr::Div(a, b), 9))
        }
        OP_NEG => {
            let a = read_u32(data, 1)?;
            Some((CORDICInstr::Neg(a), 5))
        }
        OP_ABS => {
            let a = read_u32(data, 1)?;
            Some((CORDICInstr::Abs(a), 5))
        }
        OP_SIN => {
            let a = read_u32(data, 1)?;
            Some((CORDICInstr::Sin(a), 5))
        }
        OP_COS => {
            let a = read_u32(data, 1)?;
            Some((CORDICInstr::Cos(a), 5))
        }
        OP_TAN => { let a = read_u32(data, 1)?; Some((CORDICInstr::Tan(a), 5)) }
        OP_ASIN => { let a = read_u32(data, 1)?; Some((CORDICInstr::Asin(a), 5)) }
        OP_ACOS => { let a = read_u32(data, 1)?; Some((CORDICInstr::Acos(a), 5)) }
        OP_ATAN => { let a = read_u32(data, 1)?; Some((CORDICInstr::Atan(a), 5)) }
        OP_SINH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Sinh(a), 5)) }
        OP_COSH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Cosh(a), 5)) }
        OP_TANH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Tanh(a), 5)) }
        OP_ASINH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Asinh(a), 5)) }
        OP_ACOSH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Acosh(a), 5)) }
        OP_ATANH => { let a = read_u32(data, 1)?; Some((CORDICInstr::Atanh(a), 5)) }
        OP_SQRT => { let a = read_u32(data, 1)?; Some((CORDICInstr::Sqrt(a), 5)) }
        OP_EXP => { let a = read_u32(data, 1)?; Some((CORDICInstr::Exp(a), 5)) }
        OP_LN => { let a = read_u32(data, 1)?; Some((CORDICInstr::Ln(a), 5)) }
        OP_HYPOT => {
            let a = read_u32(data, 1)?;
            let b = read_u32(data, 5)?;
            Some((CORDICInstr::Hypot(a, b), 9))
        }
        OP_ATAN2 => {
            let a = read_u32(data, 1)?;
            let b = read_u32(data, 5)?;
            Some((CORDICInstr::Atan2(a, b), 9))
        }
        OP_MIN => {
            let a = read_u32(data, 1)?;
            let b = read_u32(data, 5)?;
            Some((CORDICInstr::Min(a, b), 9))
        }
        OP_MAX => {
            let a = read_u32(data, 1)?;
            let b = read_u32(data, 5)?;
            Some((CORDICInstr::Max(a, b), 9))
        }
        OP_CLAMP => {
            let val = read_u32(data, 1)?;
            let lo = read_u32(data, 5)?;
            let hi = read_u32(data, 9)?;
            Some((CORDICInstr::Clamp { val, lo, hi }, 13))
        }
        _ => None,
    }
}