#include <string.h>

#include "med2.h"

void MED2_Init(MED2_Data& Data)
{
  for (unsigned char a = 0; a < Data.Size; a++) { Data.Index[a] = a; Data.Buff[a] = 0; }
  Data.Last = 0;
}

long MED2_Update(long Value, MED2_Data& Data)
{
  unsigned char* index=Data.Index;
  unsigned char size=Data.Size;
  unsigned char& last=Data.Last;
  long* buff=Data.Buff;
  //---
  buff[last] = Value;
  //---
  unsigned char id;
  for (id = 0; id < size; id++) if (index[id] == last) break;
  //---
  bool f = (!id || (Value > buff[index[id - 1]]));

  if (f)
  {
    for (unsigned char a = id; a < size - 1; a++)
    {
      if (buff[index[a + 1]] >= Value) break;
      unsigned char tmp = index[a];
      index[a] = index[a + 1];
      index[a + 1] = tmp;
    }
  }
  else
  {
    for (unsigned char a = id; a > 0; a--)
    {
      if (buff[index[a - 1]] <= Value) break;
      unsigned char tmp = index[a - 1];
      index[a - 1] = index[a];
      index[a] = tmp;
    }
  }

  last++;
  if (last >= size) last = 0;
  
  if(Data.Smooth>1)
  {
    long ret=0;
    long s = Data.Smooth/2;
    for(long a=-s; a<s; a++) ret+=buff[index[size/2+a]];
    return ret/Data.Smooth;
  }
  
  return buff[index[size/2]];
}