From bcfad0cc522755f6a60b9c7b68cfcebde242fcfd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Franti=C5=A1ek=20Boh=C3=A1=C4=8Dek?=
 <fandabohacek@gmail.com>
Date: Fri, 28 Jul 2023 21:24:54 +0200
Subject: [PATCH] feat(src): add module for linear interpolation

---
 src/linear_interpolation.rs | 70 +++++++++++++++++++++++++++++++++++++
 1 file changed, 70 insertions(+)
 create mode 100644 src/linear_interpolation.rs

diff --git a/src/linear_interpolation.rs b/src/linear_interpolation.rs
new file mode 100644
index 0000000000000000000000000000000000000000..fe12aae396a3f7022f079128d7e51555aae18ba8
--- /dev/null
+++ b/src/linear_interpolation.rs
@@ -0,0 +1,70 @@
+use alloc::vec::Vec;
+
+pub struct Point<Position, Value> {
+    position: Position,
+    value: Value,
+}
+
+impl<Position, Value> Point<Position, Value> {
+    pub fn new(position: Position, value: Value) -> Self {
+        Self { position, value }
+    }
+}
+
+impl<Position: Copy, Value: Copy> Point<Position, Value> {
+    pub fn deconstruct(&self) -> (Position, Value) {
+        (self.position, self.value)
+    }
+}
+
+pub struct LinearInterpolation<Position, Value> {
+    points: Vec<Point<Position, Value>>,
+}
+
+impl<Position: Ord + PartialOrd + Copy, Value: Copy> LinearInterpolation<Position, Value> {
+    pub fn closest_position_indices(&self, position: Position) -> Option<(usize, usize)> {
+        let mut closest_lower_index = 0;
+        let mut closest_upper_index = self.points.len() - 1;
+        let p = &self.points;
+        for i in 1..self.points.len() {
+            let curr_position = p[i].position;
+            if curr_position > p[closest_lower_index].position && curr_position <= position {
+                closest_lower_index = i;
+            }
+
+            if curr_position < p[closest_upper_index].position && curr_position >= position {
+                closest_upper_index = i;
+            }
+        }
+
+        if p[closest_lower_index].position <= position
+            && p[closest_upper_index].position >= position
+        {
+            Some((closest_lower_index, closest_upper_index))
+        } else {
+            None
+        }
+    }
+}
+
+impl LinearInterpolation<u16, u16> {
+    pub fn new(points: Vec<Point<u16, u16>>) -> Self {
+        Self { points }
+    }
+
+    pub fn interpolate(&self, position: u16) -> Option<u16> {
+        let (lower, upper) = self.closest_position_indices(position)?;
+
+        if lower == upper {
+            return Some(self.points[lower].value);
+        }
+
+        let (lower, upper) = (&self.points[lower], &self.points[upper]);
+
+        let diff = upper.position - lower.position;
+        let value_diff = upper.value as i32 - lower.value as i32;
+        let position_relative = (position - lower.position) as f32 / diff as f32;
+
+        Some((lower.value as f32 + (value_diff as f32 * position_relative)) as u16)
+    }
+}