From 58c0fb45392fc83321013d5054cba23cf1c65a98 Mon Sep 17 00:00:00 2001
From: Andrey <rusefillc@gmail.com>
Date: Wed, 24 Nov 2021 21:53:32 -0500
Subject: [PATCH] MAP phase sensing #3544

---
 unit_tests/tests/trigger/test_map_cam.cpp | 68 +++++++++++++++++++++++
 1 file changed, 68 insertions(+)
 create mode 100644 unit_tests/tests/trigger/test_map_cam.cpp

diff --git a/unit_tests/tests/trigger/test_map_cam.cpp b/unit_tests/tests/trigger/test_map_cam.cpp
new file mode 100644
index 0000000000..a2c538a3b3
--- /dev/null
+++ b/unit_tests/tests/trigger/test_map_cam.cpp
@@ -0,0 +1,68 @@
+
+
+#include "pch.h"
+
+#define MAP_CAM_BUFFER 8
+
+static int getZigZag(int index) {
+	index = index % 1000;
+
+	if (index < 400) {
+		// going up from 0 to 400
+		return index;
+	}
+	if (index < 500) {
+		// going down from 400 to 300
+		return 800 - index;
+	}
+	if (index < 600) {
+		// going up from 300 to 400
+		return index - 200;
+	}
+	// going down from 400 to 0
+	return 1000 - index;
+}
+
+struct MapState {
+	float current, previous, prevPrevious;
+	cyclic_buffer<float, MAP_CAM_BUFFER> mapBuffer;
+
+	void add(float value) {
+		// rotate state
+		prevPrevious = previous;
+		previous = current;
+
+		// add new value
+		mapBuffer.add(value);
+		current = mapBuffer.sum(MAP_CAM_BUFFER);
+	}
+
+	bool isPeak() {
+		return previous > prevPrevious && previous >= current;
+	}
+};
+
+TEST(trigger, map_cam) {
+
+	MapState state;
+
+	int i = 0;
+	for (;i<404;i++) {
+		state.add(getZigZag(i));
+
+		if (state.mapBuffer.getCount() > MAP_CAM_BUFFER) {
+			ASSERT_FALSE(state.isPeak()) << "At " << i;
+		}
+	}
+
+	state.add(getZigZag(i));
+	ASSERT_TRUE(state.isPeak()) << "At " << i;
+
+	for (;i<604;i++) {
+		state.add(getZigZag(i));
+		ASSERT_FALSE(state.isPeak()) << "At " << i;
+	}
+
+	state.add(getZigZag(i));
+	ASSERT_TRUE(state.isPeak()) << "At " << i;
+}