#ifndef FIXEDFREQUENCYINTERPOLATOR_H
#define FIXEDFREQUENCYINTERPOLATOR_H

#include "Interpolator.h"
#include "../data/Timestamp.h"
#include <functional>

/**
 * 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
 */
template <typename Entry> class FixedFrequencyInterpolator {

private:

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

	Timestamp lastTS;
	Entry lastEntry;


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 (lastTS.isZero()) {
			lastTS = ts;
			lastEntry = entry;
			return;
		}

		// 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());
			const Entry res = lastEntry + (entry - lastEntry) * percent;

			callback(Timestamp::fromMS(t), res);

		}

		lastEntry = entry;
		lastTS = ts;

	}

};

#endif // FIXEDFREQUENCYINTERPOLATOR_H