#include <WiFi.h>
#include <WiFiUdp.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_MAX3010x.h>
#include <QMC5883LCompass.h>

// ---------------- CONFIG ----------------
#define DEVICE_ID "1"  // Set "0" for Player 1, "1" for Player 2

const char* ssid = "Master";            // ESP32 AP SSID
const char* password = "12345678";      // ESP32 AP Password
const char* receiverIP = "192.168.4.1"; // ESP32 IP
const int receiverPort = 12345;         // ESP32 listening port

WiFiUDP udp;
Adafruit_MPU6050 mpu;
Adafruit_MAX3010x max30102;
QMC5883LCompass compass;

float axFiltered = 0, ayFiltered = 0, azFiltered = 0;
float alpha = 0.1;  // Low-pass filter smoothing factor

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // Wi-Fi Connection
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.println("Connecting to WiFi...");
  }
  Serial.println("Connected to WiFi");

  // MPU6050 Init
  if (!mpu.begin()) {
    Serial.println("MPU6050 not detected!");
    while (1) delay(10);
  }
  Serial.println("MPU6050 initialized");

  // MAX30102 Init
  if (!max30102.begin()) {
    Serial.println("MAX30102 not detected!");
    while (1) delay(10);
  }
  max30102.setup();
  Serial.println("MAX30102 initialized");

  // Compass Init
  compass.init();
  // Optional: Calibrate these values based on your device
  compass.setCalibration(-1135, 1805, -1707, 1023, -1294, 2043);
  Serial.println("QMC5883L Compass initialized");
}

void loop() {
  // ----- Read MPU6050 -----
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  axFiltered = alpha * a.acceleration.x + (1 - alpha) * axFiltered;
  ayFiltered = alpha * a.acceleration.y + (1 - alpha) * ayFiltered;
  azFiltered = alpha * a.acceleration.z + (1 - alpha) * azFiltered;

  int ax = map(axFiltered * 100, -100, 100, 0, 100);
  int ay = map(ayFiltered * 100, -100, 100, 0, 100);
  int az = map(azFiltered * 100, -100, 100, 0, 100);

  // ----- Read MAX30102 -----
  uint16_t ir = 0, red = 0;
  max30102.check();
  if (max30102.available()) {
    red = max30102.getRed();
    ir = max30102.getIR();
    max30102.nextSample();
  }

  // ----- Read Compass -----
  compass.read();
  int heading = compass.getAzimuth(); // Angle in degrees

  // ----- Format Data -----
  String packet = String(DEVICE_ID) + "," +
                  String(ax) + "," + String(ay) + "," + String(az) + "," +
                  String(red) + "," + String(ir) + "," +
                  String(heading);

  // ----- Send UDP Packet -----
  udp.beginPacket(receiverIP, receiverPort);
  udp.print(packet);
  udp.endPacket();

  // ----- Debug -----
  Serial.println(packet);

  delay(50);  // 20Hz loop (adjust as needed)
}