#ifndef INTERPOLATOR_H
#define INTERPOLATOR_H

#include "../Assertions.h"
#include "../Exception.h"

#include <vector>
#include <algorithm>

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

public:

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

protected:

	/** all added entries, SORTED by their key */
	std::vector<InterpolatorEntry> 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(InterpolatorEntry(key, value));
	}

	/** get the interpolated value for the given key */
	Value get(const Key key) const {

		const int idx2 = getIdxAfter(key);
		if (idx2 == 0)	{return entries.front().value;}			// key < everything within the list
		if (idx2 == -1)	{return entries.back().value;}			// key > everything within the list

		// interpolate
		const int idx1 = (idx2 > 0) ? (idx2 - 1) : (idx2);
		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;

	}

protected:

	/** get the nearest index for the given key */
	int getIdxAfter(const Key key) const {

		// compare key against one entry
		static auto comp = [] (const Key& key, const InterpolatorEntry& ie1) {return key < ie1.key;};

		// find the next entry > than key
		const auto it = std::upper_bound(entries.begin(), entries.end(), key, comp);

		// none found? (key > everything within the list)
		return (it == entries.end()) ? (-1) : (it-entries.begin());
		//if (it == entries.end()) {throw Exception("key not found: " + std::to_string(key));}
		//return it - entries.begin();

	}

};

#endif // INTERPOLATOR_H