/**
 * fuzzy_controller.ino
 **/

// eFLL includes
#include <Fuzzy.h>
#include <FuzzyComposition.h>
#include <FuzzyInput.h>
#include <FuzzyIO.h>
#include <FuzzyOutput.h>
#include <FuzzyRule.h>
#include <FuzzyRuleAntecedent.h>
#include <FuzzyRuleConsequent.h>
#include <FuzzySet.h>

// pins
#define LDR A0
#define TRIGGER 4
#define ECHO 5
#define LED 6

// object library
Fuzzy *fuzzy = new Fuzzy();

void setup() {
  // set console and pins
  Serial.begin(9600);
  pinMode(LDR, INPUT);
  pinMode(TRIGGER, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(LED, OUTPUT);

  // fuzzy sets
  // distance (0-400cm in Wokwi)
  FuzzySet *small = new FuzzySet(0, 0, 0, 120); //trapizoid MF (a,b,c,d)
  FuzzySet *mid = new FuzzySet(60, 120, 120, 180);
  FuzzySet *big = new FuzzySet(120, 180, 180, 240);
  
  // ldr (Potentiometer) (0-1023 in wokwi)
  FuzzySet *lowldr = new FuzzySet(0, 0, 0, 100);
  FuzzySet *midldr = new FuzzySet(60, 200, 500, 700);
  FuzzySet *highldr = new FuzzySet(400 ,700, 1023, 1023);
  
  // brightness
  FuzzySet *off = new FuzzySet(0, 0, 0, 0);
  FuzzySet *lowb = new FuzzySet(0, 0, 25, 100);
  FuzzySet *highb = new FuzzySet(155, 230, 255, 255);

  // variables
  // variable distance with universe 0-400 as input
  FuzzyInput *distance = new FuzzyInput(1);
  distance->addFuzzySet(small);
  distance->addFuzzySet(mid);
  distance->addFuzzySet(big);
  fuzzy->addFuzzyInput(distance);

  // variable ldr with universe 0-1023 as input
  FuzzyInput *ldr = new FuzzyInput(2);
  ldr->addFuzzySet(lowldr);
  ldr->addFuzzySet(midldr);
  ldr->addFuzzySet(highldr);
  fuzzy->addFuzzyInput(ldr); 

  // variable brightness with universe 0-255 as output
  FuzzyOutput *brightness = new FuzzyOutput(1);
  brightness->addFuzzySet(off);
  brightness->addFuzzySet(lowb);
  brightness->addFuzzySet(highb);
  fuzzy->addFuzzyOutput(brightness);

  // Rules 1
// if distance is small and ldr is low then brightness is high
FuzzyRuleAntecedent *ifDistanceSmallAndLdrIsLow1 = new FuzzyRuleAntecedent();
ifDistanceSmallAndLdrIsLow1->joinWithAND(small, lowldr);
FuzzyRuleConsequent *thenBrightnessHigh1 = new FuzzyRuleConsequent();
thenBrightnessHigh1->addOutput(highb);
FuzzyRule *fuzzyRule1 = new FuzzyRule(1, ifDistanceSmallAndLdrIsLow1, thenBrightnessHigh1);
fuzzy->addFuzzyRule(fuzzyRule1);

// Rules 2
// if distance is small and ldr is low then brightness is midb
FuzzyRuleAntecedent *ifDistanceSmallAndLdrIsLow2 = new FuzzyRuleAntecedent();
ifDistanceSmallAndLdrIsLow2->joinWithAND(small, midldr);
FuzzyRuleConsequent *thenBrightnessMidb2 = new FuzzyRuleConsequent();
thenBrightnessMidb2->addOutput(midb);
FuzzyRule *fuzzyRule2 = new FuzzyRule(2, ifDistanceSmallAndLdrIsLow2, thenBrightnessMidb2);
fuzzy->addFuzzyRule(fuzzyRule2);

// Rules 3
// if distance is small and ldr is high then brightness is off
FuzzyRuleAntecedent *ifDistanceSmallAndLdrIsHigh = new FuzzyRuleAntecedent();
ifDistanceSmallAndLdrIsHigh->joinWithAND(small, highldr);
FuzzyRuleConsequent *thenBrightnessOff = new FuzzyRuleConsequent();
thenBrightnessOff->addOutput(off);
FuzzyRule *fuzzyRule3 = new FuzzyRule(3, ifDistanceSmallAndLdrIsHigh, thenBrightnessOff);
fuzzy->addFuzzyRule(fuzzyRule3);

// Rules 4
// if distance is mid and ldr is low then brightness is high
FuzzyRuleAntecedent *ifDistanceMidAndLdrIsLow = new FuzzyRuleAntecedent();
ifDistanceMidAndLdrIsLow->joinWithAND(mid, lowldr);
FuzzyRuleConsequent *thenBrightnessHigh4 = new FuzzyRuleConsequent();
thenBrightnessHigh4->addOutput(highb);
FuzzyRule *fuzzyRule4 = new FuzzyRule(4, ifDistanceMidAndLdrIsLow, thenBrightnessHigh4);
fuzzy->addFuzzyRule(fuzzyRule4);

// Rules 5
// if distance is mid and ldr is mid then brightness is midb
FuzzyRuleAntecedent *ifDistanceMidAndLdrIsMid = new FuzzyRuleAntecedent();
ifDistanceMidAndLdrIsMid->joinWithAND(mid, midldr);
FuzzyRuleConsequent *thenBrightnessMidb5 = new FuzzyRuleConsequent();
thenBrightnessMidb5->addOutput(midb);
FuzzyRule *fuzzyRule5 = new FuzzyRule(5, ifDistanceMidAndLdrIsMid, thenBrightnessMidb5);
fuzzy->addFuzzyRule(fuzzyRule5);

// Rules 6
// if distance is mid and ldr is high then brightness is lowb
FuzzyRuleAntecedent *ifDistanceMidAndLdrIsHigh = new FuzzyRuleAntecedent();
ifDistanceMidAndLdrIsHigh->joinWithAND(mid, highldr);
FuzzyRuleConsequent *thenBrightnessLowb6 = new FuzzyRuleConsequent();
thenBrightnessLowb6->addOutput(lowb);
FuzzyRule *fuzzyRule6 = new FuzzyRule(6, ifDistanceMidAndLdrIsHigh, thenBrightnessLowb6);
fuzzy->addFuzzyRule(fuzzyRule6);

// Rules 7
// if distance is big and ldr is low then brightness is midb
FuzzyRuleAntecedent *ifDistanceBigAndLdrIsLow = new FuzzyRuleAntecedent();
ifDistanceBigAndLdrIsLow->joinWithAND(big, lowldr);
FuzzyRuleConsequent *thenBrightnessMidb7 = new FuzzyRuleConsequent();
thenBrightnessMidb7->addOutput(midb);
FuzzyRule *fuzzyRule7 = new FuzzyRule(7, ifDistanceBigAndLdrIsLow, thenBrightnessMidb7);
fuzzy->addFuzzyRule(fuzzyRule7);

// Rules 8
// if distance is big and ldr is mid then brightness is lowb
FuzzyRuleAntecedent *ifDistanceBigAndLdrIsMid = new FuzzyRuleAntecedent();
ifDistanceBigAndLdrIsMid->joinWithAND(big, midldr);
FuzzyRuleConsequent *thenBrightnessLowb8 = new FuzzyRuleConsequent();
thenBrightnessLowb8->addOutput(lowb);
FuzzyRule *fuzzyRule8 = new FuzzyRule(8, ifDistanceBigAndLdrIsMid, thenBrightnessLowb8);
fuzzy->addFuzzyRule(fuzzyRule8);

// Rules 9
// if distance is big and ldr is high then brightness is off
FuzzyRuleAntecedent *ifDistanceBigAndLdrIsHigh = new FuzzyRuleAntecedent();
ifDistanceBigAndLdrIsHigh->joinWithAND(big, highldr);
FuzzyRuleConsequent *thenBrightnessOff9 = new FuzzyRuleConsequent();
thenBrightnessOff9->addOutput(off);
FuzzyRule *fuzzyRule9 = new FuzzyRule(9, ifDistanceBigAndLdrIsHigh, thenBrightnessOff9);
fuzzy->addFuzzyRule(fuzzyRule9);
}

// returns the distance
int distance() {
  digitalWrite(TRIGGER, LOW);
  delayMicroseconds(5);
  digitalWrite(TRIGGER, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER, LOW);
  long pulse = pulseIn(ECHO, HIGH) / 2;
  return pulse * 10 / 292;
}

// returns the brightness
int brightness() {
  return analogRead(LDR);
}

// main method
void loop() {
  // get distance and light
  int dist = distance();
  int light = brightness();

  // if the inputs are weird, ignore them
  if (dist < 0 || dist > 400 || light > 1023) return;

  // fuzzyfication
  fuzzy->setInput(1, dist);
  fuzzy->setInput(2, light);
  fuzzy->fuzzify();

  // defuzzyfication
  int output = fuzzy->defuzzify(1);
  
  analogWrite(LED, output);

  Serial.print("distance: ");
  Serial.print(dist);
  Serial.print(" light: ");
  Serial.print(light);
  Serial.print(" => output: ");
  Serial.print(output);
  Serial.println();
  delay(100);
}