From 71c6e52f4d23f18fce74f0abbd20cda575c0c207 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Fri, 12 Feb 2021 12:03:53 +0100
Subject: [PATCH] complex: add some traits

---
 dsp/src/complex.rs | 49 ++++++++++++++++++++++++++++++++++++++++++++++
 dsp/src/lockin.rs  | 11 ++++++-----
 2 files changed, 55 insertions(+), 5 deletions(-)

diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs
index ff3feb5..3edc0cc 100644
--- a/dsp/src/complex.rs
+++ b/dsp/src/complex.rs
@@ -1,8 +1,20 @@
+use core::convert::From;
+use core::ops::Mul;
+
 use super::{atan2, cossin};
 
 #[derive(Copy, Clone, Default, PartialEq, Debug)]
 pub struct Complex<T>(pub T, pub T);
 
+impl<T: Copy> Complex<T> {
+    pub fn map<F>(&self, func: F) -> Self
+    where
+        F: Fn(T) -> T,
+    {
+        Complex(func(self.0), func(self.1))
+    }
+}
+
 impl Complex<i32> {
     /// Return a Complex on the unit circle given an angle.
     ///
@@ -79,3 +91,40 @@ impl Complex<i32> {
         atan2(self.1, self.0)
     }
 }
+
+impl Mul for Complex<i32> {
+    type Output = Self;
+
+    fn mul(self, other: Self) -> Self {
+        let a = self.0 as i64;
+        let b = self.1 as i64;
+        let c = other.0 as i64;
+        let d = other.1 as i64;
+        Complex(
+            ((a * c - b * d + (1 << 31)) >> 32) as i32,
+            ((b * c + a * d + (1 << 31)) >> 32) as i32,
+        )
+    }
+}
+
+impl Mul<i32> for Complex<i32> {
+    type Output = Self;
+
+    fn mul(self, other: i32) -> Self {
+        Complex(
+            ((other as i64 * self.0 as i64 + (1 << 31)) >> 32) as i32,
+            ((other as i64 * self.1 as i64 + (1 << 31)) >> 32) as i32,
+        )
+    }
+}
+
+impl Mul<i16> for Complex<i32> {
+    type Output = Self;
+
+    fn mul(self, other: i16) -> Self {
+        Complex(
+            (other as i32 * (self.0 >> 16) + (1 << 15)) >> 16,
+            (other as i32 * (self.1 >> 16) + (1 << 15)) >> 16,
+        )
+    }
+}
diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index 100a845..b2f38c1 100644
--- a/dsp/src/lockin.rs
+++ b/dsp/src/lockin.rs
@@ -21,13 +21,14 @@ impl Lockin {
         // Get the LO signal for demodulation.
         let lo = Complex::from_angle(phase);
 
-        // Mix with the LO signal, filter with the IIR lowpass,
+        // Mix with the LO signal
+        let mix = lo * sample;
+
+        // Filter with the IIR lowpass,
         // return IQ (in-phase and quadrature) data.
         Complex(
-            self.state[0]
-                .update((sample as i32 * (lo.0 >> 16) + (1 << 15)) >> 16, k),
-            self.state[1]
-                .update((sample as i32 * (lo.1 >> 16) + (1 << 15)) >> 16, k),
+            self.state[0].update(mix.0, k),
+            self.state[1].update(mix.1, k),
         )
     }
 }