#include <Wire.h>
#include <MPU6050.h> // Include MPU6050 library
#include <TCA9548A.h> // Include TCA9548 Multiplexer library

#include <Wire.h>
#include <MPU6050.h>

// TCA9548A I2C Multiplexer
#define TCAADDR 0x70

// MPU6050 objects, renamed as per your request
MPU6050 LeftHandSensor(0x68);
MPU6050 RightHandSensor(0x68);
MPU6050 LeftKneeSensor(0x68);
MPU6050 RightKneeSensor(0x68);

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

  tcaselect(0);
  LeftHandSensor.initialize();
  calibrateMPU(LeftHandSensor);

  tcaselect(1);
  RightHandSensor.initialize();
  calibrateMPU(RightHandSensor);

  tcaselect(2);
  LeftKneeSensor.initialize();
  calibrateMPU(LeftKneeSensor);

  tcaselect(3);
  RightKneeSensor.initialize();
  calibrateMPU(RightKneeSensor);
}

void loop() {
  int16_t ax, ay, az, gx, gy, gz;

  tcaselect(0);
  LeftHandSensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  printSensorData("Left Hand", ax, ay, az, gx, gy, gz);

  tcaselect(1);
  RightHandSensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  printSensorData("Right Hand", ax, ay, az, gx, gy, gz);

  tcaselect(2);
  LeftKneeSensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  printSensorData("Left Knee", ax, ay, az, gx, gy, gz);

  tcaselect(3);
  RightKneeSensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  printSensorData("Right Knee", ax, ay, az, gx, gy, gz);

  delay(500);
}

void tcaselect(uint8_t i) {
  if (i > 7) return;
  Wire.beginTransmission(TCAADDR);
  Wire.write(1 << i);
  Wire.endTransmission();  
}

void calibrateMPU(MPU6050 &mpu) {
  int16_t ax, ay, az, gx, gy, gz;
  long axSum = 0, aySum = 0, azSum = 0, gxSum = 0, gySum = 0, gzSum = 0;

  for (int i = 0; i < 100; i++) {
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    axSum += ax;
    aySum += ay;
    azSum += az - 16384; // Assuming gravity is aligned with Z axis
    gxSum += gx;
    gySum += gy;
    gzSum += gz;
    delay(10);
  }

  mpu.setXAccelOffset(-axSum / 100);
  mpu.setYAccelOffset(-aySum / 100);
  mpu.setZAccelOffset(-(azSum / 100 + 16384));
  mpu.setXGyroOffset(-gxSum / 100);
  mpu.setYGyroOffset(-gySum / 100);
  mpu.setZGyroOffset(-gzSum / 100);
}

void printSensorData(String label, int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz) {
  Serial.print(label + ": ");
  Serial.print("Axyz: ");
  Serial.print(ax); Serial.print(" ");
  Serial.print(ay); Serial.print(" ");
  Serial.print(az); Serial.print(" ");
  Serial.print("Gxyz: ");
  Serial.print(gx); Serial.print(" ");
  Serial.print(gy); Serial.print(" ");
  Serial.print(gz);
  Serial.println();
}