#ifndef FIXEDFREQUENCYINTERPOLATOR_H
#define FIXEDFREQUENCYINTERPOLATOR_H

#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.
 * supports both: up and downscaling
 */
template <typename Entry> class FixedFrequencyInterpolator {

private:

	bool first = true;

	/** how often to provide output data */
	Timestamp outputInterval;

	/** 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:

	/** ctor with the desired output inteval */
	FixedFrequencyInterpolator(const Timestamp outputInterval) : outputInterval(outputInterval) {

	}

	/** add a new sensor entry. the callback is called for every interpolated output */
	void add(const Timestamp ts, const Entry& entry, std::function<void(Timestamp, const Entry&)> callback) {

		// first value?
		if (first) {
			first = false;
			last = Timed(ts, entry);
			nextOutput = last.ts;// + outputInterval;
			return;
		}

		// new value
		Timed cur(ts, entry);

		// no time-change? -> ignore
		if (last.ts == cur.ts) {return;}

		// output needed?
		//if (nextOutput > last.ts) {

			// available timeslice ybetween last and current entry
			const Timestamp diff = cur.ts - last.ts;

			// create outputs
			while(nextOutput < cur.ts) {

				// interpolation rate
				const float percent = (nextOutput.finest() - last.ts.finest()) / (float) (diff.finest());

				// 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;

	}

};

#endif // FIXEDFREQUENCYINTERPOLATOR_H