1 files changed, 141 insertions, 0 deletions
diff --git a/src/devices/math_device.rs b/src/devices/math_device.rs
new file mode 100644
index 0000000..015545e
--- /dev/null
+++ b/src/devices/math_device.rs
@@ -0,0 +1,141 @@
+use bedrock_core::*;
+
+
+pub struct MathDevice {
+    pub op1: u16,
+    pub op2: u16,
+
+    pub sqrt: Option<u16>,
+    pub atan: Option<u16>,
+    pub prod: Option<(u16, u16)>,  // (low, high)
+    pub quot: Option<u16>,
+    pub rem:  Option<u16>,
+}
+
+impl MathDevice {
+    pub fn new() -> Self {
+        Self {
+            op1: 0,
+            op2: 0,
+
+            sqrt: None,
+            atan: None,
+            prod: None,
+            quot: None,
+            rem:  None,
+        }
+    }
+
+    pub fn clear(&mut self) {
+        self.sqrt = None;
+        self.atan = None;
+        self.prod = None;
+        self.quot = None;
+        self.rem  = None;
+    }
+
+    pub fn atan(&mut self) -> u16 {
+        match self.atan {
+            Some(atan) => atan,
+            None => {
+                let x = self.op1 as i16 as f64;
+                let y = self.op2 as i16 as f64;
+                const SCALE: f64 = 10430.378350470453; // PI * 32768
+                self.atan = Some((f64::atan2(x, y) * SCALE) as i16 as u16);
+                self.atan.unwrap()
+            }
+        }
+    }
+
+    pub fn sqrt(&mut self) -> u16 {
+        match self.sqrt {
+            Some(sqrt) => sqrt,
+            None => {
+                let input = ((self.op1 as u32) << 16) | (self.op2 as u32);
+                self.sqrt = Some((input as f64).sqrt() as u16);
+                self.sqrt.unwrap()
+            }
+        }
+    }
+
+    pub fn prod(&mut self) -> (u16, u16) {
+        match self.prod {
+            Some(prod) => prod,
+            None => {
+                self.prod = Some(self.op1.widening_mul(self.op2));
+                self.prod.unwrap()
+            }
+        }
+    }
+
+    pub fn quot(&mut self) -> u16 {
+        match self.quot {
+            Some(quot) => quot,
+            None => {
+                self.quot = Some(self.op1.checked_div(self.op2).unwrap_or(0));
+                self.quot.unwrap()
+            }
+        }
+    }
+
+    pub fn rem(&mut self) -> u16 {
+        match self.rem {
+            Some(rem) => rem,
+            None => {
+                self.rem = Some(self.op1.checked_rem(self.op2).unwrap_or(0));
+                self.rem.unwrap()
+            }
+        }
+    }
+}
+
+impl Device for MathDevice {
+    fn read(&mut self, port: u8) -> u8 {
+        match port {
+            0x0 => read_h!(self.op1),
+            0x1 => read_l!(self.op1),
+            0x2 => read_h!(self.op2),
+            0x3 => read_l!(self.op2),
+            0x4 => read_h!(self.sqrt()),
+            0x5 => read_l!(self.sqrt()),
+            0x6 => read_h!(self.atan()),
+            0x7 => read_l!(self.atan()),
+            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.op1, value); self.clear(); },
+            0x1 => { write_l!(self.op1, value); self.clear(); },
+            0x2 => { write_h!(self.op2, value); self.clear(); },
+            0x3 => { write_l!(self.op2, value); self.clear(); },
+            0x4 => (),
+            0x5 => (),
+            0x6 => (),
+            0x7 => (),
+            0x8 => (),
+            0x9 => (),
+            0xa => (),
+            0xb => (),
+            0xc => (),
+            0xd => (),
+            0xe => (),
+            0xf => (),
+            _ => unreachable!(),
+        };
+        return None;
+    }
+
+    fn wake(&mut self) -> bool {
+        false
+    }
+}