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

const int stepsPerRevolution = 200; // Devrim başına adım sayısı
const float degreesPerStep = 360.0 / stepsPerRevolution; // Her adımın açısal değeri

// Motor pin numaraları
const int dirPinZ = 2;
const int stepPinZ = 3;

const int dirPinX = 13; 
const int stepPinX = 11; 

// MPU6050 sensör nesnesini başlat
MPU6050 mpu;

// Stepper motor nesnelerini başlat
Stepper myStepperZ(stepsPerRevolution, dirPinZ, stepPinZ);
Stepper myStepperX(stepsPerRevolution, dirPinX, stepPinX);

// Önceki Z ve X rotasyon değerleri
float prevRotationZ = 0.0;
float prevRotationX = 0.0;

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

  // MPU6050 sensörünü başlat
  mpu.initialize();

  // İleri veya geri dönüşü belirlemek için eşik değerlerini ayarla
  mpu.setZAccelOffset(0);
  mpu.setXAccelOffset(0);
}

void loop() {
  // Z-ekseni rotasyon değerini oku
  float rotationZ = calculateRotationZ();

  // Önceki Z rotasyon değeri ile mevcut değer arasındaki farkı hesapla
  float deltaRotationZ = rotationZ - prevRotationZ;

  // Her adım için bir açısal değer hesapla ve motoru döndür
  myStepperZ.step(static_cast<int>(deltaRotationZ * degreesPerStep));

  // X-ekseni rotasyon değerini oku
  float rotationX = calculateRotationX();

  // Önceki X rotasyon değeri ile mevcut değer arasındaki farkı hesapla
  float deltaRotationX = rotationX - prevRotationX;

  // Her adım için bir açısal değer hesapla ve motoru döndür
  myStepperX.step(static_cast<int>(deltaRotationX * degreesPerStep));

  // Önceki Z ve X rotasyon değerlerini güncelle
  prevRotationZ = rotationZ;
  prevRotationX = rotationX;

  // Referans açısını ekleyerek +180 ile -180 arasında sınırla
  float boundedRotationZ = rotationZ;
  float boundedRotationX = rotationX;

  // Elde edilen Z ve X rotasyon değerlerini Serial Monitor'a yazdır
  Serial.println("Z ekseninde dönme açısı: " + String(boundedRotationZ/1.2421) + " derece");
  Serial.println("X ekseninde dönme açısı: " + String(boundedRotationX/1.2421) + " derece");

  // Döngünün makul bir hızda çalışmasına izin vermek için gecikme
  delay(500);
}

float calculateRotationZ() {
  // Z eksenindeki rotasyonu yaklaşık olarak hesapla
  float gyroZ = mpu.getRotationZ();
  
  // Açı değerini atan2 fonksiyonu ile hesapla ve dereceye çevir
  return atan2(gyroZ, 32768.0) * 497 / PI;
}

float calculateRotationX() {
  // X eksenindeki rotasyonu yaklaşık olarak hesapla
  float gyroX = mpu.getRotationX();
  
  // Açı değerini atan2 fonksiyonu ile hesapla ve dereceye çevir
  return atan2(gyroX, 32768.0) * 497 / PI;
}