#include <Fuzzy.h>

// Instantiating a Fuzzy object
Fuzzy *fuzzy = new Fuzzy();
const int inputPin = A0;
const int outputPin = 3;
void setup()
{
pinMode(inputPin, INPUT);
pinMode(outputPin, OUTPUT);
// Set the Serial output
Serial.begin(9600);
// Set a random seed
// randomSeed (analogRead(0));

// Instantiating a FuzzyInput object
FuzzyInput *distance = new FuzzyInput(1);
// Instantiating a FuzzySet object
FuzzySet *small = new FuzzySet(0, 40, 40, 80);
// Including the FuzzySet into Fuzzy Input
distance->addFuzzySet(small);
// Instantiating a FuzzySet object
FuzzySet *safe = new FuzzySet(60, 100, 100, 140);
// Including the FuzzySet into FuzzyInput
distance->addFuzzySet(safe);
// Instantiating a FuzzySet object 
FuzzySet *big = new FuzzySet(120, 160, 160, 200);
// Including the PuzzySet into FuzzyInput
distance->addFuzzySet(big);
// Including the FuzzyInput into Fuzzy
fuzzy->addFuzzyInput(distance);
// Instantiating a FuzzyOutput objects

FuzzyOutput *speed = new FuzzyOutput(1);
// Instantiating a FuzzySet object 
FuzzySet *slow = new FuzzySet(0, 10, 10, 20);
// Including the FuzzySet into FuzzyOutput
speed->addFuzzySet(slow);
// Instantiating a FuzzySet object M
FuzzySet *average = new FuzzySet(10, 20, 20, 40);
// Including the FuzzySet into FuzzyOutput
speed->addFuzzySet(average);
// Instantiating a FuzzySet object
FuzzySet *fast = new FuzzySet(30, 40, 40, 50);
// Including the FuzzySet into FuzzyOutput
speed->addFuzzySet(fast);
// Including the FuzzyOutput into Fuzzy
fuzzy->addFuzzyOutput(speed);

// Building FuzzyRule "IF distance = small THEN speed = slow"
// Instantiating a FuzzyRuleAntecedent objects
FuzzyRuleAntecedent *ifDistanceSmall = new FuzzyRuleAntecedent();
// Creating a FuzzyRuleAntecedent with just a single FuzzySet 
ifDistanceSmall->joinSingle(small);
// Instantiating a FuzzyRuleConsequent objects 
FuzzyRuleConsequent *thenSpeedSlow = new FuzzyRuleConsequent();
// Including a FuzzySet to this FuzzyRuleConsequent
thenSpeedSlow->addOutput(slow);
// Instantiating a FuzzyRule objects
FuzzyRule *fuzzyRule01 = new FuzzyRule(1, ifDistanceSmall, thenSpeedSlow);
// Including the FuzzyRule into Fuzzy
fuzzy->addFuzzyRule(fuzzyRule01);

// Building FuzzyRule "IF distance = safe THEN speed = average"
// Instantiating a FuzzyRuleAntecedent objects
FuzzyRuleAntecedent *ifDistanceSafe = new FuzzyRuleAntecedent();
// Creating a FuzzyRuleAntecedent with just a single FuzzySet 
ifDistanceSafe->joinSingle(safe);
// Instantiating a FuzzyRuleConsequent objects
FuzzyRuleConsequent *thenSpeedAverage = new FuzzyRuleConsequent();
// Including a FuzzySet to this FuzzyRuleConsequent
thenSpeedAverage->addOutput(average);
// Instantiating a FuzzyRule objects
FuzzyRule *fuzzyRule02 = new FuzzyRule(2, ifDistanceSafe, thenSpeedAverage);
// Including the FuzzyRule into Fuzzy
fuzzy->addFuzzyRule(fuzzyRule02);

// Building FuzzyRule "IF distance = big THEN speed = high"
// Instantiating a FuzzyRuleAntecedent objects
FuzzyRuleAntecedent *ifDistanceBIg = new FuzzyRuleAntecedent();
// Creating a FuzzyRuleAntecedent with just a single FuzzySet
ifDistanceSafe->joinSingle(big);
// Instantiating a FuzzyRuleConsequent objects
FuzzyRuleConsequent *thenSpeedFast = new FuzzyRuleConsequent();
// Including a FuzzySet to this FuzzyRuleConsequent
thenSpeedFast->addOutput(fast);
// Instantiating a FuzzyRule objects
FuzzyRule *fuzzyRule03 = new FuzzyRule(3, ifDistanceBIg, thenSpeedFast);
// Including the FuzzyRule into Fuzzy
fuzzy->addFuzzyRule(fuzzyRule03);
}

void loop()
{
// Getting a random value
int input = analogRead(inputPin);
// Printing something
Serial.println("\n\n\nEntrance: ");
Serial.print("\t\t\tDistance: ");
Serial.println(input);
// Set the random value as an input
fuzzy->setInput(1, input);
// Running the Fuzzification
fuzzy->fuzzify();
// Running the Defuzzification
float output = fuzzy->defuzzify(1);
// Printing something
analogWrite(outputPin, output);
Serial.println("Result: ");
Serial.print("\t\t\tSpeed: ");
Serial.println(output);
// wait 12 seconds
delay(5000);
}