#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <MPU6050.h>
#include <Servo.h>
#include <SD.h>

// OLED display configuration
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// MPU6050 configuration
MPU6050 mpu;

// SD card configuration
const int chipSelect = 10;

// Servo configuration
Servo servo1;
Servo servo2;

void setup() {
  // Initialize serial communication
  Serial.begin(115200);

  // Initialize I2C communication for MPU6050
  Wire.begin();
  mpu.initialize();

  if (!mpu.testConnection()) {
    Serial.println(F("MPU6050 connection failed"));
    while (1);
  }

  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.display();
  delay(2000);
  display.clearDisplay();

  // Initialize SD card
  if (!SD.begin(chipSelect)) {
    Serial.println("SD card initialization failed!");
    return;
  }

  // Attach servo motors to PWM pins
  servo1.attach(9);
  servo2.attach(10);
}

void loop() {
  // Read MPU6050 data
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float accelX = ax / 16384.0;
  float accelY = ay / 16384.0;
  float accelZ = az / 16384.0;
  float gyroX = gx / 131.0;
  float gyroY = gy / 131.0;
  float gyroZ = gz / 131.0;
  float temperature = mpu.getTemperature() / 340.00 + 36.53;

  // Display data on OLED
  displayData(accelX, accelY, accelZ, gyroX, gyroY, gyroZ, temperature);

  // Save data to SD card
  saveData(accelX, accelY, accelZ, gyroX, gyroY, gyroZ, temperature);

  // Control servo motors
  controlServos(gyroX, gyroY);

  // Delay for a while
  delay(1000);
}

void displayData(float accelX, float accelY, float accelZ, float gyroX, float gyroY, float gyroZ, float temp) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  display.setCursor(0, 0);
  display.print("Accelerometer (m/s^2):");
  display.setCursor(0, 10);
  display.print(accelX);
  display.print(", ");
  display.print(accelY);
  display.print(", ");
  display.print(accelZ);

  display.setCursor(0, 20);
  display.print("Gyroscope (rps):");
  display.setCursor(0, 30);
  display.print(gyroX);
  display.print(", ");
  display.print(gyroY);
  display.print(", ");
  display.print(gyroZ);

  display.setCursor(0, 40);
  display.print("Temperature (degC):");
  display.setCursor(0, 50);
  display.print(temp);

  display.display();

  Serial.print("Accelerometer - m/s^2: ");
  Serial.print(accelX);
  Serial.print(", ");
  Serial.print(accelY);
  Serial.print(", ");
  Serial.print(accelZ);
  Serial.println();

  Serial.print("Gyroscope - rps: ");
  Serial.print(gyroX);
  Serial.print(", ");
  Serial.print(gyroY);
  Serial.print(", ");
  Serial.print(gyroZ);
  Serial.println();

  Serial.print("Temperature: ");
  Serial.print(temp);
  Serial.println(" degC");
}

void saveData(float accelX, float accelY, float accelZ, float gyroX, float gyroY, float gyroZ, float temp) {
  // SD karta verilerin yazdırılması
  File dataFile = SD.open("mpu6050.txt", FILE_WRITE);
  if (dataFile) {
    dataFile.print("İvmeölçer - m/s^2: "); // Başlık yazdırılır.
    dataFile.print(accelX); // X eksenindeki ivmeölçer değeri yazdırılır.
    dataFile.print(", "); // Virgül yazdırılır.
    dataFile.print(accelY); // Y eksenindeki ivmeölçer değeri yazdırılır.
    dataFile.print(", "); // Virgül yazdırılır.
    dataFile.print(accelZ); // Z eksenindeki ivmeölçer değeri yazdırılır.
    dataFile.println(); // Yeni satıra geçilir.

    dataFile.print("Gyroskop - rps: "); // Başlık yazdırılır.
    dataFile.print(gyroX); // X eksenindeki gyroskop değeri yazdırılır.
    dataFile.print(", "); // Virgül yazdırılır.
    dataFile.print(gyroY); // Y eksenindeki gyroskop değeri yazdırılır.
    dataFile.print(", "); // Virgül yazdırılır.
    dataFile.print(gyroZ); // Z eksenindeki gyroskop değeri yazdırılır.
    dataFile.println(); // Yeni satıra geçilir.

    dataFile.print("Sıcaklık: "); // Başlık yazdırılır.
    dataFile.print(temp); // Sıcaklık değeri yazdırılır.
    dataFile.println(" dereceC"); // DereceC birimiyle yeni satıra geçilir.

    dataFile.close(); // Dosya kapatılır.

    // SD karta yazılan verilerin seri porta yazdırılması
    Serial.println("SD karta yazılan ölçümler:"); // Başlık yazdırılır.
    Serial.print("İvmeölçer - m/s^2: "); // Başlık yazdırılır.
    Serial.print(accelX); // X eksenindeki ivmeölçer değeri yazdırılır.
    Serial.print(", "); // Virgül yazdırılır.
    Serial.print(accelY); // Y eksenindeki ivmeölçer değeri yazdırılır.
    Serial.print(", "); // Virgül yazdırılır.

    Serial.print(", ");
    Serial.print(accelZ);
    Serial.println();

    Serial.print("Gyroscope - rps: ");
    Serial.print(gyroX);
    Serial.print(", ");
    Serial.print(gyroY);
    Serial.print(", ");
    Serial.print(gyroZ);
    Serial.println();

    Serial.print("Temperature: ");
    Serial.print(temp);
    Serial.println(" degC");
  } else {
    Serial.println("Error opening mpu6050.txt");
  }
}

void controlServos(float gyroX, float gyroY) {
  int servo1Angle = map(gyroX, -250, 250, 0, 180);
  int servo2Angle = map(gyroY, -250, 250, 0, 180);

  servo1.write(servo1Angle);
  servo2.write(servo2Angle);
}