From d2c78e453e5fdd0f92357ff982ad3bb4e509c722 Mon Sep 17 00:00:00 2001
From: Ahmet Inan <xdsopl@gmail.com>
Date: Mon, 15 Apr 2024 15:30:05 +0200
Subject: [PATCH] oops

---
 .../java/xdsopl/robot36/FrequencyModulation.java   | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/app/src/main/java/xdsopl/robot36/FrequencyModulation.java b/app/src/main/java/xdsopl/robot36/FrequencyModulation.java
index 7f2c287..cb85245 100644
--- a/app/src/main/java/xdsopl/robot36/FrequencyModulation.java
+++ b/app/src/main/java/xdsopl/robot36/FrequencyModulation.java
@@ -9,12 +9,22 @@ package xdsopl.robot36;
 public class FrequencyModulation {
 	private float prev;
 	private final float scale;
+	private final float Pi, TwoPi;
 	FrequencyModulation(float bandwidth, float sampleRate) {
-		scale = sampleRate / (bandwidth * (float) Math.PI);
+		this.Pi = (float) Math.PI;
+		this.TwoPi = 2 * this.Pi;
+		scale = sampleRate / (bandwidth * Pi);
+	}
+	private float wrap(float value) {
+		if (value < -Pi)
+			return value + TwoPi;
+		if (value > Pi)
+			return value - TwoPi;
+		return value;
 	}
 	float demod(Complex input) {
 		float phase = input.arg();
-		float delta = (phase - prev) % (float) Math.PI;
+		float delta = wrap(phase - prev);
 		prev = phase;
 		return scale * delta;
 	}