adjusted code and test-cases for fixed-freq-interpolater

math/FixedFrequencyInterpolator.h

@@ -4,21 +4,36 @@
 #include "Interpolator.h"
 #include "../data/Timestamp.h"
 #include <functional>
+#include "../Assertions.h"
 
 /**
  * performs interpolation on provided sensor data
  * for sensors that do not send their data at a fixed frequency
- * or to adjust the frequency of the data provided by a sensor
+ * or to adjust the frequency of the data provided by a sensor.
+ * supports both: up and downscaling
  */
 template <typename Entry> class FixedFrequencyInterpolator {
 
 private:
 
+	bool first = true;
+
 	/** how often to provide output data */
 	Timestamp outputInterval;
 
-	Timestamp lastTS;
-	Entry lastEntry;
+	/** track the timestamps when to output the next value */
+	Timestamp nextOutput;
+
+	/** combine value-at-timestamp */
+	struct Timed {
+		Timestamp ts;
+		Entry entry;
+		Timed(const Timestamp ts, const Entry entry) : ts(ts), entry(entry) {;}
+		Timed() : ts(), entry() {;}
+	};
+
+	Timed last;
+
 
 
 public:
@@ -32,31 +47,74 @@ public:
 	void add(const Timestamp ts, const Entry& entry, std::function<void(Timestamp, const Entry&)> callback) {
 
 		// first value?
-		if (lastTS.isZero()) {
-			lastTS = ts;
-			lastEntry = entry;
+		if (first) {
+			first = false;
+			last = Timed(ts, entry);
+			nextOutput = last.ts;// + outputInterval;
 			return;
 		}
 
-		// available timeslice between last and current entry
-		const Timestamp diff = ts - lastTS;
+		// new value
+		Timed cur(ts, entry);
 
-		// the region to output
-		const uint64_t start = std::ceil(lastTS.ms() / (float)outputInterval.ms() + 0.00001f) * outputInterval.ms();
-		const uint64_t end = std::floor(ts.ms() / (float)outputInterval.ms()) * outputInterval.ms();
+		// no time-change? -> ignore
+		if (last.ts == cur.ts) {return;}
 
-		// perform output
-		for (uint64_t t = start; t <= end; t += outputInterval.ms()) {
+		// output needed?
+		//if (nextOutput > last.ts) {
 
-			const float percent = (t - lastTS.ms()) / (float) (diff.ms());
-			const Entry res = lastEntry + (entry - lastEntry) * percent;
+			// available timeslice ybetween last and current entry
+			const Timestamp diff = cur.ts - last.ts;
 
-			callback(Timestamp::fromMS(t), res);
+			// create outputs
+			while(nextOutput < cur.ts) {
 
-		}
+				// interpolation rate
+				const float percent = (nextOutput.ms() - last.ts.ms()) / (float) (diff.ms());
 
-		lastEntry = entry;
-		lastTS = ts;
+				// sanity checks
+				Assert::isNotNaN(percent, "detected NaN for interpolation");
+				Assert::isTrue(percent <= 1, "detected an invalid interpolation value");
+
+				const Entry res = last.entry + (cur.entry - last.entry) * percent;
+				callback(nextOutput, res);
+
+				// increment
+				nextOutput += outputInterval;
+
+			}
+
+		//}
+
+		// next step
+		last = cur;
+
+
+
+//		// available timeslice between last and current entry
+//		const Timestamp diff = ts - lastTS;
+
+//		// the region to output
+//		const uint64_t start = std::ceil(lastTS.ms() / (float)outputInterval.ms() + 0.00001f) * outputInterval.ms();
+//		const uint64_t end = std::floor(ts.ms() / (float)outputInterval.ms()) * outputInterval.ms();
+
+//		// perform output
+//		for (uint64_t t = start; t <= end; t += outputInterval.ms()) {
+
+//			const float percent = (t - lastTS.ms()) / (float) (diff.ms());
+
+//			// sanity checks
+//			Assert::isNotNaN(percent, "detected NaN for interpolation");
+//			Assert::isTrue(percent <= 1, "detected an invalid interpolation value");
+
+//			const Entry res = lastEntry + (entry - lastEntry) * percent;
+
+//			callback(Timestamp::fromMS(t), res);
+
+//		}
+
+//		lastEntry = entry;
+//		lastTS = ts;
 
 	}