//Plate stabilazor with ESP32
//Button starts the script for before leveling and adjusting the plate with the servo's
//Fix MPU6050 on the bottom of the plate
//Still some software adjustments need to be done

#include <Wire.h>
#include <MPU6050.h>
#include <ESP32Servo.h>  // Include the ESP32 Servo library

MPU6050 mpu;  // MPU6050 object

Servo servoX;  // Servo objects, using standard Servo class from ESP32Servo library
Servo servoY;

const int servoPinX = 18;  // Use PWM-capable pins
const int servoPinY = 19;

const int switchPin = 23;  // Switch pin

bool systemActive = false;  // Operation status variables
bool lastSwitchState = HIGH;

long timer = 0;
float dt;

const float alpha = 0.98;  // Filter parameters

float angleX = 0;  // Angle variables
float angleY = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }

  servoX.attach(servoPinX);
  servoY.attach(servoPinY);
  pinMode(switchPin, INPUT_PULLUP);

  timer = micros();
}

void loop() {
  bool currentSwitchState = digitalRead(switchPin);
  if (currentSwitchState != lastSwitchState) {
    if (currentSwitchState == LOW) {
      systemActive = !systemActive;
    }
    lastSwitchState = currentSwitchState;
  }

  if (systemActive) {
    int16_t ax, ay, az, gx, gy, gz;
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    long currentTime = micros();
    dt = (currentTime - timer) / 1000000.0;
    timer = currentTime;

    float accelAngleX = atan2(ay, az + abs(ax)) * 180 / PI;
    float accelAngleY = atan2(ax, az + abs(ay)) * 180 / PI;

    float gyroRateX = gx / 131.0;
    float gyroRateY = gy / 131.0;

    angleX = alpha * (angleX + gyroRateX * dt) + (1 - alpha) * accelAngleX;
    angleY = alpha * (angleY + gyroRateY * dt) + (1 - alpha) * accelAngleY;

    int servoAngleX = map(angleX, -90, 90, 0, 180);
    int servoAngleY = map(angleY, -90, 90, 0, 180);

    servoX.write(servoAngleX);
    servoY.write(servoAngleY);
  } else {
    servoX.write(90);
    servoY.write(90);
  }

  Serial.print("Switch status: "); Serial.println(currentSwitchState);
  Serial.print("System active: "); Serial.println(systemActive);

  delay(1);
}