#ifndef INTERPOLATOR_H
#define INTERPOLATOR_H

#include "../Assertions.h"

/**
 * interpolate between two adjacent values based on their key
 */
template <typename Key, typename Value> class Interpolator {

public:

	/** value at key */
	struct Entry {
		Key key;
		Value value;
		Entry(const Key key, const Value& pos) : key(key), value(value) {;}
	};

protected:

	/** all added entries, SORTED by their key */
	std::vector<Entry> entries;

public:

	/** add a new value with its key. entries must be added in sorted order! */
	void add(const Key key, const Value& value) {
		Assert::isTrue (entries.empty() || entries.back().key < key, "entries must be ordered!");
		entries.push_back(Entry(key, value));
	}

	/** get the interpolated value for the given key */
	Value get(const Key key) const {
		const int idx1 = getIdx(key);
		const int idx2 = idx1+1;
		const float percent = (key - entries[idx1].key) / (float) (entries[idx2].key - entries[idx1].key);
		return entries[idx1].value + (entries[idx2].value - entries[idx1].value) * percent;
	}

	/** get the nearest index for the given key */
	int getIdx(const Key key) const {
		// TODO: use O(log(n)) search here!
		for (size_t i = 0; i < entries.size(); ++i) {
			if (entries[i].key > key) {return i-1;}
		}
		throw "should not happen";
	}

};

#endif // INTERPOLATOR_H