Initial commit

1 files changed, 199 insertions, 0 deletions

diff --git a/src/devices/math_device.rs b/src/devices/math_device.rs
new file mode 100644
index 0000000..e7043b9
--- /dev/null
+++ b/src/devices/math_device.rs
@@ -0,0 +1,199 @@
+use crate::*;
+
+const ANGLE_SCALE: f64 = 10430.378350470453; // 65536 / 2π
+
+
+pub struct MathDevice {
+    pub x: u16,
+    pub y: u16,
+    pub r: u16,
+    pub t: u16,
+    pub x_read: Option<u16>,
+    pub y_read: Option<u16>,
+    pub r_read: Option<u16>,
+    pub t_read: Option<u16>,
+    pub prod: Option<(u16, u16)>,  // (low, high)
+    pub quot: Option<u16>,
+    pub rem:  Option<u16>,
+}
+
+
+impl Device for MathDevice {
+    fn read(&mut self, port: u8) -> u8 {
+        match port {
+            0x0 => read_h!(self.x()),
+            0x1 => read_l!(self.x()),
+            0x2 => read_h!(self.y()),
+            0x3 => read_l!(self.y()),
+            0x4 => read_h!(self.r()),
+            0x5 => read_l!(self.r()),
+            0x6 => read_h!(self.t()),
+            0x7 => read_l!(self.t()),
+            0x8 => read_h!(self.prod().1),
+            0x9 => read_l!(self.prod().1),
+            0xA => read_h!(self.prod().0),
+            0xB => read_l!(self.prod().0),
+            0xC => read_h!(self.quot()),
+            0xD => read_l!(self.quot()),
+            0xE => read_h!(self.rem()),
+            0xF => read_l!(self.rem()),
+            _ => unreachable!(),
+        }
+    }
+
+    fn write(&mut self, port: u8, value: u8) -> Option<Signal> {
+        match port {
+            0x0 => { write_h!(self.x, value); self.clear_polar(); },
+            0x1 => { write_l!(self.x, value); self.clear_polar(); },
+            0x2 => { write_h!(self.y, value); self.clear_polar(); },
+            0x3 => { write_l!(self.y, value); self.clear_polar(); },
+            0x4 => { write_h!(self.r, value); self.clear_cartesian(); },
+            0x5 => { write_l!(self.r, value); self.clear_cartesian(); },
+            0x6 => { write_h!(self.t, value); self.clear_cartesian(); },
+            0x7 => { write_l!(self.t, value); self.clear_cartesian(); },
+            0x8 => (),
+            0x9 => (),
+            0xA => (),
+            0xB => (),
+            0xC => (),
+            0xD => (),
+            0xE => (),
+            0xF => (),
+            _ => unreachable!(),
+        };
+        return None;
+    }
+
+    fn wake(&mut self) -> bool {
+        false
+    }
+
+    fn reset(&mut self) {
+        self.x = 0;
+        self.y = 0;
+        self.r = 0;
+        self.t = 0;
+        self.clear_cartesian();
+        self.clear_polar();
+    }
+}
+
+
+impl MathDevice {
+    pub fn new() -> Self {
+        Self {
+            x: 0,
+            y: 0,
+            r: 0,
+            t: 0,
+            x_read: None,
+            y_read: None,
+            r_read: None,
+            t_read: None,
+            prod: None,
+            quot: None,
+            rem:  None,
+        }
+    }
+
+    pub fn clear_cartesian(&mut self) {
+        self.x_read = None;
+        self.y_read = None;
+    }
+
+    pub fn clear_polar(&mut self) {
+        self.r_read = None;
+        self.t_read = None;
+        self.prod = None;
+        self.quot = None;
+        self.rem = None;
+    }
+
+    pub fn x(&mut self) -> u16 {
+        match self.x_read {
+            Some(x) => x,
+            None => {
+                let r = self.r as f64;
+                let t = self.t as f64;
+                let angle = t / ANGLE_SCALE;
+                let x = angle.cos() * r;
+                self.x_read = match x > (i16::MIN as f64) && x < (i16::MAX as f64) {
+                    true => Some(x as i16 as u16),
+                    false => Some(0),
+                };
+                self.x_read.unwrap()
+            }
+        }
+    }
+
+    pub fn y(&mut self) -> u16 {
+        match self.y_read {
+            Some(y) => y,
+            None => {
+                let r = self.r as f64;
+                let t = self.t as f64;
+                let angle = t / ANGLE_SCALE;
+                let y = angle.sin() * r;
+                self.y_read = match y > (i16::MIN as f64) && y < (i16::MAX as f64) {
+                    true => Some(y as i16 as u16),
+                    false => Some(0),
+                };
+                self.y_read.unwrap()
+            }
+        }
+    }
+
+    pub fn r(&mut self) -> u16 {
+        match self.r_read {
+            Some(r) => r,
+            None => {
+                let sum = (self.x as f64).powi(2) + (self.y as f64).powi(2);
+                self.r_read = Some(sum.sqrt() as u16);
+                self.r_read.unwrap()
+            }
+        }
+    }
+
+    pub fn t(&mut self) -> u16 {
+        match self.t_read {
+            Some(t) => t,
+            None => {
+                let x = self.x as i16 as f64;
+                let y = self.x as i16 as f64;
+                let angle = f64::atan2(y, x) * ANGLE_SCALE;
+                self.t_read = Some(angle as i16 as u16);
+                self.t_read.unwrap()
+            }
+        }
+    }
+
+    pub fn prod(&mut self) -> (u16, u16) {
+        match self.prod {
+            Some(prod) => prod,
+            None => {
+                self.prod = Some(self.x.widening_mul(self.y));
+                self.prod.unwrap()
+            }
+        }
+    }
+
+    pub fn quot(&mut self) -> u16 {
+        match self.quot {
+            Some(quot) => quot,
+            None => {
+                self.quot = Some(self.x.checked_div(self.y).unwrap_or(0));
+                self.quot.unwrap()
+            }
+        }
+    }
+
+    pub fn rem(&mut self) -> u16 {
+        match self.rem {
+            Some(rem) => rem,
+            None => {
+                self.rem = Some(self.x.checked_rem(self.y).unwrap_or(0));
+                self.rem.unwrap()
+            }
+        }
+    }
+}