use ga3::{Bivector, Vector};
use scalars::Exp;

fn approx_eq(a: Vector<f32>, b: Vector<f32>) -> bool {
    (a.x - b.x).abs() < 1e-5 && (a.y - b.y).abs() < 1e-5 && (a.z - b.z).abs() < 1e-5
}

#[test]
fn bivector_exp_does_not_nan() {
    let bivector = Bivector::new(0.0f32, 0.0, 1.0);
    let rotor = bivector.exp();
    assert!(!rotor.scalar.is_nan());
    assert!(!rotor.xy.is_nan());
    assert!(!rotor.xz.is_nan());
    assert!(!rotor.yz.is_nan());
}

#[test]
fn xy_rotation_preserves_z() {
    let rotor = Bivector::xy(std::f32::consts::FRAC_PI_4).exp();
    let result = rotor.rotate(Vector::new(0.0, 0.0, 1.0));
    assert!(
        approx_eq(result, Vector::new(0.0, 0.0, 1.0)),
        "expected (0,0,1), got ({}, {}, {})",
        result.x,
        result.y,
        result.z
    );
}

#[test]
fn xz_rotation_preserves_y() {
    let rotor = Bivector::xz(std::f32::consts::FRAC_PI_4).exp();
    let result = rotor.rotate(Vector::new(0.0, 1.0, 0.0));
    assert!(
        approx_eq(result, Vector::new(0.0, 1.0, 0.0)),
        "expected (0,1,0), got ({}, {}, {})",
        result.x,
        result.y,
        result.z
    );
}

#[test]
fn yz_rotation_preserves_x() {
    let rotor = Bivector::yz(std::f32::consts::FRAC_PI_4).exp();
    let result = rotor.rotate(Vector::new(1.0, 0.0, 0.0));
    assert!(
        approx_eq(result, Vector::new(1.0, 0.0, 0.0)),
        "expected (1,0,0), got ({}, {}, {})",
        result.x,
        result.y,
        result.z
    );
}

#[test]
fn xy_rotation_rotates_x() {
    let rotor = Bivector::xy(std::f32::consts::FRAC_PI_4).exp();
    let result = rotor.rotate(Vector::new(1.0, 0.0, 0.0));
    eprintln!(
        "xy rotation of (1,0,0): ({}, {}, {})",
        result.x, result.y, result.z
    );
    eprintln!(
        "rotor: s={}, xy={}, xz={}, yz={}",
        rotor.scalar, rotor.xy, rotor.xz, rotor.yz
    );
    assert!((result.z).abs() < 1e-5, "z should be zero");
    assert!(
        (result.x * result.x + result.y * result.y - 1.0).abs() < 1e-5,
        "should be unit length"
    );
}

#[test]
fn identity_rotation() {
    let rotor = Bivector::new(0.0f32, 0.0, 0.0).exp();
    let v = Vector::new(3.0, 4.0, 5.0);
    let result = rotor.rotate(v);
    assert!(approx_eq(result, v));
}

fn approx_eq_bv(a: Bivector<f32>, b: Bivector<f32>) -> bool {
    (a.xy - b.xy).abs() < 1e-5 && (a.xz - b.xz).abs() < 1e-5 && (a.yz - b.yz).abs() < 1e-5
}

#[test]
fn xy_rotation_preserves_xy_bivector() {
    let rotor = Bivector::xy(std::f32::consts::FRAC_PI_4).exp();
    let result = rotor.rotate_bivector(Bivector::xy(1.0));
    assert!(
        approx_eq_bv(result, Bivector::xy(1.0)),
        "expected xy(1), got ({}, {}, {})",
        result.xy,
        result.xz,
        result.yz
    );
}

#[test]
fn xy_rotation_rotates_xz_bivector() {
    let rotor = Bivector::xy(std::f32::consts::FRAC_PI_4).exp();
    let result = rotor.rotate_bivector(Bivector::xz(1.0));
    assert!(
        result.xy.abs() < 1e-5,
        "xy should be zero, got {}",
        result.xy
    );
    assert!(
        (result.xz * result.xz + result.yz * result.yz - 1.0).abs() < 1e-5,
        "should be unit length"
    );
}

#[test]
fn bivector_identity_rotation() {
    let rotor = Bivector::new(0.0f32, 0.0, 0.0).exp();
    let b = Bivector::new(1.0, 2.0, 3.0);
    let result = rotor.rotate_bivector(b);
    assert!(approx_eq_bv(result, b));
}
