#include "acceleration.h"
#include "OutlierFilter.h"
#include "OutlierFilter2.h"
//#include "SMA.h"

OutlierFilter filter(0.1f, 10);
//SMA filter(10);

void setup() {
  Serial.begin(115200);
  pinMode(17, INPUT_PULLUP);
  pinMode(16, INPUT_PULLUP);
  delay(1000);
}

void loop() {

  uint32_t value = 1000;

  if (digitalRead(17)) value = 1000;
  else value = 500;
  if (!digitalRead(16)) value = 1100;

  uint32_t value1 = value;
  filter.update(value);

  Serial.printf("x1:1500,x2:-500,v1:%d,v2:%d\n", value1, value);

  delay(100);
}