diff --git a/cmds/statsd/Android.mk b/cmds/statsd/Android.mk
index 6c6e037ea38ac..b09d961cd61bd 100644
--- a/cmds/statsd/Android.mk
+++ b/cmds/statsd/Android.mk
@@ -89,6 +89,7 @@ LOCAL_C_INCLUDES += $(LOCAL_PATH)/src
 LOCAL_SRC_FILES := \
     ../../core/java/android/os/IStatsManager.aidl \
     src/StatsService.cpp \
+    tests/indexed_priority_queue_test.cpp \
     src/LogEntryPrinter.cpp \
     src/LogReader.cpp \
     tests/LogReader_test.cpp \
diff --git a/cmds/statsd/src/AnomalyMonitor.cpp b/cmds/statsd/src/AnomalyMonitor.cpp
index 8338d5026b460..d73de957881aa 100644
--- a/cmds/statsd/src/AnomalyMonitor.cpp
+++ b/cmds/statsd/src/AnomalyMonitor.cpp
@@ -59,8 +59,7 @@ void AnomalyMonitor::remove(sp<const AnomalyAlarm> alarm) {
     }
     std::lock_guard<std::mutex> lock(mLock);
     if (DEBUG) ALOGD("Removing alarm with time %u", alarm->timestampSec);
-    // TODO: make priority queue able to have items removed from it !!!
-    // mPq.remove(alarm);
+    mPq.remove(alarm);
     if (mPq.empty()) {
         if (DEBUG) ALOGD("Queue is empty. Cancel any alarm.");
         mRegisteredAlarmTimeSec = 0;
diff --git a/cmds/statsd/src/AnomalyMonitor.h b/cmds/statsd/src/AnomalyMonitor.h
index 63b806416c7e7..5418cf0dc5525 100644
--- a/cmds/statsd/src/AnomalyMonitor.h
+++ b/cmds/statsd/src/AnomalyMonitor.h
@@ -17,6 +17,7 @@
 #ifndef ANOMALY_MONITOR_H
 #define ANOMALY_MONITOR_H
 
+#include <indexed_priority_queue.h>
 #include <android/os/IStatsCompanionService.h>
 #include <utils/RefBase.h>
 
@@ -40,11 +41,10 @@ struct AnomalyAlarm : public RefBase {
 
     const uint32_t timestampSec;
 
-    /** AnomalyAlarm a is higher priority than b if its timestamp is sooner. */
-    struct Comparator {
-        bool operator()(sp<const AnomalyAlarm> a, sp<const AnomalyAlarm> b) const
-        {
-            return (a->timestampSec > b->timestampSec);
+    /** AnomalyAlarm a is smaller (higher priority) than b if its timestamp is sooner. */
+    struct SmallerTimestamp {
+        bool operator()(sp<const AnomalyAlarm> a, sp<const AnomalyAlarm> b) const {
+            return (a->timestampSec < b->timestampSec);
         }
     };
 };
@@ -98,10 +98,7 @@ class AnomalyMonitor {
     /**
      * Priority queue of alarms, prioritized by soonest alarm.timestampSec.
      */
-    // TODO: use a priority queue from which elements can be removed
-    //       Can I use the one in toolchain/gcc/gcc-4.9/include/fibheap.h?
-    std::priority_queue<sp<const AnomalyAlarm>, std::vector<sp<const AnomalyAlarm>>,
-                        AnomalyAlarm::Comparator> mPq;
+    indexed_priority_queue<AnomalyAlarm, AnomalyAlarm::SmallerTimestamp> mPq;
 
     /**
      * Binder interface for communicating with StatsCompanionService.
diff --git a/cmds/statsd/src/indexed_priority_queue.h b/cmds/statsd/src/indexed_priority_queue.h
new file mode 100644
index 0000000000000..d302f855b1bf3
--- /dev/null
+++ b/cmds/statsd/src/indexed_priority_queue.h
@@ -0,0 +1,195 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef STATSD_INDEXED_PRIORITY_QUEUE_H
+#define STATSD_INDEXED_PRIORITY_QUEUE_H
+
+// ALOGE can be called from this file. If header loaded by another class, use their LOG_TAG instead.
+#ifndef LOG_TAG
+#define LOG_TAG "statsd(indexed_priority_queue)"
+#endif //LOG_TAG
+
+#include <cutils/log.h>
+#include <unordered_map>
+#include <utils/RefBase.h>
+#include <vector>
+
+using namespace android;
+
+namespace statsd {
+
+/** Defines a hash function for sp<AA>, returning the hash of the underlying pointer. */
+template <class AA>
+struct SpHash {
+    size_t operator()(const sp<const AA>& k) const {
+        return std::hash<const AA*>()(k.get());
+    }
+};
+
+/**
+ * Min priority queue for generic type AA.
+ * Unlike a regular priority queue, this class is also capable of removing interior elements.
+ * @tparam Comparator must implement [bool operator()(sp<const AA> a, sp<const AA> b)], returning
+ *    whether a should be closer to the top of the queue than b.
+ */
+template <class AA, class Comparator>
+class indexed_priority_queue {
+ public:
+    indexed_priority_queue();
+    /** Adds a into the priority queue. If already present or a==nullptr, does nothing. */
+    void push(sp<const AA> a);
+    /** Removes a from the priority queue. If not present or a==nullptr, does nothing. */
+    void remove(sp<const AA> a);
+    /** Removes all elements. */
+    void clear();
+    /** Returns whether priority queue contains a (not just a copy of a, but a itself). */
+    bool contains(sp<const AA> a) const;
+    /** Returns min element. Returns nullptr iff empty(). */
+    sp<const AA> top() const;
+    /** Returns number of elements in priority queue. */
+    size_t size() const { return pq.size() - 1; } // pq is 1-indexed
+    /** Returns true iff priority queue is empty. */
+    bool empty() const { return size() < 1; }
+
+ private:
+    /** Vector representing a min-heap (1-indexed, with nullptr at 0). */
+    std::vector<sp<const AA>> pq;
+    /** Mapping of each element in pq to its index in pq (i.e. the inverse of a=pq[i]). */
+    std::unordered_map<sp<const AA>, size_t, SpHash<AA>> indices;
+
+    void init();
+    void sift_up(size_t idx);
+    void sift_down(size_t idx);
+    /** Returns whether pq[idx1] is considered higher than pq[idx2], according to Comparator. */
+    bool higher(size_t idx1, size_t idx2) const;
+    void swap_indices(size_t i, size_t j);
+};
+
+// Implementation must be done in this file due to use of template.
+
+template <class AA, class Comparator>
+indexed_priority_queue<AA,Comparator>::indexed_priority_queue() {
+    init();
+}
+
+template <class AA, class Comparator>
+void indexed_priority_queue<AA,Comparator>::push(sp<const AA> a) {
+    if (a == nullptr) return;
+    if (contains(a)) return;
+    pq.push_back(a);
+    size_t idx = size(); // index of last element since 1-indexed
+    indices.insert({a, idx});
+    sift_up(idx); // get the pq back in order
+}
+
+template <class AA, class Comparator>
+void indexed_priority_queue<AA,Comparator>::remove(sp<const AA> a) {
+    if (a == nullptr) return;
+    if (!contains(a)) return;
+    size_t idx = indices[a];
+    if (idx >= pq.size()) {
+        ALOGE("indexed_priority_queue: Invalid index in map of indices.");
+        return;
+    }
+    if (idx == size()) { // if a is the last element, i.e. at index idx == size() == (pq.size()-1)
+        pq.pop_back();
+        indices.erase(a);
+        return;
+    }
+    // move last element (guaranteed not to be at idx) to idx, then delete a
+    sp<const AA> last_a = pq.back();
+    pq[idx] = last_a;
+    pq.pop_back();
+    indices[last_a] = idx;
+    indices.erase(a);
+
+    // get the heap back in order (since the element at idx is not in order)
+    sift_up(idx);
+    sift_down(idx);
+}
+
+template <class AA, class Comparator>
+void indexed_priority_queue<AA,Comparator>::clear() {
+    pq.clear();
+    indices.clear();
+    init();
+}
+
+template <class AA, class Comparator>
+sp<const AA> indexed_priority_queue<AA,Comparator>::top() const {
+    if (empty()) return nullptr;
+    return pq[1];
+}
+
+template <class AA, class Comparator>
+void indexed_priority_queue<AA,Comparator>::init() {
+    pq.push_back(nullptr); // so that pq is 1-indexed.
+    indices.insert({nullptr, 0}); // just to be consistent with pq.
+}
+
+template <class AA, class Comparator>
+void indexed_priority_queue<AA,Comparator>::sift_up(size_t idx) {
+    while (idx > 1) {
+        size_t parent = idx/2;
+        if (higher(idx, parent)) swap_indices(idx, parent);
+        else break;
+        idx = parent;
+    }
+}
+
+template <class AA, class Comparator>
+void indexed_priority_queue<AA,Comparator>::sift_down(size_t idx) {
+    while (2*idx <= size()) {
+        size_t child = 2 * idx;
+        if (child < size() && higher(child+1, child)) child++;
+        if (higher(child, idx)) swap_indices(child, idx);
+        else break;
+        idx = child;
+    }
+}
+
+template <class AA, class Comparator>
+bool indexed_priority_queue<AA,Comparator>::higher(size_t idx1, size_t idx2) const {
+    if (!(0u < idx1 && idx1 < pq.size() && 0u < idx2 && idx2 < pq.size())) {
+        ALOGE("indexed_priority_queue: Attempting to access invalid index");
+        return false; // got to do something.
+    }
+    return Comparator()(pq[idx1], pq[idx2]);
+}
+
+template <class AA, class Comparator>
+bool indexed_priority_queue<AA,Comparator>::contains(sp<const AA> a) const {
+    if (a == nullptr) return false; // publicly, we pretend that nullptr is not actually in pq.
+    return indices.count(a) > 0;
+}
+
+template <class AA, class Comparator>
+void indexed_priority_queue<AA,Comparator>::swap_indices(size_t i, size_t j) {
+    if (!(0u < i && i < pq.size() && 0u < j && j < pq.size())) {
+        ALOGE("indexed_priority_queue: Attempting to swap invalid index");
+        return;
+    }
+    sp<const AA> val_i = pq[i];
+    sp<const AA> val_j = pq[j];
+    pq[i] = val_j;
+    pq[j] = val_i;
+    indices[val_i] = j;
+    indices[val_j] = i;
+}
+
+} // namespace statsd
+
+#endif //STATSD_INDEXED_PRIORITY_QUEUE_H
diff --git a/cmds/statsd/tests/indexed_priority_queue_test.cpp b/cmds/statsd/tests/indexed_priority_queue_test.cpp
new file mode 100644
index 0000000000000..a679128dd37e8
--- /dev/null
+++ b/cmds/statsd/tests/indexed_priority_queue_test.cpp
@@ -0,0 +1,188 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "../src/indexed_priority_queue.h"
+
+#include <gtest/gtest.h>
+
+using namespace statsd;
+
+/** struct for template in indexed_priority_queue */
+struct AATest : public RefBase {
+    AATest(uint32_t val) : val(val) {
+    }
+
+    const int val;
+
+    struct Smaller {
+        bool operator()(const sp<const AATest> a, const sp<const AATest> b) const {
+            return (a->val < b->val);
+        }
+    };
+};
+
+#ifdef __ANDROID__
+TEST(indexed_priority_queue, empty_and_size) {
+    indexed_priority_queue<AATest, AATest::Smaller> ipq;
+    sp<const AATest> aa4 = new AATest{4};
+    sp<const AATest> aa8 = new AATest{8};
+
+    EXPECT_EQ(0u, ipq.size());
+    EXPECT_TRUE(ipq.empty());
+
+    ipq.push(aa4);
+    EXPECT_EQ(1u, ipq.size());
+    EXPECT_FALSE(ipq.empty());
+
+    ipq.push(aa8);
+    EXPECT_EQ(2u, ipq.size());
+    EXPECT_FALSE(ipq.empty());
+
+    ipq.remove(aa4);
+    EXPECT_EQ(1u, ipq.size());
+    EXPECT_FALSE(ipq.empty());
+
+    ipq.remove(aa8);
+    EXPECT_EQ(0u, ipq.size());
+    EXPECT_TRUE(ipq.empty());
+}
+
+TEST(indexed_priority_queue, top) {
+    indexed_priority_queue<AATest, AATest::Smaller> ipq;
+    sp<const AATest> aa2 = new AATest{2};
+    sp<const AATest> aa4 = new AATest{4};
+    sp<const AATest> aa8 = new AATest{8};
+    sp<const AATest> aa12 = new AATest{12};
+    sp<const AATest> aa16 = new AATest{16};
+    sp<const AATest> aa20 = new AATest{20};
+
+    EXPECT_EQ(ipq.top(), nullptr);
+
+    // add 8, 4, 12
+    ipq.push(aa8);
+    EXPECT_EQ(ipq.top(), aa8);
+
+    ipq.push(aa12);
+    EXPECT_EQ(ipq.top(), aa8);
+
+    ipq.push(aa4);
+    EXPECT_EQ(ipq.top(), aa4);
+
+    // remove 12, 4
+    ipq.remove(aa12);
+    EXPECT_EQ(ipq.top(), aa4);
+
+    ipq.remove(aa4);
+    EXPECT_EQ(ipq.top(), aa8);
+
+    // add 16, 2, 20
+    ipq.push(aa16);
+    EXPECT_EQ(ipq.top(), aa8);
+
+    ipq.push(aa2);
+    EXPECT_EQ(ipq.top(), aa2);
+
+    ipq.push(aa20);
+    EXPECT_EQ(ipq.top(), aa2);
+
+    // remove 2, 20, 16, 8
+    ipq.remove(aa2);
+    EXPECT_EQ(ipq.top(), aa8);
+
+    ipq.remove(aa20);
+    EXPECT_EQ(ipq.top(), aa8);
+
+    ipq.remove(aa16);
+    EXPECT_EQ(ipq.top(), aa8);
+
+    ipq.remove(aa8);
+    EXPECT_EQ(ipq.top(), nullptr);
+}
+
+TEST(indexed_priority_queue, push_same_aa) {
+    indexed_priority_queue<AATest, AATest::Smaller> ipq;
+    sp<const AATest> aa4_a = new AATest{4};
+    sp<const AATest> aa4_b = new AATest{4};
+
+    ipq.push(aa4_a);
+    EXPECT_EQ(1u, ipq.size());
+    EXPECT_TRUE(ipq.contains(aa4_a));
+    EXPECT_FALSE(ipq.contains(aa4_b));
+
+    ipq.push(aa4_a);
+    EXPECT_EQ(1u, ipq.size());
+    EXPECT_TRUE(ipq.contains(aa4_a));
+    EXPECT_FALSE(ipq.contains(aa4_b));
+
+    ipq.push(aa4_b);
+    EXPECT_EQ(2u, ipq.size());
+    EXPECT_TRUE(ipq.contains(aa4_a));
+    EXPECT_TRUE(ipq.contains(aa4_b));
+}
+
+
+TEST(indexed_priority_queue, remove_nonexistant) {
+    indexed_priority_queue<AATest, AATest::Smaller> ipq;
+    sp<const AATest> aa4 = new AATest{4};
+    sp<const AATest> aa5 = new AATest{5};
+
+    ipq.push(aa4);
+    ipq.remove(aa5);
+    EXPECT_EQ(1u, ipq.size());
+    EXPECT_TRUE(ipq.contains(aa4));
+    EXPECT_FALSE(ipq.contains(aa5));
+}
+
+TEST(indexed_priority_queue, remove_same_aa) {
+    indexed_priority_queue<AATest, AATest::Smaller> ipq;
+    sp<const AATest> aa4_a = new AATest{4};
+    sp<const AATest> aa4_b = new AATest{4};
+
+    ipq.push(aa4_a);
+    ipq.push(aa4_b);
+    EXPECT_EQ(2u, ipq.size());
+    EXPECT_TRUE(ipq.contains(aa4_a));
+    EXPECT_TRUE(ipq.contains(aa4_b));
+
+    ipq.remove(aa4_b);
+    EXPECT_EQ(1u, ipq.size());
+    EXPECT_TRUE(ipq.contains(aa4_a));
+    EXPECT_FALSE(ipq.contains(aa4_b));
+
+    ipq.remove(aa4_a);
+    EXPECT_EQ(0u, ipq.size());
+    EXPECT_FALSE(ipq.contains(aa4_a));
+    EXPECT_FALSE(ipq.contains(aa4_b));
+}
+
+TEST(indexed_priority_queue, nulls) {
+    indexed_priority_queue<AATest, AATest::Smaller> ipq;
+
+    EXPECT_TRUE(ipq.empty());
+    EXPECT_FALSE(ipq.contains(nullptr));
+
+    ipq.push(nullptr);
+    EXPECT_TRUE(ipq.empty());
+    EXPECT_FALSE(ipq.contains(nullptr));
+
+    ipq.remove(nullptr);
+    EXPECT_TRUE(ipq.empty());
+    EXPECT_FALSE(ipq.contains(nullptr));
+}
+
+#else
+GTEST_LOG_(INFO) << "This test does nothing.\n";
+#endif