Chris@4: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
Chris@4: 
Chris@4: #ifndef _MEDIAN_H_
Chris@4: #define _MEDIAN_H_
Chris@4: 
Chris@4: #include <algorithm>
Chris@4: #include <cassert>
Chris@9: #include <cmath>
Chris@9: #include <iostream>
Chris@4: 
Chris@4: template <typename T>
Chris@4: class MedianFilter
Chris@4: {
Chris@4: public:
Chris@4:     MedianFilter(int size, float percentile = 50.f) :
Chris@4: 	m_size(size),
Chris@4: 	m_frame(new T[size]),
Chris@4: 	m_sorted(new T[size]),
Chris@4: 	m_sortend(m_sorted + size - 1) {
Chris@4:         setPercentile(percentile);
Chris@4: 	reset();
Chris@4:     }
Chris@4: 
Chris@4:     ~MedianFilter() { 
Chris@4: 	delete[] m_frame;
Chris@4: 	delete[] m_sorted;
Chris@4:     }
Chris@4: 
Chris@4:     void setPercentile(float p) {
Chris@4:         m_index = int((m_size * p) / 100.f);
Chris@4:         if (m_index >= m_size) m_index = m_size-1;
Chris@4:         if (m_index < 0) m_index = 0;
Chris@4:     }
Chris@4: 
Chris@4:     void push(T value) {
Chris@7:         if (value != value) return; // nan
Chris@4: 	drop(m_frame[0]);
Chris@4: 	const int sz1 = m_size-1;
Chris@4: 	for (int i = 0; i < sz1; ++i) m_frame[i] = m_frame[i+1];
Chris@4: 	m_frame[m_size-1] = value;
Chris@4: 	put(value);
Chris@4:     }
Chris@4: 
Chris@4:     T get() const {
Chris@4: 	return m_sorted[m_index];
Chris@4:     }
Chris@4: 
Chris@4:     T getAt(float percentile) {
Chris@4: 	int ix = int((m_size * percentile) / 100.f);
Chris@4:         if (ix >= m_size) ix = m_size-1;
Chris@4:         if (ix < 0) ix = 0;
Chris@4: 	return m_sorted[ix];
Chris@4:     }
Chris@4: 
Chris@4:     void reset() {
Chris@4: 	for (int i = 0; i < m_size; ++i) m_frame[i] = 0;
Chris@4: 	for (int i = 0; i < m_size; ++i) m_sorted[i] = 0;
Chris@4:     }
Chris@4: 
Chris@4: private:
Chris@4:     const int m_size;
Chris@4:     T *const m_frame;
Chris@4:     T *const m_sorted;
Chris@4:     T *const m_sortend;
Chris@4:     int m_index;
Chris@4: 
Chris@4:     void put(T value) {
Chris@4: 	// precondition: m_sorted contains m_size-1 values, packed at start
Chris@4: 	// postcondition: m_sorted contains m_size values, one of which is value
Chris@4: 	T *point = std::lower_bound(m_sorted, m_sortend, value);
Chris@4: 	const int n = m_sortend - point;
Chris@4: 	for (int i = n; i > 0; --i) point[i] = point[i-1];
Chris@4: 	*point = value;
Chris@4:     }
Chris@4: 
Chris@4:     void drop(T value) {
Chris@4: 	// precondition: m_sorted contains m_size values, one of which is value
Chris@4: 	// postcondition: m_sorted contains m_size-1 values, packed at start
Chris@4: 	T *point = std::lower_bound(m_sorted, m_sortend + 1, value);
Chris@4: 	if (*point != value) {
Chris@4: 	    std::cerr << "WARNING: MedianFilter::drop: *point is " << *point
Chris@4: 		      << ", expected " << value << std::endl;
Chris@4: 	}
Chris@4: 	const int n = m_sortend - point;
Chris@4: 	for (int i = 0; i < n; ++i) point[i] = point[i+1];
Chris@4: 	*m_sortend = T(0);
Chris@4:     }
Chris@4: };
Chris@4: 
Chris@4: #endif
Chris@4: