//! Conversion functions to and from the formats used in Grid-EYE sensors.
//! One thing to note is that since the Grid-EYE stores temperatures as integer multiples of 0.25
//! or 0.0625, these values can be *exactly* represented in IEEE 754 floating point (the
//! denominators of the fractions are powers of two (2^2 and 2^4 respectively)).
//!
//! # Temperature Formats
//!
//! Temperature values are stored as 12-bits across two registers. In this case, the eight least
//! significant bits are stored in the `*Lower` register, with the remainder in the `*Upper`
//! register. There are two ways temperatures are encoded:
//!
//! ## Pixel Temperatures
//! The temperature of camera pixels (which is also used in setting the interrupt threshold values)
//! is a 12-bit, two's complement integer scaled to 0.25°C. Ex: `0x000` is 0°, `0x001`
//! is 0.25°, and `0xFFF` is -0.25°.
//!
//! ## Themristor Temperatures
//! This is a 12-bit value, with the most significant bit being a sign bit, and the eleven
//! other bits an integer representing 0.0625°C increments. Ex: `0x000` is 0°, `0x001`
//! is 0.0625°, and `0xFFF` is -127.9375°.

use core::convert::TryInto;

const PIXEL_SCALE: f32 = 0.25;
const THERMISTOR_SCALE: f32 = 0.0625;

/// Convert a 12-bit two's complement integer to a single precision floating point value.
#[inline]
pub fn pixel_temperature_to_float(bit_temp: &[u8; 2]) -> f32 {
    // This operation is performed 64 times for each frame, so I've tried to keep it as fast as
    // possible (and probably also do some premature optimization). This implementation replaced an
    // earlier one that's closer to the reverse of [float_to_pixel_temperature]. This
    // implementation is branchless and the compiler can auto-vectorize it on x86_64 and aarch64
    // (see https://godbolt.org/z/eeET8WW8v for examples across x86_64 and aarch64). Interestingly,
    // 32-bit ARM doesn't use NEON instructions even on CPUs that support them.
    // It takes advantage of Rust defining right shifts on signed types as *arithmetic* shifts
    // (which extend the sign bit across).
    let signed = i16::from_le_bytes(*bit_temp);
    // Logical shift left by 4 to get the sign bit in the left most place. Then *arithmetic* shift
    // right 4, extending the sign bit.
    (((signed << 4) >> 4) as f32) * PIXEL_SCALE
}

/// Convert a buffer containing the pixel data as read from the Grid-EYE to a buffer of floats.
///
/// The Grid-EYE has an 8x8 resolution, with each pixel represented by 12 bits across two bytes,
/// little-endian. The data is *not* packed, the extra bits can be ignored.
pub fn pixel_buffer_to_float_buffer(src: &[u8; 128]) -> [f32; 64] {
    let mut dest = [0_f32; 64];
    for (chunk, out_temp) in src.chunks_exact(2).zip(dest.iter_mut()) {
        *out_temp = pixel_temperature_to_float(
            chunk
                .try_into()
                .expect("the chunk slice to have exactly two elements"),
        );
    }
    dest
}

#[cfg(test)]
mod pixel_to_float_tests {
    // Test values taken from section 4-11-11 of the AGM88** data sheet. Also note that the
    // Grid-EYE's registers present multi-byte values as little-endian.
    use super::pixel_temperature_to_float;

    #[test]
    fn zero() {
        let result = pixel_temperature_to_float(&[0, 0]);
        assert_eq!(result, 0_f32);
    }

    #[test]
    fn positive() {
        let expected: [f32; 3] = [0.25, 25.0, 125.0];
        let inputs = [[0x01, 0x00], [0x64, 0x00], [0xF4, 0x01]];
        let results = inputs.iter().map(pixel_temperature_to_float);
        for pair in results.zip(expected.iter()) {
            let result = pair.0;
            assert_eq!(result, *pair.1);
        }
    }

    #[test]
    fn negative() {
        let expected: [f32; 3] = [-0.25, -25.0, -55.0];
        let inputs = [[0xFF, 0x0F], [0x9C, 0x0F], [0x24, 0x0F]];
        let results = inputs.iter().map(pixel_temperature_to_float);
        for pair in results.zip(expected.iter()) {
            let result = pair.0;
            assert_eq!(result, *pair.1);
        }
    }
}

pub fn thermistor_temperature_to_float(buffer: &[u8; 2]) -> f32 {
    let combined = u16::from_le_bytes(*buffer);
    let mut scaled = (combined & 0x7FF) as f32 * THERMISTOR_SCALE;
    if combined & 0x800 == 0x800 {
        scaled *= -1_f32;
    }
    scaled
}

#[cfg(test)]
mod thermistor_to_float_tests {
    // Test values taken from section 4-11-9 of the AGM88** data sheet. Also note that the
    // Grid-EYE's registers present multi-byte values as little-endian.
    use super::thermistor_temperature_to_float;

    #[test]
    fn zero() {
        let result = thermistor_temperature_to_float(&[0, 0]);
        assert_eq!(result, 0_f32);
    }

    #[test]
    fn positive() {
        let expected: [f32; 3] = [0.25, 25.0, 127.9375];
        let inputs = [[0x04, 0x00], [0x90, 0x01], [0xFF, 0x07]];
        let results = inputs.iter().map(thermistor_temperature_to_float);
        for pair in results.zip(expected.iter()) {
            let result = pair.0;
            assert_eq!(result, *pair.1);
        }
    }

    #[test]
    fn negative() {
        let expected: [f32; 2] = [-0.25, -59.6875];
        let inputs = [[0x04, 0x08], [0xBB, 0x0B]];
        let results = inputs.iter().map(thermistor_temperature_to_float);
        for pair in results.zip(expected.iter()) {
            let result = pair.0;
            assert_eq!(result, *pair.1);
        }
    }
}

pub fn float_to_pixel_temperature(float_temp: &f32) -> Option<[u8; 2]> {
    let is_negative: bool = *float_temp < 0_f32;
    let mut bit_temp: u16 = (float_temp.copysign(0_f32) / PIXEL_SCALE) as u16;
    if is_negative {
        if bit_temp > 0x800 {
            return None;
        }
        // convert to 12-bit two's complement
        bit_temp = (!bit_temp & 0xFFF) + 1;
    } else if bit_temp > 0x7FF {
        return None;
    }
    Some(bit_temp.to_le_bytes())
}

#[cfg(test)]
mod float_to_pixel_tests {
    // Test values taken from section 4-11-11 of the AGM88** data sheet. Also note that the
    // Grid-EYE's registers present multi-byte values as little-endian.
    use super::float_to_pixel_temperature;

    #[test]
    fn zero() {
        let result = float_to_pixel_temperature(&0.0);
        assert!(result.is_some());
        assert_eq!(result.unwrap(), [0, 0]);
    }

    #[test]
    fn positive() {
        let expected = [[0x1, 0x0], [0x64, 0x0], [0xF4, 0x1]];
        let inputs: [f32; 3] = [0.25, 25.0, 125.0];
        let results = inputs.iter().map(|x| float_to_pixel_temperature(x));
        for pair in results.zip(expected.iter()) {
            let result = pair.0;
            assert!(result.is_some());
            assert_eq!(result.unwrap(), *pair.1);
        }
    }

    #[test]
    fn negative() {
        let expected = [[0xFF, 0xF], [0x9C, 0xF], [0x24, 0xF]];
        let inputs: [f32; 3] = [-0.25, -25.0, -55.0];
        let results = inputs.iter().map(|x| float_to_pixel_temperature(x));
        for pair in results.zip(expected.iter()) {
            let result = pair.0;
            assert!(result.is_some());
            assert_eq!(result.unwrap(), *pair.1);
        }
    }

    #[test]
    fn limits() {
        // The upper limit is 511.75
        assert_ne!(float_to_pixel_temperature(&511.75_f32), None);
        assert_eq!(float_to_pixel_temperature(&512_f32), None);
        // And the lower limit is 512
        assert_ne!(float_to_pixel_temperature(&-512_f32), None);
        assert_eq!(float_to_pixel_temperature(&-512.25_f32), None);
    }
}