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

#define STEP_LEFT  25   // STEP motor kiri
#define DIR_LEFT   26   // DIR motor kiri

#define STEP_RIGHT 27   // STEP motor kanan
#define DIR_RIGHT  14   // DIR motor kanan

MPU6050 mpu;

float Kp = 10.0;   // Gain proporsional untuk balancing

enum Mode {BALANCING, MAJU, MUNDUR, RESET_POSISI};
Mode mode = BALANCING;

void setup() {
  Serial.begin(115200);
  Wire.begin(); // SDA=21, SCL=22 default ESP32

  pinMode(STEP_LEFT, OUTPUT);
  pinMode(DIR_LEFT, OUTPUT);
  pinMode(STEP_RIGHT, OUTPUT);
  pinMode(DIR_RIGHT, OUTPUT);

  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while(1);
  }
  Serial.println("MPU6050 ready");
  Serial.println("Ketik 'b' untuk Balancing, 'f' untuk Maju, 'r' untuk Mundur, 'z' untuk Reset Posisi");
}

void loop() {
  // Cek input serial untuk ganti mode
  if (Serial.available()) {
    char c = Serial.read();
    if (c == 'b') {
      mode = BALANCING;
      Serial.println("Mode: BALANCING");
    }
    else if (c == 'f') {
      mode = MAJU;
      Serial.println("Mode: MAJU");
    }
    else if (c == 'r') {
      mode = MUNDUR;
      Serial.println("Mode: MUNDUR");
    }
    else if (c == 'z') {
      mode = RESET_POSISI;
      Serial.println("Mode: RESET POSISI");
    }
  }

  switch(mode) {
    case BALANCING: {
      int16_t gx, gy, gz;
      mpu.getRotation(&gx, &gy, &gz);
      float gyroX = gx / 131.0;  // konversi ke deg/detik

      float output = gyroX * Kp;

      bool dirLeft = output > 0;
      bool dirRight = output < 0;

      digitalWrite(DIR_LEFT, dirLeft);
      digitalWrite(DIR_RIGHT, dirRight);

      int steps = min(abs((int)output), 200);

      Serial.print("Mode BALANCING | gyroX: "); Serial.print(gyroX, 2);
      Serial.print(" | Output: "); Serial.print(output, 2);
      Serial.print(" | Motor kiri: "); Serial.print(dirLeft ? "Maju" : "Berhenti");
      Serial.print(" | Motor kanan: "); Serial.println(dirRight ? "Maju" : "Berhenti");

      for (int i = 0; i < steps; i++) {
        digitalWrite(STEP_LEFT, HIGH);
        digitalWrite(STEP_RIGHT, HIGH);
        delayMicroseconds(800);
        digitalWrite(STEP_LEFT, LOW);
        digitalWrite(STEP_RIGHT, LOW);
        delayMicroseconds(800);
      }
      break;
    }

    case MAJU: {
      digitalWrite(DIR_LEFT, HIGH);
      digitalWrite(DIR_RIGHT, HIGH);

      Serial.println("Mode MAJU | Motor kiri dan kanan maju");

      for (int i = 0; i < 200; i++) {
        digitalWrite(STEP_LEFT, HIGH);
        digitalWrite(STEP_RIGHT, HIGH);
        delayMicroseconds(800);
        digitalWrite(STEP_LEFT, LOW);
        digitalWrite(STEP_RIGHT, LOW);
        delayMicroseconds(800);
      }
      break;
    }

    case MUNDUR: {
      digitalWrite(DIR_LEFT, LOW);
      digitalWrite(DIR_RIGHT, LOW);

      Serial.println("Mode MUNDUR | Motor kiri dan kanan mundur");

      for (int i = 0; i < 200; i++) {
        digitalWrite(STEP_LEFT, HIGH);
        digitalWrite(STEP_RIGHT, HIGH);
        delayMicroseconds(800);
        digitalWrite(STEP_LEFT, LOW);
        digitalWrite(STEP_RIGHT, LOW);
        delayMicroseconds(800);
      }
      break;
    }

    case RESET_POSISI: {
      resetPosition(200); // contoh reset dengan 200 langkah mundur
      mode = BALANCING;   // kembali ke mode balancing setelah reset
      break;
    }
  }

  delay(50);
}

void resetPosition(int steps) {
  Serial.println("Reset posisi motor ke 0");

  // Set arah mundur
  digitalWrite(DIR_LEFT, LOW);
  digitalWrite(DIR_RIGHT, LOW);

  for (int i = 0; i < steps; i++) {
    digitalWrite(STEP_LEFT, HIGH);
    digitalWrite(STEP_RIGHT, HIGH);
    delayMicroseconds(800);
    digitalWrite(STEP_LEFT, LOW);
    digitalWrite(STEP_RIGHT, LOW);
    delayMicroseconds(800);
  }

  Serial.println("Reset posisi selesai");
}