Efficient streaming sliding window algorithm

/*** peakhold.h
*******************************************************************************/
/*
 * Streaming sliding window algorithm with O(M log N). M = number of samples,
 * N is 2^n windowsize). This algorithm is pretty fast as it relies only on
 * simple data structures and is further strategically optimised by choice
 * of buffer sizes.
 *
 * Pushing a new sample is O(log N) in the worst case, but is around O(1)
 * on average (N being window size). There are reasons for this I may explain
 * at a later time.
 *
 * Finding the maximum value in a window is O(log N). To give you an idea of
 * its speed, on a laptop i5 compiled with GCC -O3, it can calculate 100
 *  seconds of samples at 44.1kz with a window size of 1 second in just 2000ms.
 *
 * More info: http://algorithmsandme.in/2014/03/heaps-sliding-window-algorithm/
 *
 * TODO: Rename peakhold to sliding_window (was originally written for
 * audio DSP).
 */

#ifndef PEAK_HOLD_H
#define PEAK_HOLD_H

#include <stdlib.h> /* size_t */

#ifdef __cplusplus
extern "C" {
#endif


/*! \brief Allocates instance with zeroed buffers. */
struct PeakHold* PeakHold_Create(size_t max_window_size);
/*! \brief Destroys all memory associated with the instance */
void PeakHold_Destroy(struct PeakHold *self);
/*! \brief Clears all buffers */
void PeakHold_Clear(struct PeakHold *self);
/*! \brief Inserts next sample */
void PeakHold_Insert(struct PeakHold *self, float value);
/*! \brief Returns maximum sample within the given window size */
float PeakHold_CalcMaxPeak(struct PeakHold *self, size_t window_size);

#ifdef __cplusplus
}
#endif

#endif /* PEAK_HOLD_H */


/*** peakhold.c
*******************************************************************************/
#include "peakhold.h"

#include <stdio.h>
#include <math.h>
#include <float.h>
#include <time.h>
#include <stdlib.h>
#include <stddef.h>
#include <assert.h>
#include <stdbool.h>
#include <string.h>

struct PeakHold_Sample {
  float value;
  unsigned int is_valid;
};

typedef unsigned char PeakHold_Bool;
enum { PeakHold_False, PeakHold_True = !PeakHold_False };

typedef struct PeakHold_Sample *PeakHold_SamplePtr;

struct PeakHold {
  /*! \brief Stores layers of ring buffers.

     Holds a binary tree of samples where a parent
     holds the max value of its two children. level_0
     holds the leaf nodes. See below, level_1[3] is equal
     to Max(level_0[6], level_0[7]). level_0[6] and
     level_0[7] are the children of level_1[3].

     The buffer is 'buffer_size' entries long, with
     level_0 being buffer_size/2 entries long.

     Each level is half the size of the previous. level_0
     is first in memory, followed by level_1, and so on.

     | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 | level_0
     |   3   |   2   |   1   |   0   | level_1
     |       1       |       0       | level_2
  */
  struct PeakHold_Sample *buffer;
  /*! Stores pointers into 'buffer' for each level. */
  PeakHold_SamplePtr *level_ptr;
  /*! Number of entries in each level. */
  size_t *level_size;
  size_t levels;
  size_t buffer_size;
  size_t position;
};

static long long PeakHold_RandU64() {
  long long value = 0;
  for(int i = 0; i < 8; ++i) {
    ((char*)&value)[i] = rand();
  }
  return value;
}

static float PeakHold_RandFloat() {
  float fval = rand() & 0x7fff;
  fval *= (rand() & 1) ? 1.f : -1.f;
  fval /= 32768.f;
  fval *= 100.f;
  return fval;
}

void PeakHold_Destroy(struct PeakHold *self) {
  free(self->level_size);
  self->level_size = 0;
  free(self->level_ptr);
  self->level_ptr = 0;
  free(self->buffer);
  self->buffer = 0;
  self->buffer_size = 0;
  free(self);
}

static void PeakHold_SetSample(struct PeakHold *self, size_t level, size_t idx,
            float value);

static float PeakHold_FloatMax(float a, float b) { return a > b ? a : b; }

static int SizetIsPow2(size_t val) { return val == (val & -val); }
static int UintIsPow2(unsigned int val) { return val == (val & -val); }
static int IntIsPow2(int val) { return val == (val & -val); }

static size_t PeakHold_MinSizeT(size_t a, size_t b) { return a < b ? a : b; }
static size_t PeakHold_MaxSizeT(size_t a, size_t b) { return a > b ? a : b; }
static size_t PeakHold_CalcLevels(size_t size);

struct PeakHold* PeakHold_Create(size_t max_window_size) {

  /* Find power of 2 size that is at least twice the size of `max_window_size` */

  size_t actual_size = 1;
  while(actual_size < (max_window_size * 2)) actual_size <<= 1;



  struct PeakHold *ptr = calloc(1, sizeof(struct PeakHold));
  ptr->buffer = calloc(actual_size, sizeof(struct PeakHold_Sample));
  ptr->buffer_size = actual_size;
  ptr->position = 0;


  /* Calculate number of levels */
  ptr->levels = PeakHold_CalcLevels(ptr->buffer_size / 2);
  ptr->level_ptr = calloc(ptr->levels, sizeof(PeakHold_SamplePtr));
  ptr->level_size = calloc(ptr->levels, sizeof(size_t));

  ptr->level_ptr[0] = ptr->buffer;
  ptr->level_size[0] = ptr->buffer_size / 2;

  for(size_t i = 1; i < ptr->levels; ++i) {
    ptr->level_ptr[i] = ptr->level_ptr[i-1] + ptr->level_size[i-1];
    ptr->level_size[i] = ptr->level_size[i-1] / 2;
  }

  return ptr;
}

void PeakHold_Clear(struct PeakHold *self) {
  memset(self->buffer, 0, sizeof(struct PeakHold_Sample) * self->buffer_size);
  self->position = 0;
}

void PeakHold_Insert(struct PeakHold *self, float value) {
  PeakHold_SetSample(self, 0, self->position, value);
  ++self->position;
  self->position &= self->buffer_size / 2 - 1;
}

static PeakHold_Bool PeakHold_IsSampleValid(struct PeakHold *self, size_t level,
                     size_t idx) {
  assert(self);
  assert(level < self->levels);
  /* buffer_size is a power of 2, thus this will mask / wrap idx */
  idx &= self->buffer_size / 2 - 1;
  assert(idx < self->buffer_size);

  size_t level_idx = idx >> level;
  return self->level_ptr[level][level_idx].is_valid;
}

static float PeakHold_GetSampleAt(struct PeakHold *self, size_t level, size_t idx) {
  assert(self);
  assert(level < self->levels);
  /* buffer_size is a power of 2, thus this will mask / wrap idx */
  idx &= self->buffer_size - 1;
  assert(idx < self->buffer_size);

  size_t level_idx = idx >> level;
  return self->level_ptr[level][level_idx].value;
}

/* Returns highest level with valid samples at the given sample position */
static size_t PeakHold_GetHighestLevel(struct PeakHold *self, size_t max_length,
                size_t idx) {
  size_t highest_level = 0;
  for(size_t level = 0; level < self->levels; ++level) {
    size_t size = 1 << level;
    PeakHold_Bool is_valid = PeakHold_IsSampleValid(self, level, idx);
    PeakHold_Bool can_fit = size <= max_length;
    if(!(is_valid && can_fit))
      return highest_level;
    highest_level = level;
  }

  return highest_level;
}

float PeakHold_CalcMaxPeak(struct PeakHold *self, size_t window_size) {
  assert(self);
  if(window_size == 0)
    return 0.f;

  float max_peak = -FLT_MAX;

  size_t idx_mask = self->buffer_size / 2 - 1;

  size_t offset = 0;
  size_t level = 0;
  while(offset < window_size) {
    size_t idx = (self->position - offset - 1) & idx_mask;
    size_t remaining_size = window_size - offset;
    size_t level = PeakHold_GetHighestLevel(self, remaining_size, idx);
    size_t level_size = 1 << level;
    size_t is_valid = PeakHold_IsSampleValid(self, level, idx);


    /* Window may include samples not yet set. */
    if(!is_valid)
      return max_peak;

    float cur_value = PeakHold_GetSampleAt(self, level, idx);
    max_peak = PeakHold_FloatMax(max_peak, cur_value);

    offset += level_size;
  }

  return max_peak;
}

/* Calculates the number of buffer levels for a given power-of-2 window size */
static size_t PeakHold_CalcLevels(size_t size){
  size_t l = size;
  size_t levels = 0;
  while(l > 1) {
    l >>= 1;
    ++levels;
  }
  return levels;
}

static void PrintAll(struct PeakHold_Sample *sample, size_t sz) {
  size_t level = 0;
  while(sz > 1) {
    printf("Level %zu (%zu) :: %p ***\n", level, sz, sample);
    for(size_t i = 0; i < sz; ++i) {
      printf("  sample(%f), is_valid(%s)\n", sample[i].value,
         sample[i].is_valid ? "true" : "false");
    }
    sample += sz;
    sz /= 2;
    ++level;
  }
}

static void PeakHold_InvalidateSample(struct PeakHold *self, size_t level,
                   size_t idx) {
  PeakHold_Bool has_child = level < self->levels - 1;
  PeakHold_Bool invalidate_child = idx % 2 == 0;
  self->level_ptr[level][idx].is_valid = PeakHold_False;
  if(has_child && invalidate_child)
    PeakHold_InvalidateSample(self, level + 1, idx / 2);
}

/* Sets the sample at a given sample index */
static void PeakHold_SetSample(struct PeakHold *self, size_t level, size_t idx,
            float value) {
  struct PeakHold_Sample *cur_level = self->level_ptr[level];
  self->level_ptr[level][idx].value = value;
  self->level_ptr[level][idx].is_valid = PeakHold_True;
  PeakHold_Bool validate_child = idx % 2 == 1;
  PeakHold_Bool has_child = level < self->levels - 1;
  if(!has_child)
    return;


  if(validate_child){
    float max_val = PeakHold_FloatMax(cur_level[idx].value,
                      cur_level[idx ^ 1].value);
    PeakHold_SetSample(self, level+1, idx / 2, max_val);
  } else {
    PeakHold_InvalidateSample(self, level+1, idx / 2);
  }
}

/* Tests the method for calculating number of levels based on max_window_size */
static void TestCalcLevels() {
  assert(PeakHold_CalcLevels(0L) == 0L);
  assert(PeakHold_CalcLevels(1L) == 0L);
  assert(PeakHold_CalcLevels(2L) == 1L);
  assert(PeakHold_CalcLevels(4L) == 2L);
  assert(PeakHold_CalcLevels(8L) == 3L);
  assert(PeakHold_CalcLevels(16L) == 4L);
}

/* Tests power of 2 test */
static void TestPow2() {
  assert(SizetIsPow2(2));
  assert(SizetIsPow2(4UL));
  assert(!SizetIsPow2(3UL));
  assert(IntIsPow2(4));
  assert(UintIsPow2(8));
  assert(!UintIsPow2(3));
}

static void Print(struct PeakHold *ptr) {
  PrintAll(ptr->buffer, ptr->buffer_size/2);
}

static void PeakHold_BruteSearch(const float *arr, float *out, size_t sz,
              size_t window_sz) {
  for(size_t i = 0; i < sz; ++i) {
    float max_val = -FLT_MAX;
    for(size_t j = 0; j < window_sz && j <= i; ++j) {
      max_val = PeakHold_FloatMax(max_val, arr[i - j]);
    }
    out[i] = max_val;
  }
}

static void PrintSamples(struct PeakHold *self) {
  for(size_t i = 0; i < self->buffer_size; ++i) {
    PeakHold_Bool is_valid = PeakHold_IsSampleValid(self, 0, i);
    if(i%4 == 0 && i > 0)
      printf("\n");
    if(is_valid) {
      printf("%f,\t", PeakHold_GetSampleAt(self, 0, i));
    } else {
      printf("xxxxxxxx,\t");
    }
  }

  printf("\n");
}

static void TimePeakHold(int seconds, size_t window_size) {
  size_t sample_count = (size_t)seconds * 44100ULL;
  struct PeakHold *ptr = PeakHold_Create(window_size);
  time_t timer = time(0);
  for(size_t sample_pos = 0; sample_pos < sample_count; ++sample_pos) {
    PeakHold_Insert(ptr, PeakHold_RandFloat());
    (void)PeakHold_CalcMaxPeak(ptr, window_size);
  }
  time_t end = time(0);

  PeakHold_Destroy(ptr);

  int diff = difftime(end, timer);

  printf("Seconds (%d), Window (%zu): seconds (%ds)\n", seconds,
     window_size, diff);
}

static float* CreateFloatData(size_t size) {
  float *data = malloc(size * sizeof(float));
  for(size_t i = 0; i < size; ++i)
    data[i] = PeakHold_RandFloat();

  return data;
}

static void PeakHold_HoldSearch(struct PeakHold *self, const float *data, float *out,
             size_t sz, size_t window_size) {
  for(size_t i = 0; i < sz; ++i) {
    PeakHold_Insert(self, data[i]);
    out[i] = PeakHold_CalcMaxPeak(self, window_size);
  }
}

static PeakHold_Bool FloatNearEq(float a, float b) {
  if(a == 0) return b == 0;

  float norm = b / a;
  float diff = fabs(1.f - norm);
  return diff < 0.000001f;
}

static void ValidatePeakHold(float seconds, size_t window_size) {
  size_t sample_count = (size_t)(seconds * 44100.f);
  float *data = CreateFloatData(sample_count);
  float *brute_calc = calloc(sample_count, sizeof(float));
  float *hold_calc = calloc(sample_count, sizeof(float));
  struct PeakHold *ptr = PeakHold_Create(window_size);

  PeakHold_BruteSearch(data, brute_calc, sample_count, window_size);
  PeakHold_HoldSearch(ptr, data, hold_calc, sample_count, window_size);

  for(size_t i = 0; i < sample_count; ++i) {
    if(!FloatNearEq(brute_calc[i], hold_calc[i]))
      printf("%f: %f .. %f\n", data[i], brute_calc[i], hold_calc[i]);
  }

  PeakHold_Destroy(ptr);
  free(data);
  free(brute_calc);
  free(hold_calc);
}

static void TestPeakHold(){
  /* ValidatePeakHold(10.f, 44100); */
  TimePeakHold(100, 44100);
}

int main() {
  time_t t;
  srand((unsigned) time(&t));
  TestPeakHold();
  return 0;
}