#include <SimpleKalmanFilter.h>

/*
 This sample code demonstrates how to use the SimpleKalmanFilter object. 
 Use a potentiometer in Analog input A0 as a source for the reference real value.
 Some random noise will be generated over this value and used as a measured value.
 The estimated value obtained from SimpleKalmanFilter should match the real
 reference value.

 SimpleKalmanFilter(e_mea, e_est, q);
 e_mea: Measurement Uncertainty 
 e_est: Estimation Uncertainty 
 q: Process Noise
 */
SimpleKalmanFilter simpleKalmanFilter(300, 300, 3);

// Serial output refresh time
const long SERIAL_REFRESH_TIME = 100;
long refresh_time;
volatile uint32_t T1 = 0;
volatile uint32_t T2 = 0;
volatile uint32_t T3 = 0;
volatile uint32_t T1bis = 0;
volatile uint32_t T2bis = 0;
volatile uint32_t T3bis = 0;
int realValue = 0;
int noise = 0;
int measValue = 0;
int estValue = 0;


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

void loop() {

  T1bis = millis();
  if(T1bis-T1 >= 2000){
    realValue = random(0,300);
    T1 = millis();
  }

  T2bis = millis();
  if(T2bis-T2 >= 300){
    noise = random(-50,51);
    T2 = millis();
  }
  
  T3bis = millis();
  if(T3bis-T3 >=100){ 
    measValue = realValue + noise;
    estValue = simpleKalmanFilter.updateEstimate(measValue);

    // send to Serial output every 100ms
    // use the Serial Ploter for a good visualization
    if (millis() > refresh_time) {
      Serial.print(realValue);
      Serial.print("  |  ");
      //Serial.print(noise);
      //Serial.print("  |  ");
      Serial.print(measValue);
      Serial.print("  |  ");
      Serial.println(estValue);
    }

    T3=millis();
  }
}