#include <SimpleKalmanFilter.h>

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


#include <Adafruit_AS5600.h>

#include <Wire.h>
#include <Servo.h>


Adafruit_AS5600 as5600;

#define REPORT_INTERVAL 200   // report every 200 ms while held
// Make Window Size smaller/bigger if data scatter is less/more
// (For MAX6675 the optimal WindowSize is ~0.6 | suggested to use a simple moving average before DWFilter for better results)
#define WindowSize 20

Servo myServo;

unsigned long lastReportTime = 0;
int lastButtonState = HIGH;

void setup() {
//  Wire.begin();
  Serial.begin(115200);
  Serial.println("DEBUG MODE ON");
  myServo.attach(9);  // Servo signal on pin 9
  as5600.begin();
  as5600.setHysteresis(AS5600_HYSTERESIS_2LSB);
//  as5600.setOutputStage(AS5600_OUTPUT_STAGE_DIGITAL_PWM);
  as5600.setOutputStage(AS5600_OUTPUT_STAGE_ANALOG_FULL);
  as5600.setPWMFreq(AS5600_PWM_FREQ_920HZ);
  as5600.setSlowFilter(AS5600_SLOW_FILTER_16X);
  as5600.setFastFilterThresh(AS5600_FAST_FILTER_THRESH_SLOW_ONLY);
}

void loop() {
//  uint16_t rawAngle = analogRead(A_sensor);
  uint16_t rawAngle = as5600.getRawAngle();
  float filtered=simpleKalmanFilter.updateEstimate(rawAngle);
  float angleDegrees = constrain((filtered * 360.0) / 4096.0, 0, 180);
  myServo.write((int)angleDegrees);


  // --- Report if Button Held ---
//  if (buttonHeld && millis() - lastReportTime >= REPORT_INTERVAL) {
//    Serial.print("Angle: ");
  if (millis() - lastReportTime > 10)
  {  //Serial.print(angleDegrees, 0);
  // Showing results in the Plotter
//  Serial.print("Data:");
//  Serial.print(rawAngle);
//  Serial.print("\t");
  Serial.print("FilteredData:");
  Serial.println(angleDegrees);     // filter() function receives your data, returning the filtered value

  }
//  }

  delay(10);  // Small delay for loop stability
}