// Algo from Detlef _. (detlef_a) 19.05.2024 20:53
// https://www.mikrocontroller.net/topic/567716#7668204

//typedef float median_t;
//typedef int32_t index_t;

typedef int16_t median_t;
typedef uint8_t index_t;

#define NN (3)

median_t median(median_t neu)
{
  index_t k, m;
  static index_t gross[NN];
  static index_t w = 0;
  static median_t alt[NN];

  //Ini
  if (w == 0) 
  {
    for (m = 0; m < NN; m++) alt[m] = 0;
    for (m = 0; m < NN; m++) gross[m] = m;
    w = 1;
  }

  // Den ältesten rausschmeissen
  k = 0;
  while (1) 
  {
    if (gross[k] == NN - 1) break;
    k++;
  }

  while (1) 
  {
    if (k == 0) break;
    gross[k] = gross[k - 1];
    k--;
  }

  // Wo musser denn hin?
  k = 0;
  while (1) 
  {
    if (alt[gross[k + 1]] > neu) break;
    k++;
    if (k == NN - 1)      break;
  }

  // Platz da
  m = 0;

  while (1) 
  {
    if (k == m) break;
    gross[m] = gross[m + 1];
    m++;
  }

  gross[k] = -1;
  for (m = 0; m < NN; m++) gross[m]++;
  // ruecken
  m = NN - 1;
  while (1) 
  {
    alt[m] = alt[m - 1];
    m--;
    if (m == 0) break;
  }

  alt[0] = neu;
  return (alt[gross[(NN - 1) / 2]]);
}

void setup()
{
  Serial.begin(115200);
}

void loop()
{
  uint32_t ts;
  median_t x; 
  median_t y;

  x = random(-32768,32767);
  
  ts = micros();
  y = median(x);
  uint32_t ende = micros();

  Serial.println("value" + String(x) + " time used [us]: " + String(ende - ts));
  delay(10);

}