IndexedPriorityQueue.h

#ifndef INDEXED_PRIORITY_QUEUE_H
#define INDEXED_PRIORITY_QUEUE_H

#include <set>
#include <map>

template <class IT,class PT>
class IndexedPriorityQueue
{
 public:
  typedef std::pair<PT,IT> value_type;
  typedef typename std::set<std::pair<PT,IT> >::iterator iterator;
  typedef typename std::set<std::pair<PT,IT> >::const_iterator const_iterator;

  iterator begin() { return q.begin(); }
  const_iterator begin() const { return q.begin(); }
  iterator end() { return q.end(); }
  const iterator end() const { return q.end(); }
  const value_type& front() const { return *q.begin(); }
  const value_type& back() const { const_iterator i=q.end(); --i; return *i; }
  bool empty() const { return q.empty(); }
  void clear() { q.clear(); indices.clear(); }
  size_t size() const { return q.size(); }
  const value_type& top() const { return *q.begin(); }
  void erase(iterator i) {
    size_t n=indices.erase(i->second);
    Assert(n==1);
    q.erase(i);
  }
  void pop() { 
    erase(q.begin());
  }
  iterator insert(const value_type& item) {
    iterator i=q.insert(item).first;
    Assert(indices.count(item.second)==0);
    indices[item.second] = i;
    return i;
  }
  template <class InputIterator>
  void insert(InputIterator i,InputIterator j) {
    for(InputIterator k=i;k!=j;++k)
      insert(*k);
  }
  iterator insert(const IT& index,const PT& p) {
    iterator i=q.insert(value_type(p,index)).first;
    Assert(indices.count(index)==0);
    indices[index] = i;
    return i;
  }
  iterator find(const IT& index) {
    auto i=indices.find(index);
    if(i==indices.end()) return end();
    return i->second;
  }
  iterator refresh(const IT& index,const PT& p) {
    iterator i=find(index);
    if(i != q.end()) q.erase(i);
    i = q.insert(value_type(p,index)).first;
    indices[index] = i;
    return i;
  }
  bool is_valid() const {
    for(auto i=indices.begin();i!=indices.end();++i) {
      if(i->second->second != i->first) return false;
    }
    return true;
  }

 private:
  std::map<IT,iterator> indices;
  std::set<std::pair<PT,IT> > q;
};

#endif