#include "liblocky.h"

#include <assert.h>
#include <math.h>
#include <string.h>


void blky_sensor_feed(blky_sensor_t* b, const float* v, uint64_t n) {
  assert(b->time+n <= BLKY_SENSOR_MAX_DUR);
  if (n == 0) return;

  const uint64_t t = b->time;
  memcpy(b->values+t, v, n*sizeof(float));
  b->time += n;

  for (uint64_t i = t; i < b->time; ++i) {
    b->sum += b->values[i];
  }
  b->avg = b->sum / (double) b->time;

  b->var = 0;
  for (uint64_t i = 0; i < b->time; ++i) {
    const double diff = b->values[i] - b->avg;
    b->var += diff*diff;
  }
  b->var /= (double) b->time;

  b->cov = 0;
  for (uint64_t i = 0; i < b->time; ++i) {
    const double diff_v = b->values[i] - b->avg;
    const double diff_t = (double) i - (double) b->time/2.;
    b->cov += diff_v * diff_t;
  }
  b->cov /= (double) b->time;

  const double tf   = (double) b->time;
  const double tvar =
    (tf/6*(tf+1)*(2*tf+2)-tf*tf/2-tf*tf*tf/4)/tf;

  b->prev_correl = b->correl;

  if (b->var == 0) {
    b->correl = 1;
  } else {
    b->correl = b->cov / sqrt(b->var*tvar);
  }
}

void blky_sensor_drop(blky_sensor_t* b, uint64_t until) {
  (void) b;
  (void) until;
  // TODO
}