use cord_trig::{TrigGraph, TrigOp};
use crate::compiler::CORDICProgram;
use crate::lut::CordicTable;

/// CORDIC evaluator: evaluates a TrigGraph using only integer
/// shifts, adds, and comparisons. No floating point trig.
///
/// Proof that the entire pipeline compiles down to
/// binary arithmetic -- shift, add, compare, repeat.
pub struct CORDICEvaluator {
    frac_bits: u8,
    instr_table_idx: Vec<u8>,
    tables: Vec<CordicTable>,
}

impl CORDICEvaluator {
    pub fn new(word_bits: u8) -> Self {
        let frac_bits = word_bits / 2;
        CORDICEvaluator {
            frac_bits,
            instr_table_idx: Vec::new(),
            tables: vec![CordicTable::for_word_bits(word_bits)],
        }
    }

    /// Create an evaluator from a compiled program, inheriting per-op tables.
    pub fn from_program(program: &CORDICProgram) -> Self {
        CORDICEvaluator {
            frac_bits: program.word_bits / 2,
            instr_table_idx: program.instr_table_idx.clone(),
            tables: program.tables.clone(),
        }
    }

    /// Convert f64 to fixed-point.
    fn to_fixed(&self, val: f64) -> i64 {
        (val * (1i64 << self.frac_bits) as f64).round() as i64
    }

    /// Convert fixed-point back to f64.
    fn to_float(&self, val: i64) -> f64 {
        val as f64 / (1i64 << self.frac_bits) as f64
    }

    /// Fixed-point multiply: (a * b) >> frac_bits
    fn fixed_mul(&self, a: i64, b: i64) -> i64 {
        ((a as i128 * b as i128) >> self.frac_bits) as i64
    }

    /// Fixed-point square root via Newton's method.
    fn fixed_sqrt(&self, val: i64) -> i64 {
        if val <= 0 { return 0; }
        // Initial guess: convert to float, sqrt, convert back
        let mut x = self.to_fixed(self.to_float(val).sqrt());
        if x <= 0 { x = 1; }
        // Two Newton iterations for refinement
        for _ in 0..2 {
            let div = self.fixed_div(val, x);
            x = (x + div) >> 1;
        }
        x
    }

    /// Fixed-point divide: (a << frac_bits) / b
    fn fixed_div(&self, a: i64, b: i64) -> i64 {
        if b == 0 {
            return if a >= 0 { i64::MAX } else { i64::MIN };
        }
        (((a as i128) << self.frac_bits) / b as i128) as i64
    }

    /// Resolve the table for a given instruction index.
    fn table_for(&self, instr_idx: usize) -> &CordicTable {
        if self.instr_table_idx.is_empty() || self.tables.is_empty() {
            return &self.tables[0];
        }
        let tidx = self.instr_table_idx.get(instr_idx).copied().unwrap_or(0) as usize;
        &self.tables[tidx.min(self.tables.len() - 1)]
    }

    /// CORDIC rotation mode using a specific table.
    fn cordic_rotation_with(&self, angle: i64, table: &CordicTable) -> (i64, i64) {
        let mut x = table.gain;
        let mut y: i64 = 0;
        let mut z = angle;

        for i in 0..table.iterations as usize {
            let d = if z >= 0 { 1i64 } else { -1 };
            let x_new = x - d * (y >> i);
            let y_new = y + d * (x >> i);
            z -= d * table.atan[i];
            x = x_new;
            y = y_new;
        }

        (x, y)
    }

    /// CORDIC vectoring mode using a specific table.
    fn cordic_vectoring_with(&self, x_in: i64, y_in: i64, table: &CordicTable) -> (i64, i64) {
        let mut x = x_in;
        let mut y = y_in;
        let mut z: i64 = 0;

        let negate_x = x < 0;
        if negate_x {
            x = -x;
            y = -y;
        }

        for i in 0..table.iterations as usize {
            let d = if y < 0 { 1i64 } else { -1 };
            let x_new = x - d * (y >> i);
            let y_new = y + d * (x >> i);
            z -= d * table.atan[i];
            x = x_new;
            y = y_new;
        }

        let magnitude = self.fixed_mul(x, table.gain);

        if negate_x {
            let pi = self.to_fixed(std::f64::consts::PI);
            let angle = if z >= 0 { pi - z } else { -pi - z };
            (magnitude, angle)
        } else {
            (magnitude, z)
        }
    }


    /// Evaluate the entire trig graph using only CORDIC operations.
    /// Returns the output as f64 (converted from fixed-point at the end).
    pub fn evaluate(&self, graph: &TrigGraph, x: f64, y: f64, z: f64) -> f64 {
        let mut vals = vec![0i64; graph.nodes.len()];
        let use_per_instr = !self.instr_table_idx.is_empty() && self.tables.len() > 1;

        for (i, op) in graph.nodes.iter().enumerate() {
            vals[i] = match op {
                TrigOp::InputX => self.to_fixed(x),
                TrigOp::InputY => self.to_fixed(y),
                TrigOp::InputZ => self.to_fixed(z),
                TrigOp::Const(c) => self.to_fixed(*c),

                TrigOp::Add(a, b) => vals[*a as usize] + vals[*b as usize],
                TrigOp::Sub(a, b) => vals[*a as usize] - vals[*b as usize],
                TrigOp::Mul(a, b) => self.fixed_mul(vals[*a as usize], vals[*b as usize]),
                TrigOp::Div(a, b) => self.fixed_div(vals[*a as usize], vals[*b as usize]),
                TrigOp::Neg(a) => -vals[*a as usize],
                TrigOp::Abs(a) => vals[*a as usize].abs(),

                TrigOp::Sin(a) => {
                    let t = if use_per_instr { self.table_for(i) } else { &self.tables[0] };
                    let (_, sin) = self.cordic_rotation_with(vals[*a as usize], t);
                    sin
                }
                TrigOp::Cos(a) => {
                    let t = if use_per_instr { self.table_for(i) } else { &self.tables[0] };
                    let (cos, _) = self.cordic_rotation_with(vals[*a as usize], t);
                    cos
                }
                TrigOp::Tan(a) => {
                    let t = if use_per_instr { self.table_for(i) } else { &self.tables[0] };
                    let (cos, sin) = self.cordic_rotation_with(vals[*a as usize], t);
                    self.fixed_div(sin, cos)
                }
                TrigOp::Asin(a) => {
                    let t = if use_per_instr { self.table_for(i) } else { &self.tables[0] };
                    let xv = vals[*a as usize];
                    let one = self.to_fixed(1.0);
                    let x2 = self.fixed_mul(xv, xv);
                    let rem = one - x2;
                    let sqrt_rem = self.fixed_sqrt(rem);
                    let (_, angle) = self.cordic_vectoring_with(sqrt_rem, xv, t);
                    angle
                }
                TrigOp::Acos(a) => {
                    let t = if use_per_instr { self.table_for(i) } else { &self.tables[0] };
                    let xv = vals[*a as usize];
                    let one = self.to_fixed(1.0);
                    let x2 = self.fixed_mul(xv, xv);
                    let rem = one - x2;
                    let sqrt_rem = self.fixed_sqrt(rem);
                    let (_, angle) = self.cordic_vectoring_with(xv, sqrt_rem, t);
                    angle
                }
                TrigOp::Atan(a) => {
                    let t = if use_per_instr { self.table_for(i) } else { &self.tables[0] };
                    let one = self.to_fixed(1.0);
                    let (_, angle) = self.cordic_vectoring_with(one, vals[*a as usize], t);
                    angle
                }
                // Phasor-mode CORDIC implementation coming
                TrigOp::Sinh(_) => unimplemented!("sinh: phasor CORDIC pending"),
                TrigOp::Cosh(_) => unimplemented!("cosh: phasor CORDIC pending"),
                TrigOp::Tanh(_) => unimplemented!("tanh: phasor CORDIC pending"),
                TrigOp::Exp(_) => unimplemented!("exp: phasor CORDIC pending"),
                TrigOp::Ln(_) => unimplemented!("ln: phasor CORDIC pending"),

                TrigOp::Asinh(a) => self.to_fixed(self.to_float(vals[*a as usize]).asinh()),
                TrigOp::Acosh(a) => self.to_fixed(self.to_float(vals[*a as usize]).acosh()),
                TrigOp::Atanh(a) => self.to_fixed(self.to_float(vals[*a as usize]).atanh()),
                TrigOp::Sqrt(a) => self.fixed_sqrt(vals[*a as usize]),

                TrigOp::Hypot(a, b) => {
                    let t = if use_per_instr { self.table_for(i) } else { &self.tables[0] };
                    let (mag, _) = self.cordic_vectoring_with(vals[*a as usize], vals[*b as usize], t);
                    mag
                }
                TrigOp::Atan2(a, b) => {
                    let t = if use_per_instr { self.table_for(i) } else { &self.tables[0] };
                    let (_, angle) = self.cordic_vectoring_with(vals[*b as usize], vals[*a as usize], t);
                    angle
                }

                TrigOp::Min(a, b) => vals[*a as usize].min(vals[*b as usize]),
                TrigOp::Max(a, b) => vals[*a as usize].max(vals[*b as usize]),

                TrigOp::Clamp { val, lo, hi } => {
                    vals[*val as usize].clamp(vals[*lo as usize], vals[*hi as usize])
                }
            };
        }

        self.to_float(vals[graph.output as usize])
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use cord_trig::ir::TrigOp;

    #[test]
    fn test_sin_cos() {
        let eval = CORDICEvaluator::new(32);
        let mut g = TrigGraph::new();
        let angle = g.push(TrigOp::Const(std::f64::consts::FRAC_PI_4));
        let sin_node = g.push(TrigOp::Sin(angle));
        g.set_output(sin_node);

        let result = eval.evaluate(&g, 0.0, 0.0, 0.0);
        let expected = std::f64::consts::FRAC_PI_4.sin();
        assert!((result - expected).abs() < 0.001,
            "sin(π/4): CORDIC={result}, expected={expected}");
    }

    #[test]
    fn test_hypot() {
        let eval = CORDICEvaluator::new(32);
        let mut g = TrigGraph::new();
        let a = g.push(TrigOp::Const(3.0));
        let b = g.push(TrigOp::Const(4.0));
        let h = g.push(TrigOp::Hypot(a, b));
        g.set_output(h);

        let result = eval.evaluate(&g, 0.0, 0.0, 0.0);
        assert!((result - 5.0).abs() < 0.01,
            "hypot(3,4): CORDIC={result}, expected=5.0");
    }

    #[test]
    fn test_sphere_sdf() {
        let eval = CORDICEvaluator::new(32);
        let f64_eval = cord_trig::eval::evaluate;

        let mut builder = cord_trig::lower::SdfBuilder::new();
        let p = builder.root_point();
        let dist = builder.sphere(p, 5.0);
        let graph = builder.finish(dist);

        // Point on surface
        let cordic_val = eval.evaluate(&graph, 5.0, 0.0, 0.0);
        let float_val = f64_eval(&graph, 5.0, 0.0, 0.0);
        assert!((cordic_val - float_val).abs() < 0.01,
            "sphere surface: CORDIC={cordic_val}, f64={float_val}");

        // Point inside
        let cordic_val = eval.evaluate(&graph, 2.0, 1.0, 1.0);
        let float_val = f64_eval(&graph, 2.0, 1.0, 1.0);
        assert!((cordic_val - float_val).abs() < 0.1,
            "sphere inside: CORDIC={cordic_val}, f64={float_val}");
    }

    #[test]
    fn test_per_op_config_roundtrip() {
        use crate::config::{CordicConfig, CordicOp, CordicConfigFile, Defaults, Override};
        use crate::compiler::CompileConfig;

        let mut g = TrigGraph::new();
        let angle = g.push(TrigOp::Const(std::f64::consts::FRAC_PI_4));
        let s = g.push(TrigOp::Sin(angle));
        let a = g.push(TrigOp::Const(3.0));
        let b = g.push(TrigOp::Const(4.0));
        let h = g.push(TrigOp::Hypot(a, b));
        let _ = g.push(TrigOp::Add(s, h));
        g.set_output(5);

        // Config: sin at 16 iterations, hypot at 24
        let file = CordicConfigFile {
            defaults: Defaults { word_bits: 32, f: 32 },
            overrides: vec![
                Override { op: "sin".into(), f: 16 },
                Override { op: "hypot".into(), f: 24 },
            ],
        };
        let cfg = CordicConfig::from_file(&g, &file);
        assert_eq!(cfg.iterations_for(CordicOp::Sin), 16);
        assert_eq!(cfg.iterations_for(CordicOp::Hypot), 24);

        // Compile with config
        let compile_cfg = CompileConfig {
            word_bits: 32,
            cordic: Some(cfg),
        };
        let program = crate::CORDICProgram::compile(&g, &compile_cfg);
        assert!(program.tables.len() >= 2);

        // Evaluate from program
        let eval = CORDICEvaluator::from_program(&program);
        let result = eval.evaluate(&g, 0.0, 0.0, 0.0);

        // Should still produce a reasonable result (sin(pi/4) + hypot(3,4) ≈ 0.707 + 5 = 5.707)
        assert!((result - 5.707).abs() < 0.5,
            "per-op config: result={result}, expected ~5.707");
    }
}
