#include <Servo.h>
#include <Wire.h>
#include <RTClib.h>

RTC_DS3231 rtc; // Inisialisasi modul RTC

// Deklarasi servo
Servo servoVertical;
Servo servoHorizontal;

// Pin untuk servo dan sensor
int servoVerticalPin = 9;
int servoHorizontalPin = 10;
int sensorBawahPin = A0;
int sensorAtasPin = A1;
int sensorKiriPin = A2;
int sensorKananPin = A3;

// Fungsi keanggotaan kurva segitiga
float tri_mf(float a, float b, float c, float x) {
  return max(min((x - a) / (b - a), (c - x) / (c - b)), 0.0);
}

// Struktur data untuk menyimpan nilai keanggotaan
struct Sensor {
  float rendah;
  float sedang;
  float tinggi;
};

void setup() {
  // Inisialisasi servo
  servoVertical.attach(servoVerticalPin);
  servoHorizontal.attach(servoHorizontalPin);
  // Inisialisasi komunikasi Serial
  Serial.begin(9600);
  // Inisialisasi RTC
  if (!rtc.begin()) {
    Serial.println("RTC tidak ditemukan!");
    while (1);
  }

  if (rtc.lostPower()) {
    Serial.println("RTC kehilangan daya, set ulang waktu!");
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__))); // Set waktu berdasarkan waktu kompilasi
  }
}

void loop() {
  // Membaca sensor
  int sensorBawah = analogRead(sensorBawahPin);
  int sensorAtas = analogRead(sensorAtasPin);
  int sensorKiri = analogRead(sensorKiriPin);
  int sensorKanan = analogRead(sensorKananPin);

  
  // Fungsi keanggotaan untuk sensor bawah
  Sensor bawah;
  bawah.rendah = tri_mf(0, 0, 512, sensorBawah);
  bawah.sedang = tri_mf(255, 512, 768, sensorBawah);
  bawah.tinggi = tri_mf(512, 768, 1023, sensorBawah);

  // Fungsi keanggotaan untuk sensor atas
  Sensor atas;
  atas.rendah = tri_mf(0, 0, 512, sensorAtas);
  atas.sedang = tri_mf(255, 512, 768, sensorAtas);
  atas.tinggi = tri_mf(512, 768, 1023, sensorAtas);

  // Output posisi servo
  float bawahPos = 30;
  float tengahPos = 120;
  float atasPos = 180;

  // Variabel untuk bobot
  float bobot1 = 0, bobot2 = 0, bobot3 = 0;
  float bobot4 = 0, bobot5 = 0, bobot6 = 0;
  float bobot7 = 0, bobot8 = 0, bobot9 = 0;

  // Aturan fuzzy
  if (bawah.rendah > 0 && atas.rendah > 0) bobot1 = min(bawah.rendah, atas.rendah);
  if (bawah.rendah > 0 && atas.sedang > 0) bobot2 = min(bawah.rendah, atas.sedang);
  if (bawah.rendah > 0 && atas.tinggi > 0) bobot3 = min(bawah.rendah, atas.tinggi);

  if (bawah.sedang > 0 && atas.rendah > 0) bobot4 = min(bawah.sedang, atas.rendah);
  if (bawah.sedang > 0 && atas.sedang > 0) bobot5 = min(bawah.sedang, atas.sedang);
  if (bawah.sedang > 0 && atas.tinggi > 0) bobot6 = min(bawah.sedang, atas.tinggi);

  if (bawah.tinggi > 0 && atas.rendah > 0) bobot7 = min(bawah.tinggi, atas.rendah);
  if (bawah.tinggi > 0 && atas.sedang > 0) bobot8 = min(bawah.tinggi, atas.sedang);
  if (bawah.tinggi > 0 && atas.tinggi > 0) bobot9 = min(bawah.tinggi, atas.tinggi);

  // Defuzzifikasi (rata-rata berbobot)
  float weightedSum = (bobot1 * bawahPos) + (bobot2 * tengahPos) + (bobot3 * tengahPos) +
                      (bobot4 * bawahPos) + (bobot5 * tengahPos) + (bobot6 * atasPos) +
                      (bobot7 * bawahPos) + (bobot8 * tengahPos) + (bobot9 * atasPos);

  float sumOfWeights = bobot1 + bobot2 + bobot3 + bobot4 + bobot5 + bobot6 + bobot7 + bobot8 + bobot9;
  float servoPos = (sumOfWeights > 0) ? (weightedSum / sumOfWeights) : 90; // Default posisi tengah

// SERVO HORIZONTAL
//fungsi keanggotaan untuk sensor kiri
  Sensor kiri;
  kiri.rendah = tri_mf(0, 0, 512, sensorKiri);
  kiri.sedang = tri_mf(255, 512, 768, sensorKiri);
  kiri.tinggi = tri_mf(512, 768, 1023, sensorKiri);

  //fungsi keanggotaan untuk sensor kanan
  Sensor kanan;
  kanan.rendah = tri_mf(0, 0, 512, sensorKanan);
  kanan.sedang = tri_mf(255, 512, 768, sensorKanan);
  kanan.tinggi = tri_mf(512, 768, 1023, sensorKanan);

  float bobot10 = 0, bobot11 = 0, bobot12 = 0, bobot13 = 0, bobot14 = 0;
  float bobot15 = 0, bobot16 = 0, bobot17 = 0, bobot18 = 0;
//aturan fuzzy untuk servo horizontal
  if (kanan.rendah > 0 && kiri.rendah > 0) bobot10 = min(kanan.rendah, kiri.rendah);
  if (kanan.rendah > 0 && kiri.sedang > 0) bobot11 = min(kanan.rendah, kiri.sedang);
  if (kanan.rendah > 0 && kiri.tinggi > 0) bobot12 = min(kanan.rendah, kiri.tinggi);

  if (kanan.sedang > 0 && kiri.rendah > 0) bobot13 = min(kanan.sedang, kiri.rendah);
  if (kanan.sedang > 0 && kiri.sedang > 0) bobot14 = min(kanan.sedang, kiri.sedang);
  if (kanan.sedang > 0 && atas.tinggi > 0) bobot15 = min(kanan.sedang, kiri.tinggi);

  if (kanan.tinggi > 0 && kiri.rendah > 0) bobot16 = min(kanan.tinggi, kiri.rendah);
  if (kanan.tinggi > 0 && kiri.sedang > 0) bobot17 = min(kanan.tinggi, kiri.sedang);
  if (kanan.tinggi > 0 && kiri.tinggi > 0) bobot18 = min(kanan.tinggi, kiri.tinggi);

  float weightedSumm = (bobot10 * atasPos) + (bobot11 * tengahPos) + (bobot12 * bawahPos) +
                      (bobot13 * atasPos) + (bobot14 * tengahPos) + (bobot15 * bawahPos) +
                      (bobot16 * atasPos) + (bobot17 * tengahPos) + (bobot18 * bawahPos);
float sumOfWeightss = bobot10 + bobot11 + bobot12 + bobot13 + bobot14 + bobot15 + bobot16 + bobot17 + bobot18;
float servoPosHorizontal = (sumOfWeightss > 0) ?(weightedSumm / sumOfWeightss): 90; // Default posisi tengah



  // Set posisi servo
  servoVertical.write(servoPos);
  servoHorizontal.write(servoPosHorizontal);

  delay(50);


  // Tampilkan data pada Serial Monitor
  Serial.print("Sensor kiri: ");
  Serial.print(sensorKiri);
  Serial.print(" | Sensor kanan: ");
  Serial.print(sensorKanan);
  Serial.print(" | Servo Pos: ");
  Serial.println(servoPosHorizontal);

  //PEMBACAAN DATA TEGANGAN PANEL SURYA/BATERAI
  DateTime now = rtc.now();

  // Baca tegangan dari pin analog A4
  int sensorValue = analogRead(A4);
  float voltage = sensorValue * (5.0 / 1023.0); // Konversi ke tegangan dalam volt

  // Tampilkan waktu dan tegangan di Serial Monitor
  Serial.print("Pukul ");
  if (now.hour() < 10) Serial.print("0"); // Tambahkan 0 jika jam kurang dari 10
  Serial.print(now.hour());
  Serial.print(":");
  if (now.minute() < 10) Serial.print("0"); // Tambahkan 0 jika menit kurang dari 10
  Serial.print(now.minute());
  Serial.print(", Tegangan  = ");
  Serial.print(voltage, 2); // Tampilkan tegangan dengan 2 angka di belakang koma
  Serial.println("V");

  delay(60); // Tunggu 1 menit sebelum pembacaan berikutnya


  
}