#include <WiFi.h>
#include <PubSubClient.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

// --- Configuration ---
const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqtt_server = "broker.hivemq.com";
const char* topic = "ksu/shawn/gym_data";

const int LED_PIN = 7;
Adafruit_MPU6050 mpuBack;  // 0x68
Adafruit_MPU6050 mpuElbow; // 0x69

WiFiClient espClient;
PubSubClient client(espClient);

void setup() {
  Serial.begin(115200);
  pinMode(LED_PIN, OUTPUT);
  Wire.begin(8, 9); // SDA=8, SCL=9

  if (!mpuBack.begin(0x68)) Serial.println("Missing Back Sensor");
  if (!mpuElbow.begin(0x69)) Serial.println("Missing Elbow Sensor");

  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) { delay(500); Serial.print("."); }
  client.setServer(mqtt_server, 1883);
}

void reconnect() {
  while (!client.connected()) {
    String clientId = "Shawn-Gym-Final-" + String(random(0xffff), HEX);
    if (client.connect(clientId.c_str())) {
      client.subscribe(topic);
    } else {
      delay(5000);
    }
  }
}

void loop() {
  if (!client.connected()) reconnect();
  client.loop();

  sensors_event_t aB, gB, tB; 
  sensors_event_t aE, gE, tE; 
  mpuBack.getEvent(&aB, &gB, &tB);
  mpuElbow.getEvent(&aE, &gE, &tE);

  // 打包數據：傳送原始值，過濾邏輯交給 Colab 處理
  String payload = "{";
  payload += "\"back\":{\"ax\":" + String(aB.acceleration.x) + ",\"ay\":" + String(aB.acceleration.y) + ",\"az\":" + String(aB.acceleration.z) + "},";
  payload += "\"elbow\":{\"ax\":" + String(aE.acceleration.x) + ",\"ay\":" + String(aE.acceleration.y) + ",\"az\":" + String(aE.acceleration.z) + "}";
  payload += "}";

  client.publish(topic, payload.c_str());
  
  // 本地簡單警告 (如果背部 X/Y 晃動則亮燈)
  if (abs(aB.acceleration.x) + abs(aB.acceleration.y) > 3.0) digitalWrite(LED_PIN, HIGH);
  else digitalWrite(LED_PIN, LOW);

  delay(150); 
}