// MPU6050 example
// https://wokwi.com/arduino/projects/305937248748044864

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <Servo.h>
#include <LiquidCrystal.h>

Adafruit_MPU6050 mpu;

Servo s1;
Servo s2;
Servo s3;
Servo s4;

LiquidCrystal lcd(11, 10, 6, 5, 4, 3);

boolean detected_mpu = false;

float g = 9.8065;

void setup() {

  Serial.begin(115200);
  lcd.begin(16, 2);

  if (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  else if (mpu.begin()) {
    detected_mpu = true;
    Serial.println("MPU6050 ready!");
  }

  if (detected_mpu) {
    s1.attach(7); //Up Servo
    s2.attach(12); //Down Servo
    s3.attach(13); //Left Servo
    s4.attach(8); //Right Servo
    s1.write(90);
    s2.write(90);
    s3.write(90);
    s4.write(90);
    lcd.print("MPU OK. S1-4 OK.");
  }

}

sensors_event_t event;

void loop() {
  mpu.getAccelerometerSensor()->getEvent(&event);
  
  Serial.println("-----");
  Serial.println(event.acceleration.x);
  Serial.println(event.acceleration.y);
  Serial.println(event.acceleration.z);
  Serial.println("-----");
  Serial.println("");
  
  if (abs(event.acceleration.x) >= 0.1 * g) {
    Serial.println("mod x acc >= 0.1 * g");
    if (event.acceleration.x >= 0.1 * g) {
      Serial.println("    x acc >= 0.1 * g");
      if (s1.read() <= 90 + 10) {
        s1.write(s1.read() + 1);
      }

      if (s2.read() <= 90 + 10) {
        s2.write(s2.read() + 1);
      }
    }
    else if (event.acceleration.x <= -0.1 * g) {
      Serial.println("    x acc <= -0.1 * g");
      if (s1.read() >= 90 - 10) {
        s1.write(s1.read() - 1);
      }

      if (s2.read() >= 90 - 10) {
        s2.write(s2.read() - 1);
      }
    }
  }

  else if (abs(event.acceleration.x) < 0.1 * g) {
    Serial.println("mod x acc < 0.1 * g");
    s1.write(90);
    s2.write(90);
  }

  if (abs(event.acceleration.y) >= 0.1 * g) {
    Serial.println("mod y acc >= 0.1 * g");
    if (event.acceleration.y >= 0.1 * g) {
      Serial.println("    y acc >= 0.1 * g");
      if (s3.read() <= 90 + 10) {
        s3.write(s3.read() + 1);
      }

      if (s4.read() <= 90 + 10) {
        s4.write(s4.read() + 1);
      }
    }
    else if (event.acceleration.y <= -0.1 * g) {
      Serial.println("    y acc <= -0.1 * g");
      if (s3.read() >= 90 - 10) {
        s3.write(s3.read() - 1);
      }

      if (s4.read() >= 90 - 10) {
        s4.write(s4.read() - 1);
      }
    }
  }

  else if (abs(event.acceleration.y) < 0.1 * g) {
    Serial.println("mod y acc < 0.1 * g");
    s3.write(90);
    s4.write(90);
  }
  delay(500);
}