// d1 mini side (transmitter)

#include <Wire.h>
#include <MPU6050_light.h>

MPU6050 mpu(Wire);

// state
bool isConnected = false;
const int led = 23;

// Kalman filter variables
double angle = 0.0;  // Current angle estimate
double angleEstimate = 0.0; // Initial angle estimate
double angleCovariance = 1.0; // Initial covariance estimate

// Kalman filter update step
void kalman_update(double measurement) {
    // Prediction step
    double prediction = angleEstimate;
    double predictionCovariance = angleCovariance + 0.1; // Process noise

    // Calculate Kalman gain
    double kalmanGain = predictionCovariance / (predictionCovariance + 1.0); // Measurement noise

    // Update step
    angle = prediction + kalmanGain * (measurement - prediction);
    angleCovariance = (1.0 - kalmanGain) * predictionCovariance;

    angleEstimate = angle;
}

void setup() {
  Serial.begin(115200);
  pinMode(led, OUTPUT); 
  digitalWrite(led, LOW);
  //  polling
  // while (true) {
  //   // sending test message
  //   Serial.println("ping");
  //   if (Serial.available() > 0) {
  //     String inp = Serial.readStringUntil('\n');
  //     inp.trim();
  //     if (inp == "pong") {
  //       break;
  //     } //connection established
  //   }
  //   delay(100);
  // }
  // starting mpu
  Wire.begin();
  while (mpu.begin()) {
    digitalWrite(led, !(digitalRead(led)));
    delay(100);
  }
  // calibrating
  digitalWrite(led, HIGH);
  delay(500);
  // mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
  mpu.calcOffsets(); // gyro and accelero
  digitalWrite(led, LOW);
}

void loop() {
  // get data from mpu
  mpu.update();
  double zangle = mpu.getAngleZ();
  // pass through kalman filter (to provide more accurate results)
  kalman_update(zangle);
  // print to serial
  Serial.println(angle);
  delay(50);
}