#include <HardwareSerial.h>
#include <TinyGPS++.h>
#include <MPU6050_tockn.h>
#include <WiFi.h>
#include <PubSubClient.h>

// 初始化模块
TinyGPSPlus gps;
MPU6050 mpu6050(Wire);
HardwareSerial SerialGPS(1);  // UART1用于GPS
HardwareSerial Serial4G(2);   // UART2用于4G模块

// 网络配置
const char* ssid = "WiFi_SSID";
const char* password = "WiFi_Password";
const char* mqttServer = "mqtt.example.com";

WiFiClient espClient;
PubSubClient client(espClient);

void setup() {
  Serial.begin(115200);
  SerialGPS.begin(9600, SERIAL_8N1, 13, 14);  // GPS初始化
  Serial4G.begin(115200, SERIAL_8N1, 15, 16); // 4G模块初始化
  Wire.begin(21, 22);                         // MPU6050 I2C
  mpu6050.begin();
  
  // 连接WiFi（备用，主用4G）
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) delay(500);
  
  // 初始化4G模块
  Serial4G.println("AT+NETOPEN");
  delay(5000);
}

void loop() {
  // 读取GPS数据
  while (SerialGPS.available() > 0) {
    if (gps.encode(SerialGPS.read())) {
      if (gps.location.isValid()) {
        float lat = gps.location.lat();
        float lng = gps.location.lng();
        client.publish("helmet/gps", String(lat)+","+String(lng).c_str());
      }
    }
  }

  // 检测跌倒
  mpu6050.update();
  if (abs(mpu6050.getAccX()) > 2.0 || abs(mpu6050.getAccY()) > 2.0) {
    client.publish("helmet/alert", "FALL_DETECTED");
    Serial4G.println("ATD10086;");  // 拨打预设电话
  }

  // 维持MQTT连接
  if (!client.connected()) reconnect();
  client.loop();
}

void reconnect() {
  while (!client.connect("HelmetClient")) {
    delay(1000);
  }
}

// 计算合加速度
float accel = sqrt(pow(mpu6050.getAccX(), 2) + 
              pow(mpu6050.getAccY(), 2) + 
              pow(mpu6050.getAccZ(), 2));
// 判断是否超过阈值（示例阈值为2.5g）
if (accel > 2.5) {
  triggerAlarm();
}