/*
* moda.h
*
* Created on: 24 июля 2013 г.
* Author: klen
* URL: klen.org
*/

#ifndef __MODA_H__
#define __MODA_H__


#include <vector>
#include <algorithm>

//---------------------------------------------------------------------------------------------
template <typename T> class TModaFilterContainer
{
private:
protected:
public:

std::vector<T> vals; // входная последовательность
std::vector<T> gist; // копия входной последовательности для сортировки и поиска моды

TModaFilterContainer () {};
TModaFilterContainer (size_t size) { vals.resize(size); };
~TModaFilterContainer() {};
void Add(T val) {vals.push_front(val); }
void Remove () {vals.pop_back(); }
void Clear() { vals.clear(); }
void Resize(size_t size) {vals.resize(size); }
void Update(T val) { vals.pop_back(); vals.insert(vals.begin(), val); }
};

//---------------------------------------------------------------------------
template <typename T> class TMedianFilterSort : public TModaFilterContainer<T>
{
public:
TMedianFilterSort():TModaFilterContainer<T>() {};
TMedianFilterSort(size_t size):TModaFilterContainer<T>(size) {};
~TMedianFilterSort() {};

T Find()
{
this->gist = this->vals;
div_t divresult;
divresult = div( this->gist.size() , 2 );
size_t n = divresult.quot;
sort(this->gist.begin(), this->gist.end());
if ( divresult.rem )
return this->vals[n];
else
return (this->vals[n-1] + this->vals[n]) / static_cast<T>(2);
}

};
//--------------------------------------------------------------------------
template <typename T> class TMedianFilterNth : public TModaFilterContainer<T>
{
public:
TMedianFilterNth():TModaFilterContainer<T>() {};
TMedianFilterNth(size_t size):TModaFilterContainer<T>(size) {};
~TMedianFilterNth() {};

T Find()
{
this->gist = this->vals;
size_t n = this->gist.size() / 2;
nth_element(this->gist.begin(), this->gist.begin() + n, this->gist.end());
return this->gist[n];
}
};

// EXAMPLE
#if 0

#include "median.h"

typedef TMedianFilterNth<int16_t> median_t;
volatile int16_t mediana;


median_t m(8);

mediana = m.Find();

m.Update(8);
mediana = m.Find();
m.Update(9);
mediana = m.Find();
m.Update(10);
mediana = m.Find();
m.Update(8);
mediana = m.Find();
m.Update(10);
mediana = m.Find();
m.Update(11);
mediana = m.Find();
m.Update(0);
mediana = m.Find();
m.Update(1);
mediana = m.Find();
m.Update(2);
mediana = m.Find();
m.Clear();

// результат работы:

0 0 0 0 0 0 0 0
mediana:0
8 0 0 0 0 0 0 0
mediana:0
9 8 0 0 0 0 0 0
mediana:0
10 9 8 0 0 0 0 0
mediana:0
8 10 9 8 0 0 0 0
mediana:8
10 8 10 9 8 0 0 0
mediana:8
11 10 8 10 9 8 0 0
mediana:9
0 11 10 8 10 9 8 0
mediana:9
1 0 11 10 8 10 9 8
mediana:9
2 1 0 11 10 8 10 9
mediana:9

#endif


#endif /* __MODA_H__ */

размер буфера                                    добавления элемента в фифо+ вычисление медианы(максимальное значение)
4                                                                                             4,2мкс
16                                                                                             10мкс
32                                                                                            16мкс
128                                                                                           56мкс

datatype median( datatype array, int length)  // массив и его длина
{
  int  slit = length/2;
  for( int i=0; i < length; i++)
  { int s1=0, s2=0;
    datatype val = array[i];
    for( int j=0; j < length; j++)
    { if( array[j] < val)
      { if( ++s1 > slit) goto nexti;
      }
      else if( array[j] > val)
      { if( ++s2 > slit) goto nexti;
      }
    }
    return val;
nexti:
  }
  return 0;  // чистая формальность, досюда исполнение никогда не доходит
}

template <typename Sample, unsigned int WIDTH>
class FilterAverage
{
public:
    FilterAverage(void)
    {
        for (unsigned index = 0; index < WIDTH; ++index) samples[index] = 0;
        totalSum = 0;
        currentSampleIndex = 0;
    }

    Sample evaluate(Sample sample)
    {
        totalSum = totalSum - samples[currentSampleIndex];
        samples[currentSampleIndex] = sample;
        totalSum = totalSum + sample;
        Sample averageSample = totalSum / WIDTH;
        if (++currentSampleIndex >= WIDTH) currentSampleIndex = 0;
        return(averageSample);
    }

    unsigned int getWidth(void) const { return(WIDTH); }

private:
    Sample        samples[WIDTH];
    Sample        totalSum; // TODO: нужен соотв. тип, иначе переполнение при счете !!!!!
    unsigned int    currentSampleIndex;
};

FilterAverage<signed short, TEMPERATURE_FILTER_WIDTH> temperatureFilter;

signed short temperature = temperatureFilter.evaluate(temperature);

static void bubble_sort(uint16_t* p, size_t n)
{
    if (n<2) return;
    
    for (size_t i=0; i<n; i++)
    {
        for (size_t j=1; j<(n-i); j++)
        {
            if (p[j-1]>p[j])
            {
                uint16_t temp = p[j-1];
                p[j-1] = p[j];
                p[j] = temp;
            }
        }
    }
}

uint16_t median_filter(uint16_t* p, size_t n)  // портит исходный массив
{
    if (!n)  return (0);

    bubble_sort(p, n);

    size_t i = n/2;

    if (n&1)  return (p[i]);                      // Число элементов массива нечетное.
    else      return (((uint32_t)p[i-1]+p[i])/2); // Число элементов массива четное.
}

int Median(const int *data, size_t size)
{
    size_t middle = size / 2;
    int min, max;
    min = max = data[0];
    for(size_t i = 0; i < size; i++)
    {
        if(data[i] > max)
            max = data[i];
        if(data[i] < min)
            min = data[i];
    }

    int medianCandidate;
    size_t less, more;
    do
    {
        medianCandidate = (min + max) / 2;
        more = less = 0;
        for(size_t i = 0; i < size; i++)
        {
            more += data[i] > medianCandidate;
            less += data[i] < medianCandidate;
        }
        if(more < middle)
        {            
            max = medianCandidate;
        }
        else
        {
            min = medianCandidate;
        }
    }
    while(max - min > 1);
    return medianCandidate;
}