#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>

# define DS_L	13  //1
# define DS_ML	12  //2
# define DS_M	11  //3
# define DS_MR	10  //4
# define DS_R	9   //5

int val = 0;
int lastError = 0;

// Instantiating an object of library
Fuzzy* fuzzy = new Fuzzy();
 
FuzzySet* negative = new FuzzySet(-44, -44, -44, 0);
FuzzySet* zero = new FuzzySet(-44, 0, 0, 53);
FuzzySet* positive = new FuzzySet(0, 53, 53, 53);
 
FuzzySet* negatif = new FuzzySet(-97, -97, -97, 0);
FuzzySet* kosong = new FuzzySet(-97, 0, 0, 97);
FuzzySet* positif = new FuzzySet(0, 97, 97, 97);

void setup(){
  pinMode(DS_L,INPUT);
  pinMode(DS_ML,INPUT);
  pinMode(DS_M,INPUT);
  pinMode(DS_MR,INPUT);
  pinMode(DS_R,INPUT);
  
  Serial.begin(9600);
   
  // FuzzyInput
  FuzzyInput* error = new FuzzyInput(1);
  error->addFuzzySet(negative);
  error->addFuzzySet(zero);
  error->addFuzzySet(positive);
 
  fuzzy->addFuzzyInput(error);
 
  // FuzzyInput
  FuzzyInput* derror = new FuzzyInput(2);
  derror->addFuzzySet(negatif);
  derror->addFuzzySet(kosong);
  derror->addFuzzySet(positif);
 
  fuzzy->addFuzzyInput(derror);

  // FuzzyOutput
  FuzzyOutput* motors = new FuzzyOutput(1);
 
  FuzzySet* HLeft = new FuzzySet(-150, -100, -100, -50);
  motors->addFuzzySet(HLeft);
  FuzzySet* SLeft = new FuzzySet(-100, -50, -50, 0);
  motors->addFuzzySet(SLeft);
  FuzzySet* Straight = new FuzzySet(-50, 0, 0, 50);
  motors->addFuzzySet(Straight);
  FuzzySet* SRight = new FuzzySet(0, 50, 50, 100);
  motors->addFuzzySet(SRight);
  FuzzySet* HRight = new FuzzySet(50, 100, 100, 150);
  motors->addFuzzySet(HRight);
 
  fuzzy->addFuzzyOutput(motors);

  // Building FuzzyRule
  FuzzyRuleAntecedent* iferrornegativeAndderrornegatif = new FuzzyRuleAntecedent();
  iferrornegativeAndderrornegatif->joinWithAND(negative, negatif);
 
  FuzzyRuleConsequent* thenmotorsHRight = new FuzzyRuleConsequent();
  thenmotorsHRight->addOutput(HRight);
 
  FuzzyRule* fuzzyRule1 = new FuzzyRule(1, iferrornegativeAndderrornegatif, thenmotorsHRight);
  fuzzy->addFuzzyRule(fuzzyRule1);
 
  // Building FuzzyRule
  FuzzyRuleAntecedent* iferrornegativeAndderrorkosong = new FuzzyRuleAntecedent();
  iferrornegativeAndderrorkosong->joinWithAND(negative, kosong);
  FuzzyRuleAntecedent* iferrornegativeAndderrorpositif = new FuzzyRuleAntecedent();
  iferrornegativeAndderrorpositif->joinWithAND(negative, positif);
  FuzzyRuleAntecedent* iferrorzeroAndderrornegatif = new FuzzyRuleAntecedent();
  iferrorzeroAndderrornegatif->joinWithAND(zero, negatif);
  
  FuzzyRuleConsequent* thenmotorsSRight = new FuzzyRuleConsequent();
  thenmotorsSRight->addOutput(SRight);
  
  FuzzyRule* fuzzyRule2 = new FuzzyRule(2, iferrornegativeAndderrorkosong, thenmotorsSRight);
  fuzzy->addFuzzyRule(fuzzyRule2);
  FuzzyRule* fuzzyRule3 = new FuzzyRule(3, iferrornegativeAndderrorpositif, thenmotorsSRight);
  fuzzy->addFuzzyRule(fuzzyRule3);
  FuzzyRule* fuzzyRule4 = new FuzzyRule(4, iferrorzeroAndderrornegatif, thenmotorsSRight);
  fuzzy->addFuzzyRule(fuzzyRule4);
  
  // Building FuzzyRule
  FuzzyRuleAntecedent* iferrorzeroAndderrorkosong = new FuzzyRuleAntecedent();
  iferrorzeroAndderrorkosong->joinWithAND(zero, kosong);
 
  FuzzyRuleConsequent* thenmotorsStraight = new FuzzyRuleConsequent();
  thenmotorsStraight->addOutput(Straight);
 
  FuzzyRule* fuzzyRule5 = new FuzzyRule(5, iferrorzeroAndderrorkosong, thenmotorsStraight);
  fuzzy->addFuzzyRule(fuzzyRule5);
  
  // Building FuzzyRule
  FuzzyRuleAntecedent* iferrorzeroAndderrorpositif = new FuzzyRuleAntecedent();
  iferrorzeroAndderrorpositif->joinWithAND(zero, positif);
  FuzzyRuleAntecedent* iferrorpositiveAndderrornegatif = new FuzzyRuleAntecedent();
  iferrorpositiveAndderrornegatif->joinWithAND(positive, negatif);
  FuzzyRuleAntecedent* iferrorpositiveAndderrorkosong = new FuzzyRuleAntecedent();
  iferrorpositiveAndderrorkosong->joinWithAND(positive, kosong);
  
  FuzzyRuleConsequent* thenmotorsSLeft = new FuzzyRuleConsequent();
  thenmotorsSLeft->addOutput(SLeft);
  
  FuzzyRule* fuzzyRule6 = new FuzzyRule(6, iferrorzeroAndderrorpositif, thenmotorsSLeft);
  fuzzy->addFuzzyRule(fuzzyRule6);
  FuzzyRule* fuzzyRule7 = new FuzzyRule(7, iferrorpositiveAndderrornegatif, thenmotorsSLeft);
  fuzzy->addFuzzyRule(fuzzyRule7);
  FuzzyRule* fuzzyRule8 = new FuzzyRule(8, iferrorpositiveAndderrorkosong, thenmotorsSLeft);
  fuzzy->addFuzzyRule(fuzzyRule8);
  
  // Building FuzzyRule
  FuzzyRuleAntecedent* iferrorpositiveAndderrorpositif = new FuzzyRuleAntecedent();
  iferrorpositiveAndderrorpositif->joinWithAND(positive, positif);
 
  FuzzyRuleConsequent* thenmotorsHLeft = new FuzzyRuleConsequent();
  thenmotorsHLeft->addOutput(HLeft);
 
  FuzzyRule* fuzzyRule9 = new FuzzyRule(9, iferrorpositiveAndderrorpositif, thenmotorsHLeft);
  fuzzy->addFuzzyRule(fuzzyRule9);
}
 
void loop()
{
  getvalue();
  int error = val - 47;
  int derror = error - lastError;
  lastError = error;
  
  Serial.print("Value:");
  Serial.println(val);
  Serial.print("Error:");
  Serial.println(error);
  Serial.print("Derror:");
  Serial.println(derror);

  
  fuzzy->setInput(1,error);
  fuzzy->setInput(2,derror);
 
  fuzzy->fuzzify();
   
  Serial.print("Error: ");
  Serial.print(negative->getPertinence());
  Serial.print(", ");
  Serial.print(zero->getPertinence());
  Serial.print(", ");
  Serial.println(positive->getPertinence());
   
  Serial.print("Derror: ");
  Serial.print(negatif->getPertinence());
  Serial.print(", ");
  Serial.print(kosong->getPertinence());
  Serial.print(", ");
  Serial.println(positif->getPertinence());

  float motors = fuzzy->defuzzify(1);

  Serial.print("Motors: ");
  Serial.println(motors);

  delay(1000);
}

void getvalue()
{
  if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==LOW)
  {
    val = 3;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==HIGH)
  {
    val = 6;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==LOW)    //5
  {
    val = 9;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==HIGH)
  {
    val = 13;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==LOW)
  {
    val = 16;
  }
  
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==HIGH)
  {
    val = 19;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==LOW)
  {
    val = 22;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==HIGH)    //10
  {
    val = 25;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==LOW)
  {
    val = 28;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==HIGH)
  {
    val = 31;
  }
  
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==LOW)
  {
    val = 34;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==HIGH)
  {
    val = 38;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==LOW)    //15
  {
    val = 41;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==HIGH)
  {
    val = 44;
  }
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==LOW)
  {
    val = 47;
  }
  
  else if(digitalRead(DS_L)==LOW && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==HIGH)
  {
    val = 50;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==LOW)
  {
    val = 53;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==HIGH)    //20
  {
    val = 56;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==LOW)
  {
    val = 59;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==HIGH)
  {
    val = 63;
  }
  
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==LOW)
  {
    val = 66;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==HIGH)
  {
    val = 69;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==LOW)    //25
  {
    val = 72;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==LOW && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==HIGH)
  {
    val = 75;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==LOW)
  {
    val = 78;
  }
  
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==HIGH)
  {
    val = 81;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==LOW)
  {
    val = 84;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==LOW && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==HIGH)    //30
  {
    val = 88;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==LOW)
  {
    val = 91;
  }
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==LOW && digitalRead(DS_R)==HIGH)
  {
    val = 94;
  }
  
  else if(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==LOW)
  {
    val = 97;
  }
  else //(digitalRead(DS_L)==HIGH && digitalRead(DS_ML)==HIGH && digitalRead(DS_M)==HIGH && digitalRead(DS_MR)==HIGH && digitalRead(DS_R)==HIGH)
  {
    val = 100;
  }
}