File content as of revision 144:a9e7fa1c98d7:

#include "mbed.h"
#include "math.h"
#include "Filter.h"

using namespace std;

CircularBuffer::CircularBuffer(int length) {
    _length = length;
    oldest_index = 0;
    newest_index = -1;
    num = 0;
    sum = 0.0f;

    buf = (float*)malloc(_length * sizeof(float));
    sorted = (float*)malloc(_length * sizeof(float));
    for (int i = 0; i < _length; i++) {
        buf[i] = 0.0f;
        sorted[i] = 0.0f;
    }
}

float &CircularBuffer::at(int index) {
    int actual = oldest_index + index;
    if (actual >= _length) actual -= _length;
    return buf[actual];
}

void CircularBuffer::add(float x) {
    if (num < _length) {
        newest_index++;
        buf[newest_index] = x;
        sum += x;

        /*insert x into sorted array*/
        int i = num;
        while (i > 0 && sorted[i] > x) {
            sorted[i] = sorted[i - 1];
            i--;
        }
        sorted[i] = x;

        num++;
    }
    else {
        /*find sorted index of oldest element*/
        int removed;
        for (removed = 0; removed < _length; removed++) {
            if (sorted[removed] == buf[oldest_index]) break;
        }

        /*update circular buffer*/
        sum -= buf[oldest_index];
        oldest_index++;
        if (oldest_index >= _length) oldest_index -= _length;
        newest_index++;
        if (newest_index >= _length) newest_index -= _length;
        buf[newest_index] = x;
        sum += x;

        /*insert x*/
        int i;
        if (removed == _length - 1) {
            i = _length - 1;
            while (i > 0 && sorted[i] > x) {
                sorted[i] = sorted[i - 1];
                i--;
            }
            sorted[i] = x;
        }
        else if (removed == 0) {
            i = 0;
            while (i < _length - 1 && sorted[i] < x) {
                sorted[i] = sorted[i + 1];
                i++;
            }
            sorted[i] = x;
        }
        else if (sorted[removed - 1] <= x && sorted[removed + 1] >= x) {
            sorted[removed] = x;
        }
        else if (sorted[removed - 1] > x) {
            i = removed;
            while (i > 0 && sorted[i] > x) {
                sorted[i] = sorted[i - 1];
                i--;
            }
            sorted[i] = x;
        }
        else {
            i = removed;
            while (i < _length - 1 && sorted[i] < x) {
                sorted[i] = sorted[i + 1];
                i++;
            }
            sorted[i] = x;
        }
    }
}

float CircularBuffer::mean() {
    return sum / num;
}

float CircularBuffer::median() {
    if (num < _length) {
        if (num % 2 == 1) {
            return sorted[(num - 1) / 2];
        }
        else {
            return (sorted[num / 2] + sorted[num / 2 - 1]) / 2.0f;
        }
    }
    else {
        if (_length % 2 == 1) {
            return sorted[(_length - 1) / 2];
        }
        else {
            return (sorted[_length / 2] + sorted[_length / 2 - 1]) / 2.0f;
        }
    }
}