#include <Servo.h>

// Pin definitions
const int trigPin = 9;
const int echoPin = 10;
const int potPin = A0;
const int servo1Pin = 6;
const int servo2Pin = 2;

Servo servo1;
Servo servo2;

// 50,100,150
// Fuzzy membership functions
float jarakDekat(float x) {
  if (x <= 50) return 1.0;
  else if (x > 50 && x <= 60) return (60 - x) / 20;
  else return 0.0;
}

float jarakSedang(float x) {
  if (x <= 50) return 0.0;
  else if (x > 50 && x <= 130) return (x - 50) / 20;
  else if (x > 130 && x <= 150) return (x-130) / 20;
  else return 0.0;
}

float jarakJauh(float x) {
  if (x <= 130) return 0.0;
  if (x > 130 && x <= 150) return (150-x) / 20;
  else return 1.0;
}

float kecepatanLambat(float x) {
  if (x <= 30) return 1.0;
  else if (x > 30 && x <= 60) return (60 - x) / 10;
  else return 0.0;
}

float KecepatanStabil(float x) {
  if (x <= 30) return 0.0;
  else if (x > 30 && x <= 60) return (x - 30) / 10;
  else if (x > 60 && x <= 70) return (x - 60) / 10;
  else return 0.0;
}

float KecepatanCepat(float x) {
  if (x <= 60) return 0.0;
  if (x > 60 && x <= 70) return (70-x) / 10;
  else return 1.0;
}

// Fungsi defuzzifikasi
float defuzzifikasi(float jarak, float kecepatan) {
  float dekat = jarakDekat(jarak);
  float sedang = jarakSedang(jarak);
  float jauh = jarakJauh(jarak);
  float lambat = kecepatanLambat(kecepatan);
  float stabil = KecepatanStabil(kecepatan);
  float cepat = KecepatanCepat(kecepatan);

  float buka = max(
    max(min(sedang, lambat), min(jauh, lambat)),
    min(jauh, stabil)
  );

  float tutup = max(
    max(max(min(dekat, lambat), min(dekat, stabil)), min(dekat, cepat)),
    max(max(min(sedang, stabil), min(sedang, cepat)), min(jauh, cepat))
  );

  float t1 = tutup * 120 + 80;
  float t2 = buka * 120 + 80;

  float m1 = (tutup * (t1 * t1 / 2)) - (tutup * (0 * 0 / 2));
  float m2 = (((t2 * t2 * t2 / 3) - (80 * (t2 * t2)) / 2) / 120) - (((t1 * t1 * t1 / 3) - (80 * (t1 * t1)) / 2) / 120);
  float m3 = (buka * (200.0 * 200.0 / 2)) - (buka * (t2 * t2 / 2));

  float a1 = (tutup * t1) - (tutup * 0);
  float a2 = ((((t2 * t2) / 2) - (80 * t2)) / 120) - ((((t1 * t1) / 2) - (80 * t1)) / 120);
  float a3 = (buka * 200.0) - (buka * t2);

  if (a1 + a2 + a3 == 0) return 0; // Hindari pembagian dengan nol
  float z = (m1 + m2 + m3) / (a1 + a2 + a3);

  return z;
}
long duration, distance;
// Fungsi membaca jarak dengan sensor ultrasonik
float readDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = (duration / 2) / 29.1;
  return distance;
}

void setup() {
  Serial.begin(9600);
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
}

void loop() {
  // Membaca jarak dengan sensor ultrasonik
  float distance = readDistance();

  // Membaca nilai potensiometer
  int potValue = analogRead(potPin);
  float pot = map(potValue, 0, 1023, 0, 100);

  float z = defuzzifikasi(distance, pot);
  z = constrain(z, 0, 180);

  // Mengatur posisi servo berdasarkan nilai z
  if (z < 80) {
    // Servo pertama bergerak ke 180 derajat
    servo1.write(90);
    servo2.write(90); // Servo kedua bergerak ke posisi berlawanan
  
  } else {
    // Servo pertama bergerak ke 90 derajat
    servo1.write(0);
    servo2.write(180); // Servo kedua bergerak ke posisi tengah
  }


  // Menampilkan nilai jarak, potensiometer, dan z di Serial Monitor
  Serial.println("-------------------------------");
  Serial.print("Jarak: ");
  Serial.println(distance);
  Serial.print("Kecepatan: ");
  Serial.println(pot);
  Serial.print("Output (z): ");
  Serial.println(z);
  Serial.println("-------------------------------");

  // Delay untuk stabilitas
  delay(500);
}