#include <ESP32Servo.h>
#include <Fuzzy.h>
#include <DHT.h>
#include <NewPing.h>
#include <Stepper.h>

// Konfigurasi pin
#define DHTPIN 2
#define DHTTYPE DHT11
#define TRIGGER_PIN 16
#define ECHO_PIN 4
#define MAX_DISTANCE 200
#define SERVO_PIN 5
#define STEPPER_PIN1 32
#define STEPPER_PIN2 33
#define STEPPER_PIN3 25
#define STEPPER_PIN4 26
#define STEPS_PER_REVOLUTION 2048

DHT dht(DHTPIN, DHTTYPE);
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
Servo fanServo;
Stepper fanStepper(STEPS_PER_REVOLUTION, STEPPER_PIN1, STEPPER_PIN2, STEPPER_PIN3, STEPPER_PIN4);

Fuzzy *fuzzy = new Fuzzy();

// Membership functions
FuzzySet *cold = new FuzzySet(10, 15, 25, 30);
FuzzySet *medium = new FuzzySet(25, 35, 45, 55);
FuzzySet *hot = new FuzzySet(50, 60, 70, 75);

FuzzySet *closeDist = new FuzzySet(1, 10, 20, 30);
FuzzySet *mediumDistance = new FuzzySet(25, 40, 60, 75);
FuzzySet *far = new FuzzySet(70, 80, 90, 100);

FuzzySet *smallAngle = new FuzzySet(20, 30, 40, 50);
FuzzySet *mediumAngle = new FuzzySet(45, 60, 70, 80);
FuzzySet *largeAngle = new FuzzySet(75, 85, 90, 100);

FuzzySet *lowSpeed = new FuzzySet(10, 20, 30, 40);
FuzzySet *mediumSpeed = new FuzzySet(35, 50, 60, 70);
FuzzySet *highSpeed = new FuzzySet(65, 80, 90, 100);

void setup() {
  Serial.begin(9600);
  dht.begin();
  fanServo.attach(SERVO_PIN);
  fanStepper.setSpeed(15);  // Default speed

  FuzzyInput *temperature = new FuzzyInput(1);
  temperature->addFuzzySet(cold);
  temperature->addFuzzySet(medium);
  temperature->addFuzzySet(hot);
  fuzzy->addFuzzyInput(temperature);

  FuzzyInput *distance = new FuzzyInput(2);
  distance->addFuzzySet(closeDist);
  distance->addFuzzySet(mediumDistance);
  distance->addFuzzySet(far);
  fuzzy->addFuzzyInput(distance);

  FuzzyOutput *fanAngle = new FuzzyOutput(1);
  fanAngle->addFuzzySet(smallAngle);
  fanAngle->addFuzzySet(mediumAngle);
  fanAngle->addFuzzySet(largeAngle);
  fuzzy->addFuzzyOutput(fanAngle);

  FuzzyOutput *fanSpeed = new FuzzyOutput(2);
  fanSpeed->addFuzzySet(lowSpeed);
  fanSpeed->addFuzzySet(mediumSpeed);
  fanSpeed->addFuzzySet(highSpeed);
  fuzzy->addFuzzyOutput(fanSpeed);

  int ruleNumber = 1;
  FuzzySet *tempSets[] = {cold, medium, hot};
  FuzzySet *distSets[] = {closeDist, mediumDistance, far};
  FuzzySet *angleSets[] = {smallAngle, mediumAngle, largeAngle};
  FuzzySet *speedSets[] = {lowSpeed, mediumSpeed, highSpeed};

  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      FuzzyRuleAntecedent *antecedent = new FuzzyRuleAntecedent();
      antecedent->joinWithAND(tempSets[i], distSets[j]);

      FuzzyRuleConsequent *consequent = new FuzzyRuleConsequent();
      consequent->addOutput(angleSets[(i + j) / 2]);
      consequent->addOutput(speedSets[(i + j) / 2]);

      fuzzy->addFuzzyRule(new FuzzyRule(ruleNumber++, antecedent, consequent));
    }
  }
}

void loop() {
  float temperatureValue = dht.readTemperature();
  float distanceValue = sonar.ping_cm();

  if (isnan(temperatureValue) || distanceValue <= 0 || distanceValue > 100) {
    Serial.println("Sensor error");
    delay(1000);
    return;
  }

  fuzzy->setInput(1, temperatureValue);
  fuzzy->setInput(2, distanceValue);
  fuzzy->fuzzify();

  float angle = fuzzy->defuzzify(1);
  float speed = fuzzy->defuzzify(2);

  // Koreksi nilai agar tidak terlalu kecil
  int correctedAngle = constrain((int)angle, 20, 90); // agar servo bergerak jelas
  int stepsToMove = constrain(map((int)speed, 0, 100, 10, 100), 10, 100);
  int rpm = constrain(map((int)speed, 0, 100, 5, 15), 5, 15);

  fanServo.write(correctedAngle);
  delay(500);  // beri waktu servo untuk bergerak

  fanStepper.setSpeed(rpm);
  fanStepper.step(stepsToMove);
  delay(500);  // beri waktu stepper untuk bergerak

  Serial.print("Suhu: "); Serial.print(temperatureValue);
  Serial.print(" °C, Jarak: "); Serial.print(distanceValue);
  Serial.print(" cm, Sudut: "); Serial.print(correctedAngle);
  Serial.print(" °, Langkah Stepper: "); Serial.println(stepsToMove);

  delay(1000);
}