#include <stdio.h>

#define MA_SIZE 4000

typedef struct
{
	float summ;
	float error;
} sKahanAdder_t;

typedef struct
{
	float         buf[MA_SIZE];
	int           pos;
	sKahanAdder_t in;
	sKahanAdder_t out;
} sMovingAverage_t;

typedef struct
{
	sMovingAverage_t filter[2];
	int              active;
	int              n;
} sFilterSwapper_t;


void KahanAdder_Init (sKahanAdder_t k[static 1])
{
	k->summ  = 0;
	k->error = 0;
}

void KahanAdder_Add (sKahanAdder_t k[static 1], float value)
{
	float y = value - k->error;
	float t = k->summ + y;
	k->error = (t - k->summ) - y;
	k->summ  = t;
}

void MovingAverage_Init (sMovingAverage_t m[static 1])
{
	m->pos = 0;
	for (int i=0; i<MA_SIZE; i++) m->buf[i] = 0;
	KahanAdder_Init (&m->in);
	KahanAdder_Init (&m->out);
}

void MovingAverage_Step (sMovingAverage_t m[static 1], float value)
{
	float old = m->buf[m->pos];
	m->buf[m->pos] = value;
	KahanAdder_Add (&m->in, value);
	KahanAdder_Add (&m->out, old);
	m->pos++;
	if (m->pos == MA_SIZE) m->pos=0;
}

float MovingAverage_Get (const sMovingAverage_t m[static 1])
{
	return (m->in.summ - m->out.summ)/((float)MA_SIZE);
}


void FilterSwapper_Init (sFilterSwapper_t f[static 1])
{
	MovingAverage_Init(&f->filter[0]);
	MovingAverage_Init(&f->filter[1]);
	f->active=0;
	f->n = 0;
}

void FilterSwapper_Step (sFilterSwapper_t f[static 1], float value)
{
	sMovingAverage_t * factive  = &f->filter[ f->active ];
	sMovingAverage_t * fprepare = &f->filter[ f->active ^ 1];
	
	MovingAverage_Step(factive, value);
		
	if (f->n >= MA_SIZE)
	{
		MovingAverage_Step (fprepare, value);
	}
	
	if (f->n == MA_SIZE*2)
	{
		// swap filters:
		f->n -= MA_SIZE;
		MovingAverage_Init (factive);
		f->active ^= 1;
	}

	f->n++;
}

float FilterSwapper_Get (const sFilterSwapper_t f[static 1])
{
	return MovingAverage_Get (&f->filter[ f->active ]);
}


int main (int argc, char **args)
{
	// very simple test
	sFilterSwapper_t x;

	FilterSwapper_Init (&x);
	for (int i=0; i<10000; i++) 
	{
		FilterSwapper_Step (&x, (i&1) ? 100: 1000);
		printf ("%d %f\n", i, FilterSwapper_Get(&x));
	}
	
	return 1;
}