1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149

use std::fs::File;
use std::io::Read;
use std::io::Cursor;

use byteorder::{ BigEndian, ReadBytesExt };

use crate::task::TaskHandle;

/// A SensorLookupTable represents a monotonic increasing function from
/// unsigned 12 bit numbers to 64 bit floating point numbers.
/// It also defines that functions inverse.
/// 
/// Each index i in the y_values identifies the point (i*step_size, y_values[i])
#[derive(Debug)]
pub struct SensorLookupTable {
    /// 
    y_values: Vec<f64>,
    /// 
    step_size: u16
}

pub const UPPER_12: u16 = (1 << 12) - 1;

impl SensorLookupTable {
    // TODO from_y_values(y_values: Vec<f64>) -> Option<Self>
    // TODO validate that y values are increasing

    pub fn from_file(task: TaskHandle, file: &mut File) -> Option<Self> {
        // TODO validate that y values are increasing
        let mut file_data = Vec::new();
        let _len = task.res_error(file.read_to_end(&mut file_data))?;

        let mut cursor = Cursor::new(file_data);

        let n = task.res_error(cursor.read_u16::<BigEndian>())?;

        let mut y_values = Vec::new();

        while let Ok(y_val) = cursor.read_f64::<BigEndian>() {
            y_values.push(y_val);
        }

        if y_values.len() != (n as usize) {
            task.error(format!("LookupTable expected {} points, found {}", n, y_values.len()));
            None
        } else {
            let step_size = UPPER_12 / ((y_values.len()-1) as u16);
            Some(SensorLookupTable { y_values, step_size })
        }
    }

    // TODO implement a "write_to_file" function

    /// Approximates the `F(value)` for the function `F` represented by the lookup table
    /// The input value will be treated as a 12 bit number, the result is None for larger numbers
    /// 
    /// Lookups are performed by finding the nearest points with x values less than or equal to,
    /// and greater than or equal to the provided value, and then linearly interpolating between them.
    /// When the provided value matches the x value of a point, the result is guaranteed to exactly
    /// match the y value of that point with no error.
    pub fn lookup(&self, value: u16) -> Option<f64> {
        if value > UPPER_12 {
            return None;
        }

        let index  = (value / self.step_size) as usize;
        let offset = value % self.step_size;

        let lower_y: f64 = self.y_values[index];
        let upper_y: f64 = self.y_values[index+1];

        if offset == 0 {
            Some(lower_y)
        } else {
            let ratio_a = (offset as f64) / (self.step_size as f64);
            let ratio_b = 1.0 - ratio_a;

            let y_value = (ratio_a * lower_y) + (ratio_b * upper_y);
            Some(y_value)
        }
    }

    // pub fn inverse_nearest(&self, value: f64) -> Option<u16> {
    //     self.y_values.binary_search(&value).ok().map(|idx| idx as u16)
    // }

    // pub fn inverse_linear_interpolated(&self, value: f64) -> Option<u16> {

    // }

    pub fn inverse_lookup(&self, value: f64) -> Option<u16> {
        let mut low_index = 0;
        let mut high_index = self.y_values.len() - 1;

        let mut low_val = self.y_values[low_index];
        let mut high_val = self.y_values[high_index];

        if value < low_val || value > high_val {
            return None;
        }

        // Binary search of the y_values
        while high_index-low_index > 1 {
            let mid_index = (low_index + high_index) / 2;
            let mid_val = self.y_values[mid_index];

            if value <= mid_val {
                high_index = mid_index;
                high_val = mid_val
            } else {
                low_index = mid_index;
                low_val = mid_val;
            }
        }

        let average_val = (low_val + high_val) / 2.0;
        if value < average_val {
            Some(low_index as u16)
        } else {
            Some(high_index as u16)
        }
    }
}

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

    // #[test]
    /// n is the number of points in the table
    /// tables with n = 2^p + 1 for some p are "perfect" tables
    /// each step in a "perfect" table represents an x increase of 2^(12-p) called step_size
    /// when a user requests some value a that is a multiple of the step_size,
    /// then the resulting value should be exactly what was provided
    fn perfect_table_point_lookup_is_exact() {
        // TODO
        // Create multiple different "perfect" tables
        // Assert that lookups at i*step_size for 0 <= i < n returns y_values[i] 
    }

    // #[test]
    /// Verify that a number of arbitrary lookups produce values
    /// between the points it should interpolate between.
    fn lookups_are_between_points() {
        // TODO
    }

    // TODO Test that write_to_file() and from_file() work together
}