#include <Arduino.h>
#include <Wire.h>

#define MPU_ADDR 0x68
#define REG_PWR_MGMT_1 0x6B
#define REG_ACCEL_XOUT_H 0x3B

#define SERVO_X_PIN 32
#define SERVO_Y_PIN 33

const int frequency = 50;
const int resolution = 16;

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

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(REG_PWR_MGMT_1);
  Wire.write(0x00);
  Wire.endTransmission(true);

  ledcAttach(SERVO_X_PIN, frequency, resolution);
  ledcAttach(SERVO_Y_PIN, frequency, resolution);
}

int angleADuty(int angle) {
  return map(angle, 0, 180, 1638, 8192);
}

void loop() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(REG_ACCEL_XOUT_H);
  Wire.endTransmission(false);
  
  Wire.requestFrom(MPU_ADDR, 4, true);

  if (Wire.available() >= 4) {
    int16_t ax = (Wire.read() << 8) | Wire.read();
    int16_t ay = (Wire.read() << 8) | Wire.read();

    float gX = ax / 16384.0;
    float gY = ay / 16384.0;

    // 3. Mapeamos a ángulos del Servo
    long angleX = map(ax, -16384, 16384, 0, 180);
    long angleY = map(ay, -16384, 16384, 0, 180);

    angleX = constrain(angleX, 0, 180);
    angleY = constrain(angleY, 0, 180);

    // 4. Movemos los servos
    ledcWrite(SERVO_X_PIN, angleADuty(angleX));
    ledcWrite(SERVO_Y_PIN, angleADuty(angleY));

    Serial.print("Axis X: ");
    Serial.print(gX, 2);      
    Serial.print(" g (");
    Serial.print(angleX);
    Serial.print("°) | Axis Y: ");
    Serial.print(gY, 2);
    Serial.print(" g (");
    Serial.print(angleY);
    Serial.println("°)");
  }
  
  delay(50);
}