use cord_trig::{TrigGraph, TrigOp};

/// 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 {
    word_bits: u8,
    frac_bits: u8,
    atan_table: Vec<i64>,
    gain: i64,
}

impl CORDICEvaluator {
    pub fn new(word_bits: u8) -> Self {
        let frac_bits = word_bits - 1;
        let iterations = word_bits;

        // Precompute atan(2^-i) as fixed-point
        let atan_table: Vec<i64> = (0..iterations)
            .map(|i| {
                let angle = (2.0f64).powi(-(i as i32)).atan();
                (angle * (1i64 << frac_bits) as f64).round() as i64
            })
            .collect();

        // CORDIC gain K = product of 1/sqrt(1 + 2^{-2i})
        let mut k = 1.0f64;
        for i in 0..iterations {
            k *= 1.0 / (1.0 + (2.0f64).powi(-2 * i as i32)).sqrt();
        }
        let gain = (k * (1i64 << frac_bits) as f64).round() as i64;

        CORDICEvaluator { word_bits, frac_bits, atan_table, gain }
    }

    /// 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
    }

    /// CORDIC rotation mode: given angle z, compute (cos(z), sin(z)).
    /// Input z is fixed-point radians.
    /// Returns (x, y) = (cos(z), sin(z)) in fixed-point.
    ///
    /// Algorithm:
    ///   Start with x = K (gain), y = 0, z = angle
    ///   For each iteration i:
    ///     if z >= 0: rotate positive (d = +1)
    ///     else:      rotate negative (d = -1)
    ///     x_new = x - d * (y >> i)
    ///     y_new = y + d * (x >> i)
    ///     z_new = z - d * atan(2^-i)
    fn cordic_rotation(&self, angle: i64) -> (i64, i64) {
        let mut x = self.gain;
        let mut y: i64 = 0;
        let mut z = angle;

        for i in 0..self.word_bits 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 * self.atan_table[i];
            x = x_new;
            y = y_new;
        }

        (x, y) // (cos, sin)
    }

    /// CORDIC vectoring mode: given (x, y), compute magnitude and angle.
    /// Returns (magnitude, angle) in fixed-point.
    ///
    /// Algorithm:
    ///   Start with x, y, z = 0
    ///   For each iteration i:
    ///     if y < 0: rotate positive (d = +1)
    ///     else:     rotate negative (d = -1)
    ///     x_new = x - d * (y >> i)
    ///     y_new = y + d * (x >> i)
    ///     z_new = z - d * atan(2^-i)
    ///   Result: x ≈ sqrt(x₀² + y₀²) / K, z ≈ atan2(y₀, x₀)
    fn cordic_vectoring(&self, x_in: i64, y_in: i64) -> (i64, i64) {
        let mut x = x_in;
        let mut y = y_in;
        let mut z: i64 = 0;

        // Handle negative x by reflecting into right half-plane
        let negate_x = x < 0;
        if negate_x {
            x = -x;
            y = -y;
        }

        for i in 0..self.word_bits 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 * self.atan_table[i];
            x = x_new;
            y = y_new;
        }

        // Vectoring output: x_final = (1/K) * sqrt(x0^2 + y0^2).
        // self.gain stores K (~0.6073). Multiply to recover true magnitude.
        let magnitude = self.fixed_mul(x, self.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()];

        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 (_, sin) = self.cordic_rotation(vals[*a as usize]);
                    sin
                }
                TrigOp::Cos(a) => {
                    let (cos, _) = self.cordic_rotation(vals[*a as usize]);
                    cos
                }
                TrigOp::Tan(a) => {
                    let (cos, sin) = self.cordic_rotation(vals[*a as usize]);
                    self.fixed_div(sin, cos)
                }
                TrigOp::Asin(a) => {
                    // asin(x) = atan2(x, sqrt(1-x²))
                    let x = vals[*a as usize];
                    let one = self.to_fixed(1.0);
                    let x2 = self.fixed_mul(x, x);
                    let rem = one - x2;
                    let sqrt_rem = self.fixed_sqrt(rem);
                    let (_, angle) = self.cordic_vectoring(sqrt_rem, x);
                    angle
                }
                TrigOp::Acos(a) => {
                    // acos(x) = atan2(sqrt(1-x²), x)
                    let x = vals[*a as usize];
                    let one = self.to_fixed(1.0);
                    let x2 = self.fixed_mul(x, x);
                    let rem = one - x2;
                    let sqrt_rem = self.fixed_sqrt(rem);
                    let (_, angle) = self.cordic_vectoring(x, sqrt_rem);
                    angle
                }
                TrigOp::Atan(a) => {
                    let one = self.to_fixed(1.0);
                    let (_, angle) = self.cordic_vectoring(one, vals[*a as usize]);
                    angle
                }
                TrigOp::Sinh(a) => self.to_fixed(self.to_float(vals[*a as usize]).sinh()),
                TrigOp::Cosh(a) => self.to_fixed(self.to_float(vals[*a as usize]).cosh()),
                TrigOp::Tanh(a) => self.to_fixed(self.to_float(vals[*a as usize]).tanh()),
                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::Exp(a) => self.to_fixed(self.to_float(vals[*a as usize]).exp()),
                TrigOp::Ln(a) => self.to_fixed(self.to_float(vals[*a as usize]).ln()),

                TrigOp::Hypot(a, b) => {
                    let (mag, _) = self.cordic_vectoring(vals[*a as usize], vals[*b as usize]);
                    mag
                }
                TrigOp::Atan2(a, b) => {
                    let (_, angle) = self.cordic_vectoring(vals[*b as usize], vals[*a as usize]);
                    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}");
    }
}
