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

// Pin definisi
#define STEP_PIN_1 2
#define DIR_PIN_1 3
#define STEP_PIN_2 4
#define DIR_PIN_2 5
#define BLUETOOTH_RX 6
#define BLUETOOTH_TX 7

// Objek
MPU6050 mpu;
SoftwareSerial bluetooth(BLUETOOTH_RX, BLUETOOTH_TX);

// Variabel untuk MPU6050
int16_t ax, ay, az;
int16_t gx, gy, gz;
float angle;

// Variabel PID
double setpoint = 0;
double input, output;
double Kp = 15, Ki = 0.1, Kd = 0.5;
PID_v2 pid(Kp, Ki, Kd, PID::Direct);

// Variabel kontrol
int speed = 0;
int turn = 0;

// Variabel untuk timing
unsigned long lastPrint = 0;
const long printInterval = 100; // Interval print 100ms

void setup() {
  Serial.begin(9600);
  bluetooth.begin(9600);
  
  // Inisialisasi MPU6050
  Wire.begin();
  mpu.initialize();
  
  // Konfigurasi pin
  pinMode(STEP_PIN_1, OUTPUT);
  pinMode(DIR_PIN_1, OUTPUT);
  pinMode(STEP_PIN_2, OUTPUT);
  pinMode(DIR_PIN_2, OUTPUT);
  
  // Konfigurasi PID
  pid.SetMode(PID::Automatic);
  pid.SetSampleTime(10);
  pid.SetOutputLimits(-255, 255);

  Serial.println("Inisialisasi selesai");
}

void loop() {
  // Baca data dari MPU6050
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  angle = atan2(ay, az) * 180 / PI;
  
  // Hitung PID
  input = angle;
  output = pid.Run(input);
  
  // Kontrol motor
  int leftMotorSpeed = output + turn;
  int rightMotorSpeed = output - turn;
  
  // Gerakkan motor
  moveStepperMotor(1, leftMotorSpeed);
  moveStepperMotor(2, rightMotorSpeed);
  
  // Baca perintah Bluetooth
  if (bluetooth.available()) {
    char cmd = bluetooth.read();
    switch (cmd) {
      case 'F': speed += 10; break;
      case 'B': speed -= 10; break;
      case 'L': turn -= 10; break;
      case 'R': turn += 10; break;
      case 'S': speed = 0; turn = 0; break;
    }
    speed = constrain(speed, -100, 100);
    turn = constrain(turn, -50, 50);
  }

  // Print sensor data
  printSensorData();
}

void moveStepperMotor(int motor, int speed) {
  int stepPin = (motor == 1) ? STEP_PIN_1 : STEP_PIN_2;
  int dirPin = (motor == 1) ? DIR_PIN_1 : DIR_PIN_2;
  
  digitalWrite(dirPin, speed > 0 ? HIGH : LOW);
  
  int absSpeed = abs(speed);
  if (absSpeed > 0) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(500 - absSpeed * 2);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(500 - absSpeed * 2);
  }
}

void printSensorData() {
  unsigned long currentMillis = millis();
  if (currentMillis - lastPrint >= printInterval) {
    lastPrint = currentMillis;
    
    Serial.print("Angle: ");
    Serial.print(angle);
    Serial.print("\tAx: ");
    Serial.print(ax);
    Serial.print("\tAy: ");
    Serial.print(ay);
    Serial.print("\tAz: ");
    Serial.print(az);
    Serial.print("\tGx: ");
    Serial.print(gx);
    Serial.print("\tGy: ");
    Serial.print(gy);
    Serial.print("\tGz: ");
    Serial.print(gz);
    Serial.print("\tOutput: ");
    Serial.println(output);
  }
}