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

Adafruit_MPU6050 mpu;
Servo myServo;

const int ledPin = 2;
const int servoPin = 4;

const float threshold = 2.5; // 加速度變化的閾值 (單位：g)

void setup() {
  Serial.begin(115200);
  pinMode(ledPin, OUTPUT);

  myServo.attach(servoPin);

  // 初始化 MPU6050
  if (!mpu.begin()) {
    Serial.println("無法找到 MPU6050");
    while (1);
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  Serial.println("MPU6050 初始化完成");
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  float accTotal = sqrt(a.acceleration.x * a.acceleration.x +
                        a.acceleration.y * a.acceleration.y +
                        a.acceleration.z * a.acceleration.z);

  Serial.print("加速度總值: ");
  Serial.println(accTotal);

  if (abs(accTotal - 9.8) > threshold) {
    triggerAction();
  }

  delay(200); // 減少取樣頻率
}

void triggerAction() {
  // 馬達動作
  myServo.write(90);
  delay(500);
  myServo.write(0);

  // LED 閃爍
  for (int i = 0; i < 3; i++) {
    digitalWrite(ledPin, HIGH);
    delay(200);
    digitalWrite(ledPin, LOW);
    delay(200);
  }
}