#include <Fuzzy.h>
 
Fuzzy *fuzzy = new Fuzzy();
const int inputPin = A0;
const int outputPin = 3;
void setup() {
  // put your setup code here, to run once:
  pinMode(inputPin, INPUT);
  pinMode(outputPin, OUTPUT);

  Serial.begin(9600);

  FuzzyInput *distance = new FuzzyInput(1);
  FuzzySet *small = new FuzzySet(0, 40, 40, 80);
  distance->addFuzzySet(small);

  FuzzySet *safe = new FuzzySet(60, 100, 100, 140);
  distance->addFuzzySet(safe);

  FuzzySet *big = new FuzzySet(80, 400, 400, 700);
  distance->addFuzzySet(big);

  fuzzy->addFuzzyInput(distance); 

  FuzzyOutput *speed = new FuzzyOutput(1);
  FuzzySet *slow = new FuzzySet(0, 10, 10, 20);
  speed->addFuzzySet(slow);

  FuzzySet *average = new FuzzySet(10, 20, 20, 40);
  speed->addFuzzySet(average);

  FuzzySet *fast = new FuzzySet(30, 40, 40, 50);
  speed->addFuzzySet(fast);

  fuzzy->addFuzzyOutput(speed);

  FuzzyRuleAntecedent *ifDistanceSmall = new FuzzyRuleAntecedent();
  ifDistanceSmall->joinSingle(small);
  FuzzyRuleConsequent *thenSpeedSlow = new FuzzyRuleConsequent();
  thenSpeedSlow->addOutput(slow);
  FuzzyRule *fuzzyRule01 = new FuzzyRule(1, ifDistanceSmall, thenSpeedSlow);

  fuzzy->addFuzzyRule(fuzzyRule01);

  FuzzyRuleAntecedent *ifDistanceSafe = new FuzzyRuleAntecedent();
  ifDistanceSafe->joinSingle(safe);
  FuzzyRuleConsequent *thenSpeedAverage = new FuzzyRuleConsequent();
  thenSpeedAverage->addOutput(average);
  FuzzyRule *fuzzyRule02 = new FuzzyRule(2, ifDistanceSafe, thenSpeedAverage);

  fuzzy->addFuzzyRule(fuzzyRule02);

  FuzzyRuleAntecedent *ifDistanceBig = new FuzzyRuleAntecedent();
  ifDistanceBig->joinSingle(big);
  FuzzyRuleConsequent *thenSpeedFast = new FuzzyRuleConsequent();
  thenSpeedFast->addOutput(fast);
  FuzzyRule *fuzzyRule03 = new FuzzyRule(3, ifDistanceBig, thenSpeedFast);

  fuzzy->addFuzzyRule(fuzzyRule03);
}

void loop() {
  // put your main code here, to run repeatedly:
  int input = analogRead(inputPin);

  Serial.println("\n\n\nEntance: ");
  Serial.print("\t\t\tDistance: ");
  Serial.println(input);

  fuzzy->setInput(1, input);
  fuzzy->fuzzify();

  float output = fuzzy->defuzzify(1);

  analogWrite(outputPin, output);
  Serial.println("Result: ");
  Serial.print("\t\t\tSpeed");
  Serial.println(output);
  delay(2000);
}