#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;

// Zaman ölçümü için değişkenler
unsigned long prevMillis = 0;
const int loopInterval = 100; // Döngü aralığı (ms)

// Fonksiyon prototipleri
float calculateRotationZ();
float calculateRotationX();

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(1);
  mpu.setXAccelOffset(1);
}

void loop() {
  // Döngü süresini kontrol et
  unsigned long currentMillis = millis();
  if (currentMillis - prevMillis >= loopInterval) {
    // Z-ekseni rotasyon değerini oku
    float rotationZ = calculateRotationZ();

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

    // Her adım için bir açısal değer hesapla ve motorları döndür
    myStepperZ.step(static_cast<int>(rotationZ * degreesPerStep));
    myStepperX.step(static_cast<int>(rotationX * 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 + 180.0;
    float boundedRotationX = rotationX + 180.0;

    boundedRotationZ = fmod(boundedRotationZ, 360.0); // Değerin 360'ye göre modunu al
    boundedRotationX = fmod(boundedRotationX, 360.0); // Değerin 360'ye göre modunu al

    if (boundedRotationZ < 0) {
      boundedRotationZ += 360.0; // Negatif değerleri pozitife çevir
    }

    if (boundedRotationX < 0) {
      boundedRotationX += 360.0; // Negatif değerleri pozitife çevir
    }

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

    // Zamanlayıcıyı güncelle
    prevMillis = currentMillis;
  }
}

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;
}