use num_traits;
use std::f64;
use std::f32;

/// Get the next valid float in the direction of y
///
/// Base assumptions:
/// self == y -> return y
/// self >= infinity -> return infinity
/// self <= negative infinity -> return negative infinity
///
pub trait NextAfter<T: num_traits::Float> {
    fn next_after(&self, y: &T) -> T;
}

impl<T: num_traits::Float> NextAfter<T> for T {
    fn next_after(&self, y: &T) -> T {
        next_afterf(self, y)
    }
}

/// Return the next valid float from x in the direction of y
///
fn next_afterf<T: num_traits::Float>(x: &T, y: &T) -> T
{
    // If x and y are equal, there is nothing further to do
    if *y == *x {
        return *y;
    }

    // If x is somehow beyond infinity, return infinity
    if *x >= T::infinity() {
        return T::infinity();
    }

    // If x is somehow beyond negative infinity, return negative infinity
    if *x <= T::neg_infinity() {
        return T::neg_infinity();
    }

    // If x is a small number between -1 and 1, we use an algorithm for small numbers
    if T::from(-1.0).unwrap() <= *x && *x <= T::from(1.0).unwrap() {
        return if *y > *x {
            *x + T::epsilon()
        } else {
            *x - T::epsilon()
        }
    }

    // x is larger than 1 or smaller than -1, so decode it into mantissa, exponent, and sign
    let (m, e, s) = x.integer_decode();

    return if *y > *x { // Make x larger
        let adj_m = m + 1;
        T::from(s).unwrap() * T::from(adj_m).unwrap() * T::from(2f64).unwrap().powf(T::from(e).unwrap())
    } else { //Make x smaller
        let adj_m = m - 1;
        T::from(s).unwrap() * T::from(adj_m).unwrap() * T::from(2f64).unwrap().powf(T::from(e).unwrap())
    }
}

/// Decodes a float into a tuple with (mantissa: u64, exponent: i16, and sign: i8)
///
pub trait FloatDecode {
    fn float_decode(&self) -> (u64, i16, i8);
}

impl FloatDecode for f64 {
    // See https://github.com/rust-lang/rust/blob/master/src/libcore/num/dec2flt/rawfp.rs
    fn float_decode(&self) -> (u64, i16, i8) {
        let bits = self.to_bits();
        let sign: i8 = if bits >> 63 == 0 { 1 } else { -1 };
        let mut exponent: i16 = ((bits >> (f64::MANTISSA_DIGITS - 1) as u64) & 0x7ff) as i16;
        let mantissa = if exponent == 0 {
            (bits & 0xfffffffffffff) << 1
        } else {
            (bits & 0xfffffffffffff) | 0x10000000000000
        };
        exponent -= 1023 + (f64::MANTISSA_DIGITS as i16 - 1);
        (mantissa, exponent, sign)
    }
}

impl FloatDecode for f32 {
    // See https://github.com/rust-lang/rust/blob/master/src/libcore/num/dec2flt/rawfp.rs
    fn float_decode(&self) -> (u64, i16, i8) {
        let bits = self.to_bits();
        let sign: i8 = if bits >> 31 == 0 { 1 } else { -1 };
        let mut exponent: i16 = ((bits >> (f32::MANTISSA_DIGITS - 1) as u32) & 0xff) as i16;
        let mantissa =
        if exponent == 0 { (bits & 0x7fffff) << 1 } else { (bits & 0x7fffff) | 0x800000 };
        exponent -= 127 + (f32::MANTISSA_DIGITS as i16 - 1);
        (mantissa as u64, exponent, sign)
    }
}

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

    #[test]
    fn next_larger_than_0() {
        let zero = 0_f64;
        let next = zero.next_after(&std::f64::INFINITY);
        assert_eq!(next, 0.0000000000000002220446049250313_f64);
    }

    #[test]
    fn next_smaller_than_0() {
        let zero = 0_f64;
        let next = zero.next_after(&std::f64::NEG_INFINITY);
        assert_eq!(next, -0.0000000000000002220446049250313);
    }

    #[test]
    fn next_larger_than_a_big_number() {
        let big_num = 16237485966.00000437586943_f64;
        let next = big_num.next_after(&std::f64::INFINITY);
        assert_eq!(next, 16237485966.000006_f64);
    }

    #[test]
    fn next_smaller_than_a_small_number() {
        let small_num = -16237485966.00000437586943_f64;
        let next = small_num.next_after(&std::f64::NEG_INFINITY);
        assert_eq!(next, -16237485966.000002_f64);
    }

    #[test]
    fn next_larger_than_1() {
        let one = 1_f64;
        let next = one.next_after(&std::f64::INFINITY);
        assert_eq!(next, 1_f64 + std::f64::EPSILON);
    }

    #[test]
    fn next_smaller_than_1() {
        let neg_one = -1_f64;
        let next = neg_one.next_after(&std::f64::NEG_INFINITY);
        assert_eq!(next, -1_f64 - std::f64::EPSILON);
    }

    #[test]
    fn next_larger_than_0_f32() {
        let zero = 0_f32;
        let next = zero.next_after(&std::f32::INFINITY);
        assert_eq!(next, 0.00000011920929_f32);
    }

    #[test]
    fn next_smaller_than_0_f32() {
        let zero = 0_f32;
        let next = zero.next_after(&std::f32::NEG_INFINITY);
        assert_eq!(next, -0.00000011920929_f32);
    }

    #[test]
    fn next_larger_than_a_big_number_f32() {
        let big_num = 16237485966.00000437586943_f32;
        let next = big_num.next_after(&std::f32::INFINITY);
        assert_eq!(next, 16237487000_f32);
    }

    #[test]
    fn next_smaller_than_a_small_number_f32() {
        let small_num = -16237485966.00000437586943_f32;
        let next = small_num.next_after(&std::f32::NEG_INFINITY);
        assert_eq!(next, -16237485000_f32);
    }

    #[test]
    fn next_larger_than_1_f32() {
        let one = 1_f32;
        let next = one.next_after(&std::f32::INFINITY);
        assert_eq!(next, 1_f32 + std::f32::EPSILON);
    }

    #[test]
    fn next_smaller_than_1_f32() {
        let neg_one = -1_f32;
        let next = neg_one.next_after(&std::f32::NEG_INFINITY);
        assert_eq!(next, -1_f32 - std::f32::EPSILON);
    }

}