Interpolation.h

#ifndef INTERPOLATION_H_
#define INTERPOLATION_H_

#include <map>
#include <algorithm>

template<class V>
class Interpolated {
protected:
  V value;

public:
  bool isInterpolated;

public:
  Interpolated(V v, bool isIntpl = true):
    value(v), isInterpolated(isIntpl) {}

  V& operator*() {
    return value;
  }

  V* operator->() {
    return &value;
  }

  bool operator==(const Interpolated<V>& other) {
    return other.value == value && other.isInterpolated == isInterpolated;
  }

  bool operator!=(const Interpolated<V>& other) {
    return other.value != value || other.isInterpolated != isInterpolated;
  }
};

template<class Pair, class Key>
class PairLess {
public:
  bool operator()(const Pair& p, const Key& v) {
    return p.first < v;
  }

  bool operator()(const Key& v, const Pair& p) {
    return v < p.first;
  }
};

template<class Key, class V, class Pair, class InputIterator>
class NextSmaller {
public:
  typedef Interpolated<V> interpolated;
protected:

  PairLess<Pair, Key> comp;
  bool continueOutOfRange;
  V outOfRangeVal;
public:
  NextSmaller():
    continueOutOfRange(true) {}

  NextSmaller(V oorv):
    continueOutOfRange(false), outOfRangeVal(oorv) {}

  void setOutOfRangeVal(V oorv){
    outOfRangeVal = oorv;
  }

  interpolated operator()(const InputIterator& first,
              const InputIterator& last,
              const Key& pos) const{

    InputIterator right = std::upper_bound(first, last, pos, comp);

    return operator()(first, last, pos, right);
  }

  interpolated operator()(const InputIterator& first,
              const InputIterator& last,
              const Key& pos,
              InputIterator upperBound) const{
    if(first == last){
      if(continueOutOfRange)
        return interpolated(V());
      else
        return interpolated(outOfRangeVal);
    }

    if(upperBound == first){
      if(continueOutOfRange)
        return interpolated(upperBound->second);
      else
        return interpolated(outOfRangeVal);
    }

    upperBound--;
    if(upperBound->first == pos)
      return interpolated(upperBound->second, false);

    return interpolated(upperBound->second);
  }
};

template<class Key, class V, class Pair, class InputIterator>
class Nearest {
public:
  typedef Interpolated<V> interpolated;
protected:

  PairLess<Pair, Key> comp;
  bool continueOutOfRange;
  V outOfRangeVal;
public:
  Nearest():
    continueOutOfRange(true) {}

  Nearest(V oorv):
    continueOutOfRange(false), outOfRangeVal(oorv) {}

  void setOutOfRangeVal(V oorv){
    outOfRangeVal = oorv;
  }

  interpolated operator()(const InputIterator& first,
              const InputIterator& last,
              const Key& pos) const{

    InputIterator right = std::upper_bound(first, last, pos, comp);

    return operator()(first, last, pos, right);
  }

  interpolated operator()(const InputIterator& first,
              const InputIterator& last,
              const Key& pos,
              InputIterator upperBound) const{
    if(first == last){
      if(continueOutOfRange)
        return interpolated(V());
      else
        return interpolated(outOfRangeVal);
    }

    if(upperBound == first){
      if(continueOutOfRange)
        return interpolated(upperBound->second);
      else
        return interpolated(outOfRangeVal);
    }

    InputIterator left = upperBound;
    --left;

    if(left->first == pos)
      return interpolated(left->second, false);

    InputIterator right = upperBound;

    if(right == last){
      if(continueOutOfRange)
        return interpolated(left->second);
      else
        return interpolated(outOfRangeVal);
    }

    if(pos - left->first < right->first - pos)
      return interpolated(left->second);
    else
      return interpolated(right->second);
  }
};

template<class Key, class V, class Pair, class InputIterator>
class Linear {
public:
  typedef Interpolated<V> interpolated;
protected:

  PairLess<Pair, Key> comp;
  bool continueOutOfRange;
  V outOfRangeVal;
public:
  Linear():
    continueOutOfRange(true) {}

  Linear(V oorv):
    continueOutOfRange(false), outOfRangeVal(oorv) {}

  void setOutOfRangeVal(V oorv){
    outOfRangeVal = oorv;
  }

  static double linearInterpolation(const Key& t,
                    const Key& t0, const Key& t1,
                    const V& v0, const V& v1){
    return v0 + (v1 - v0) * (double)((t - t0) / (t1 - t0));
  }

  interpolated operator()(const InputIterator& first,
              const InputIterator& last,
              const Key& pos) const{

    InputIterator right = std::upper_bound(first, last, pos, comp);

    return operator()(first, last, pos, right);
  }

  interpolated operator()(const InputIterator& first,
              const InputIterator& last,
              const Key& pos,
              InputIterator upperBound) const{
    if(first == last){
      if(continueOutOfRange)
        return interpolated(V());
      else
        return interpolated(outOfRangeVal);
    }

    if(upperBound == first){
      if(continueOutOfRange)
        return interpolated(upperBound->second);
      else
        return interpolated(outOfRangeVal);
    }

    InputIterator right = upperBound;
    InputIterator left = --upperBound;

    if(left->first == pos)
      return interpolated(left->second, false);

    if(right == last){
      if(continueOutOfRange)
        return interpolated(left->second);
      else
        return interpolated(outOfRangeVal);
    }

    return interpolated(linearInterpolation(pos, left->first, right->first, left->second, right->second));
  }
};

template<class Key, class V,
     class Pair = const typename std::map<Key, V>::value_type,
     class Iterator = const typename std::map<Key, V>::const_iterator,
     class Interpolator = NextSmaller<Key, V, Pair, Iterator> >
class ConstInterpolateableIterator {
public:
  typedef Interpolated<V> interpolated;
protected:
  Iterator first;
  Iterator last;

  Iterator right;

  Key position;
  const Interpolator& interpolate;

  PairLess<Pair, Key> comp;
public:
  ConstInterpolateableIterator(Iterator first, Iterator last, const Interpolator& intpl):
    first(first), last(last), right(first), position(), interpolate(intpl), comp(){

    jumpToBegin();
  }

  bool operator==(const ConstInterpolateableIterator& other) {
    return position == other.position && right == other.right;
  }

  void jumpTo(const Key& pos) {
    if(pos == position)
      return;

    if(first != last)
      right = std::upper_bound(first, last, pos, comp);

    position = pos;
  }

  void jumpToBegin() {
    right = first;
    if(right != last) {
      position = right->first;
      right++;
    } else {
      position = Key();
    }
  }

  void iterateTo(const Key& pos) {
    if(pos == position)
      return;

    while(right != last && !(pos < right->first))
      right++;

    position = pos;
  }

  void next() {
    if(hasNext()) {
      position = right->first;
      right++;
    } else
      position += 1;
  }

  Key getNextPosition(){
    if(hasNext())
      return right->first;
    else
      return position + 1;
  }

  bool inRange() const{
    if(first == last)
      return false;

    Iterator tail = last;

    return !(position < first->first) && !((--tail)->first < position);
  }

  bool hasNext() const{
    return right != last;
  }

  interpolated getValue() const{

    return interpolate(first, last, position, right);
  }

  interpolated getNextValue() const{
    if(right == last)
      return interpolate(first, last, position + 1, right);
    else{
      Iterator tmp = right;
      return interpolate(first, last, right->first, ++tmp);
    }
  }

  Key getPosition() const{
    return position;
  }

};

template<class Key, class V,
     class Container = std::map<Key, V>,
     class Interpolator = NextSmaller<Key, V, typename Container::value_type, typename Container::const_iterator> >
class InterpolateableIterator:public ConstInterpolateableIterator<Key, V,
                                    typename Container::value_type,
                                    typename Container::iterator,
                                    Interpolator>
{
protected:
  typedef typename Container::value_type value_type;
  typedef typename Container::iterator Iterator;
  typedef ConstInterpolateableIterator<Key,V,
                      value_type,
                      Iterator,
                      Interpolator> BaseClassType;

  Container& cont;

public:
  InterpolateableIterator(Container& cont, const Interpolator& intpl):
    BaseClassType(cont.begin(), cont.end(), intpl), cont(cont) {}

  void setValue(const V& value) {
    //container is empty or position is smaller first entry
    if(this->right == this->first) {
      //insert new entry before first entry and store new entry as new first
      this->first = cont.insert(this->first, value_type(this->position, value));

    } else {
      Iterator left = this->right;
      left--;
      if(left->first == this->position) {
        left->second = value;
      } else {
        cont.insert(this->right, value_type(this->position, value));
      }
    }
  }
};

template<class Key, class V,
     class Interpolator = NextSmaller<Key, V, typename std::map<Key, V>::value_type, typename std::map<Key, V>::const_iterator> >
class InterpolateableMap:public std::map<Key, V> {
public:
  typedef std::map<Key, V> map_type;
  typedef typename map_type::const_iterator const_iterator;
  typedef typename map_type::iterator iterator;
  typedef typename map_type::value_type value_type;
  typedef InterpolateableIterator<Key, V, map_type, Interpolator> intpl_iterator;
  typedef ConstInterpolateableIterator<Key, V, const map_type,
                      typename map_type::const_iterator,
                      Interpolator> const_intpl_iterator;
  typedef Interpolated<V> interpolated;

protected:

  Interpolator interpolate;
public:

  InterpolateableMap() {}

  InterpolateableMap(V oorv):
    interpolate(oorv) {}

  void setOutOfRangeVal(V oorv){
    interpolate.setOutOfRangeVal(oorv);
  }

  interpolated getIntplValue(const Key& pos) const {
    //if(this->empty())
    //  return interpolated(V(), true);

    const_iterator it = this->upper_bound(pos);

    //if(it != this->end() && it->first == pos)
    //  return interpolated(it->second, false);


    return interpolate(this->begin(), this->end(), pos, it);
  }

  const_intpl_iterator findIntpl(const Key& pos) const{
    const_intpl_iterator it(this->begin(), this->end(), interpolate);

    it.jumpTo(pos);

    return it;
  }

  const_intpl_iterator beginIntpl() const{
    const_intpl_iterator it(this->begin(), this->end, interpolate);

    return it;
  }

  intpl_iterator findIntpl(const Key& pos) {
    intpl_iterator it(*this, interpolate);

    it.jumpTo(pos);

    return it;
  }

  intpl_iterator beginIntpl() {
    intpl_iterator it(*this, interpolate);

    return it;
  }
};
#endif /*INTERPOLATION_H_*/