#include <MPU6050_tockn.h>
#include <Wire.h>

MPU6050 mpu6050(Wire);

// Definições dos pinos da ponte H L298
#define ENA 5  // Pino de habilitação do motor A
#define ENB 6  // Pino de habilitação do motor B
#define IN1 7  // Pino de controle do motor A (para frente)
#define IN2 8  // Pino de controle do motor A (para trás)
#define IN3 9  // Pino de controle do motor B (para frente)
#define IN4 10 // Pino de controle do motor B (para trás)

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true); // Calibração automática

  // Configura os pinos da ponte H como saída
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
}

void loop() {
  mpu6050.update();

  int eixoX = mpu6050.getAngleX();
  int eixoY = mpu6050.getAngleY();

  // Mapeamento da velocidade (0 a 90 graus para 0 a 255)
  int velocidadeA = map(abs(eixoX), 0, 90, 0, 255);
  int velocidadeB = map(abs(eixoY), 0, 90, 0, 255);

  // Limita a velocidade entre 0 e 255
  velocidadeA = constrain(velocidadeA, 0, 255);
  velocidadeB = constrain(velocidadeB, 0, 255);

  Serial.print("eixoX: " + String(eixoX) + "°\t");
  Serial.print("eixoY: " + String(eixoY) + "°\n");

  // Controle do motor baseado na inclinação
  if (eixoX > 7) {
    // Mover para frente
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(ENA, velocidadeA); // Ajustar velocidade
  } else if (eixoX < -7) {
    // Mover para trás
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    analogWrite(ENA, velocidadeA); // Ajustar velocidade
  } else {
    // Parar motor A
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    analogWrite(ENA, 0); // Parar motor
  }

  if (eixoY > 7) {
    // Girar para a direita
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    analogWrite(ENB, velocidadeB); // Ajustar velocidade
  } else if (eixoY < -7) {
    // Girar para a esquerda
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    analogWrite(ENB, velocidadeB); // Ajustar velocidade
  } else {
    // Parar motor B
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
    analogWrite(ENB, 0); // Parar motor
  }

  delay(100);
}