#include <Fuzzy.h>

Fuzzy *fuzzy = new Fuzzy();
const int trigPin1 = 4; // Trigger pin for Ultrasonic Sensor 1
const int echoPin1 = 5; // Echo pin for Ultrasonic Sensor 1
const int trigPin2 = 6; // Trigger pin for Ultrasonic Sensor 2
const int echoPin2 = 7; // Echo pin for Ultrasonic Sensor 2
const int ledPin1 = 11; // PWM pin for LED 1
const int ledPin2 = 12; // PWM pin for LED 2

void setup() {
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);
  
  Serial.begin(9600);

  // Define Distance input fuzzy sets
  FuzzyInput *distance1 = new FuzzyInput(1);
  FuzzySet *dekat1 = new FuzzySet(0, 5, 5, 10);
  distance1->addFuzzySet(dekat1);
  FuzzySet *normal1 = new FuzzySet(8, 15, 15, 20);
  distance1->addFuzzySet(normal1);
  FuzzySet *jauh1 = new FuzzySet(18, 25, 25, 30);
  distance1->addFuzzySet(jauh1);
  fuzzy->addFuzzyInput(distance1);

  // Define Distance input fuzzy sets for second sensor
  FuzzyInput *distance2 = new FuzzyInput(2);
  FuzzySet *dekat2 = new FuzzySet(0, 5, 5, 10);
  distance2->addFuzzySet(dekat2);
  FuzzySet *normal2 = new FuzzySet(8, 15, 15, 20);
  distance2->addFuzzySet(normal2);
  FuzzySet *jauh2 = new FuzzySet(18, 25, 25, 30);
  distance2->addFuzzySet(jauh2);
  fuzzy->addFuzzyInput(distance2);

  // Define LED brightness output fuzzy sets
  FuzzyOutput *ledBrightness1 = new FuzzyOutput(1);
  FuzzySet *agakTerang1 = new FuzzySet(0, 35, 35, 85);
  ledBrightness1->addFuzzySet(agakTerang1);
  FuzzySet *lumayanTerang1 = new FuzzySet(65, 120, 120, 170);
  ledBrightness1->addFuzzySet(lumayanTerang1);
  FuzzySet *terangSekali1 = new FuzzySet(150, 200, 200, 255);
  ledBrightness1->addFuzzySet(terangSekali1);
  fuzzy->addFuzzyOutput(ledBrightness1);

  // Define LED brightness output fuzzy sets for second LED
  FuzzyOutput *ledBrightness2 = new FuzzyOutput(2);
  FuzzySet *agakTerang2 = new FuzzySet(0, 35, 35, 85);
  ledBrightness2->addFuzzySet(agakTerang2);
  FuzzySet *lumayanTerang2 = new FuzzySet(65, 120, 120, 170);
  ledBrightness2->addFuzzySet(lumayanTerang2);
  FuzzySet *terangSekali2 = new FuzzySet(150, 200, 200, 255);
  ledBrightness2->addFuzzySet(terangSekali2);
  fuzzy->addFuzzyOutput(ledBrightness2);

  // Define Fuzzy Rules
  // Rules for LED 1
  FuzzyRuleAntecedent *ifDistance1Dekat = new FuzzyRuleAntecedent();
  ifDistance1Dekat->joinSingle(dekat1);
  FuzzyRuleConsequent *thenBrightness1AgakTerang = new FuzzyRuleConsequent();
  thenBrightness1AgakTerang->addOutput(agakTerang1);
  fuzzy->addFuzzyRule(new FuzzyRule(1, ifDistance1Dekat, thenBrightness1AgakTerang));

  FuzzyRuleAntecedent *ifDistance1Normal = new FuzzyRuleAntecedent();
  ifDistance1Normal->joinSingle(normal1);
  FuzzyRuleConsequent *thenBrightness1LumayanTerang = new FuzzyRuleConsequent();
  thenBrightness1LumayanTerang->addOutput(lumayanTerang1);
  fuzzy->addFuzzyRule(new FuzzyRule(2, ifDistance1Normal, thenBrightness1LumayanTerang));

  FuzzyRuleAntecedent *ifDistance1Jauh = new FuzzyRuleAntecedent();
  ifDistance1Jauh->joinSingle(jauh1);
  FuzzyRuleConsequent *thenBrightness1TerangSekali = new FuzzyRuleConsequent();
  thenBrightness1TerangSekali->addOutput(terangSekali1);
  fuzzy->addFuzzyRule(new FuzzyRule(3, ifDistance1Jauh, thenBrightness1TerangSekali));

  // Rules for LED 2
  FuzzyRuleAntecedent *ifDistance2Dekat = new FuzzyRuleAntecedent();
  ifDistance2Dekat->joinSingle(dekat2);
  FuzzyRuleConsequent *thenBrightness2AgakTerang = new FuzzyRuleConsequent();
  thenBrightness2AgakTerang->addOutput(agakTerang2);
  fuzzy->addFuzzyRule(new FuzzyRule(4, ifDistance2Dekat, thenBrightness2AgakTerang));

  FuzzyRuleAntecedent *ifDistance2Normal = new FuzzyRuleAntecedent();
  ifDistance2Normal->joinSingle(normal2);
  FuzzyRuleConsequent *thenBrightness2LumayanTerang = new FuzzyRuleConsequent();
  thenBrightness2LumayanTerang->addOutput(lumayanTerang2);
  fuzzy->addFuzzyRule(new FuzzyRule(5, ifDistance2Normal, thenBrightness2LumayanTerang));

  FuzzyRuleAntecedent *ifDistance2Jauh = new FuzzyRuleAntecedent();
  ifDistance2Jauh->joinSingle(jauh2);
  FuzzyRuleConsequent *thenBrightness2TerangSekali = new FuzzyRuleConsequent();
  thenBrightness2TerangSekali->addOutput(terangSekali2);
  fuzzy->addFuzzyRule(new FuzzyRule(6, ifDistance2Jauh, thenBrightness2TerangSekali));
}

void loop() {
  // Measure distance for Sensor 1
  long duration1, distance1;
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  duration1 = pulseIn(echoPin1, HIGH);
  distance1 = duration1 * 0.034 / 2; // Convert to cm

  // Measure distance for Sensor 2
  long duration2, distance2;
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  duration2 = pulseIn(echoPin2, HIGH);
  distance2 = duration2 * 0.034 / 2; // Convert to cm

  // Fuzzify inputs
  fuzzy->setInput(1, distance1);
  fuzzy->setInput(2, distance2);
  fuzzy->fuzzify();

  // Defuzzify outputs
  float output1 = fuzzy->defuzzify(1);
  float output2 = fuzzy->defuzzify(2);

  // Write PWM values to LEDs
  analogWrite(ledPin1, output1);
  analogWrite(ledPin2, output2);

  // Print results to Serial Monitor
  Serial.print("Distance 1: ");
  Serial.print(distance1);
  Serial.print(" cm, LED 1 Output: ");
  Serial.println(output1);
  
  Serial.print("Distance 2: ");
  Serial.print(distance2);
  Serial.print(" cm, LED 2 Output: ");
  Serial.println(output2);

  delay(2000);
}