fixed some plotting issues

modified step detection

math/Delay.h

@@ -0,0 +1,47 @@
+#ifndef DELAY_H
+#define DELAY_H
+
+#include <cstdint>
+#include <vector>
+
+template <typename T> class Delay {
+
+private:
+
+	size_t size;
+
+	/** up to "size" elements */
+	std::vector<T> values;
+
+public:
+
+	/** ctor */
+	Delay(const int size) : size(size) {;}
+
+	/** add a new value */
+	void add(const T val) {
+
+		// add new value
+		values.push_back(val);
+
+		// remove old ones
+		while(values.size() > size) {
+			values.erase(values.begin());
+		}
+
+	}
+
+	/** get the delayed value */
+	T get() const {
+		return values.front();
+	}
+
+	/** delay output valid? */
+	bool isValid() const {
+		return values.size() == size;
+	}
+
+
+};
+
+#endif // DELAY_H