#![allow(dead_code)]

use std::f32;

pub use math3d::{cross, dot, CrossProduct, DotProduct, Matrix4x4, Normal, Point, Vector};

/// Clamps a value between a min and max.
pub fn clamp<T: PartialOrd>(v: T, lower: T, upper: T) -> T {
    if v < lower {
        lower
    } else if v > upper {
        upper
    } else {
        v
    }
}

// The stdlib min function is slower than a simple if statement for some reason.
pub fn fast_minf32(a: f32, b: f32) -> f32 {
    if a < b {
        a
    } else {
        b
    }
}

// The stdlib max function is slower than a simple if statement for some reason.
pub fn fast_maxf32(a: f32, b: f32) -> f32 {
    if a > b {
        a
    } else {
        b
    }
}

/// Rounds an integer up to the next power of two.
pub fn upper_power_of_two(mut v: u32) -> u32 {
    v -= 1;
    v |= v >> 1;
    v |= v >> 2;
    v |= v >> 4;
    v |= v >> 8;
    v |= v >> 16;
    v + 1
}

/// Gets the log base 2 of the given integer
pub fn log2_64(mut value: u64) -> u64 {
    // This works by doing a binary search for the largest non-zero binary
    // digit in the number.  Its bit position is then the log2 of the integer.

    let mut log = 0;

    const POWERS: [(u64, u64); 6] = [
        (32, (1 << 32) - 1),
        (16, (1 << 16) - 1),
        (8, (1 << 8) - 1),
        (4, (1 << 4) - 1),
        (2, (1 << 2) - 1),
        (1, (1 << 1) - 1),
    ];

    for &(i, j) in &POWERS {
        let tmp = value >> i;
        if tmp != 0 {
            log += i;
            value = tmp;
        } else {
            value &= j;
        }
    }

    log
}

/// Creates a coordinate system from a single vector.
///
/// The input vector, v, becomes the first vector of the
/// returned tuple, with the other two vectors in the returned
/// tuple defining the orthoganal axes.
///
/// Algorithm taken from "Building an Orthonormal Basis, Revisited" by Duff et al.
pub fn coordinate_system_from_vector(v: Vector) -> (Vector, Vector, Vector) {
    let sign = v.z().signum();
    let a = -1.0 / (sign + v.z());
    let b = v.x() * v.y() * a;
    let v2 = Vector::new(1.0 + sign * v.x() * v.x() * a, sign * b, -sign * v.x());
    let v3 = Vector::new(b, sign + v.y() * v.y() * a, -v.y());

    (v, v2, v3)
}

/// Simple mapping of a vector that exists in a z-up space to
/// the space of another vector who's direction is considered
/// z-up for the purpose.
/// Obviously this doesn't care about the direction _around_
/// the z-up, although it will be sufficiently consistent for
/// isotropic sampling purposes.
///
/// from: The vector we're transforming.
/// toz: The vector whose space we are transforming "from" into.
///
/// Returns the transformed vector.
pub fn zup_to_vec(from: Vector, toz: Vector) -> Vector {
    let (toz, tox, toy) = coordinate_system_from_vector(toz.normalized());

    // Use simple linear algebra to convert the "from"
    // vector to a space composed of tox, toy, and toz
    // as the x, y, and z axes.
    (tox * from.x()) + (toy * from.y()) + (toz * from.z())
}

/// A highly accurate approximation of the probit function.
///
/// From Peter John Acklam, sourced from here:
/// https://web.archive.org/web/20151030215612/http://home.online.no/~pjacklam/notes/invnorm/
///
/// Regarding the approximation error, he says, "The absolute value of the
/// relative error is less than 1.15 × 10−9 in the entire region."
///
/// Given that this implementation outputs 32-bit floating point values,
/// and 32-bit floating point has significantly less precision than that,
/// this approximation can essentially be considered exact.
pub fn probit(n: f32, width: f32) -> f32 {
    let n = n as f64;

    // Coefficients of the rational approximations.
    const A1: f64 = -3.969683028665376e+01;
    const A2: f64 = 2.209460984245205e+02;
    const A3: f64 = -2.759285104469687e+02;
    const A4: f64 = 1.383577518672690e+02;
    const A5: f64 = -3.066479806614716e+01;
    const A6: f64 = 2.506628277459239e+00;

    const B1: f64 = -5.447609879822406e+01;
    const B2: f64 = 1.615858368580409e+02;
    const B3: f64 = -1.556989798598866e+02;
    const B4: f64 = 6.680131188771972e+01;
    const B5: f64 = -1.328068155288572e+01;

    const C1: f64 = -7.784894002430293e-03;
    const C2: f64 = -3.223964580411365e-01;
    const C3: f64 = -2.400758277161838e+00;
    const C4: f64 = -2.549732539343734e+00;
    const C5: f64 = 4.374664141464968e+00;
    const C6: f64 = 2.938163982698783e+00;

    const D1: f64 = 7.784695709041462e-03;
    const D2: f64 = 3.224671290700398e-01;
    const D3: f64 = 2.445134137142996e+00;
    const D4: f64 = 3.754408661907416e+00;

    // Transition points between the rational functions.
    const N_LOW: f64 = 0.02425;
    const N_HIGH: f64 = 1.0 - N_LOW;

    let x = match n {
        // Lower region.
        n if 0.0 < n && n < N_LOW => {
            let q = (-2.0 * n.ln()).sqrt();
            (((((C1 * q + C2) * q + C3) * q + C4) * q + C5) * q + C6)
                / ((((D1 * q + D2) * q + D3) * q + D4) * q + 1.0)
        }

        // Central region.
        n if n <= N_HIGH => {
            let q = n - 0.5;
            let r = q * q;
            (((((A1 * r + A2) * r + A3) * r + A4) * r + A5) * r + A6) * q
                / (((((B1 * r + B2) * r + B3) * r + B4) * r + B5) * r + 1.0)
        }

        // Upper region.
        n if n < 1.0 => {
            let q = (-2.0 * (1.0 - n).ln()).sqrt();
            -(((((C1 * q + C2) * q + C3) * q + C4) * q + C5) * q + C6)
                / ((((D1 * q + D2) * q + D3) * q + D4) * q + 1.0)
        }

        // Exactly 1 or 0.  Should be extremely rare.
        n if n == 0.0 => -std::f64::INFINITY,
        n if n == 1.0 => std::f64::INFINITY,

        // Outside of valid input range.
        _ => std::f64::NAN,
    };

    x as f32 * width
}

//----------------------------------------------------------------
// Adapted to Rust from https://code.google.com/archive/p/fastapprox/

pub fn fast_ln(x: f32) -> f32 {
    use std::mem::transmute_copy;

    let mut y = unsafe { transmute_copy::<f32, u32>(&x) as f32 };
    y *= 8.262_958_288_192_749_0e-8;
    y - 87.989_971_088
}

pub fn fast_pow2(p: f32) -> f32 {
    use std::mem::transmute_copy;

    let offset: f32 = if p < 0.0 { 1.0 } else { 0.0 };
    let clipp: f32 = if p < -126.0 { -126.0 } else { p };
    let w: i32 = clipp as i32;
    let z: f32 = clipp - w as f32 + offset;

    let i: u32 = ((1 << 23) as f32
        * (clipp + 121.274_057_5 + 27.728_023_3 / (4.842_525_68 - z) - 1.490_129_07 * z))
        as u32;

    unsafe { transmute_copy::<u32, f32>(&i) }
}

pub fn fast_log2(x: f32) -> f32 {
    use std::mem::transmute_copy;

    let xi = unsafe { transmute_copy::<f32, u32>(&x) };
    let y = xi as f32 * 1.192_092_895_507_812_5e-7;
    let mx = unsafe { transmute_copy::<u32, f32>(&((xi & 0x007F_FFFF) | 0x3f00_0000)) };

    y - 124.225_514_99 - 1.498_030_302 * mx - 1.725_879_99 / (0.352_088_706_8 + mx)
}

pub fn fast_exp(p: f32) -> f32 {
    fast_pow2(f32::consts::LOG2_E * p)
}

pub fn fast_pow(x: f32, p: f32) -> f32 {
    fast_pow2(p * fast_log2(x))
}

pub fn faster_pow2(p: f32) -> f32 {
    use std::mem::transmute_copy;

    let clipp: f32 = if p < -126.0 { -126.0 } else { p };
    let i: u32 = ((1 << 23) as f32 * (clipp + 126.942_695_04)) as u32;

    unsafe { transmute_copy::<u32, f32>(&i) }
}

pub fn faster_exp(p: f32) -> f32 {
    faster_pow2(f32::consts::LOG2_E * p)
}

// End of adapted code
//----------------------------------------------------------------

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

    #[test]
    fn log2_64_test() {
        assert_eq!(0, log2_64(0));

        for i in 0..64 {
            assert_eq!(i, log2_64(1 << i));
        }

        for i in 8..64 {
            assert_eq!(i, log2_64((1 << i) + 227));
        }

        for i in 16..64 {
            assert_eq!(i, log2_64((1 << i) + 56369));
        }

        for i in 32..64 {
            assert_eq!(i, log2_64((1 << i) + 2514124923));
        }
    }
}
