#include <Fuzzy.h>

Fuzzy *fuzzy = new Fuzzy();



void setup() {
  Serial.begin(9600);
  // Вхідні змінні: світло (light) та температура (temperature)
	FuzzyInput *Front_Distance_to_Wall = new FuzzyInput(1);
	FuzzyInput *Rear_Distance_to_Wall = new FuzzyInput(2);

	FuzzySet *Front_Very_Close = new FuzzySet(0, 0, 0, 8);
	FuzzySet *Front_Close = new FuzzySet(4, 12, 12, 18);
	FuzzySet *Front_Normal = new FuzzySet(15, 20, 20, 25);
	FuzzySet *Front_Far = new FuzzySet(22, 28, 28, 36);
	FuzzySet *Front_Very_Far = new FuzzySet(32, 40, 40, 40);

  FuzzySet *Rear_Very_Close = new FuzzySet(0, 0, 0, 8);
	FuzzySet *Rear_Close = new FuzzySet(4, 12, 12, 18);
	FuzzySet *Rear_Normal = new FuzzySet(15, 20, 20, 25);
	FuzzySet *Rear_Far = new FuzzySet(22, 28, 28, 36);
	FuzzySet *Rear_Very_Far = new FuzzySet(32, 40, 40, 40);

	Front_Distance_to_Wall->addFuzzySet(Front_Very_Close);
	Front_Distance_to_Wall->addFuzzySet(Front_Close);
	Front_Distance_to_Wall->addFuzzySet(Front_Normal);
	Front_Distance_to_Wall->addFuzzySet(Front_Far);
	Front_Distance_to_Wall->addFuzzySet(Front_Very_Far);

	Rear_Distance_to_Wall->addFuzzySet(Rear_Very_Close);
	Rear_Distance_to_Wall->addFuzzySet(Rear_Close);
	Rear_Distance_to_Wall->addFuzzySet(Rear_Normal);
	Rear_Distance_to_Wall->addFuzzySet(Rear_Far);
	Rear_Distance_to_Wall->addFuzzySet(Rear_Very_Far);

	fuzzy->addFuzzyInput(Front_Distance_to_Wall);
	fuzzy->addFuzzyInput(Rear_Distance_to_Wall);

	FuzzyOutput *Left_Wheel = new FuzzyOutput(1);
	FuzzyOutput *Right_Wheel = new FuzzyOutput(2);
	
	FuzzySet *Left_Slow = new FuzzySet(0, 0, 0, 31);
	FuzzySet *Left_Medium = new FuzzySet(18, 25, 40, 60);
	FuzzySet *Left_Fast = new FuzzySet(50, 57, 72, 100);

  FuzzySet *Right_Slow = new FuzzySet(0, 0, 0, 31);
	FuzzySet *Right_Medium = new FuzzySet(18, 25, 40, 60);
	FuzzySet *Right_Fast = new FuzzySet(50, 57, 72, 100);

	Left_Wheel->addFuzzySet(Left_Slow);
	Left_Wheel->addFuzzySet(Left_Medium);
	Left_Wheel->addFuzzySet(Left_Fast);

	Right_Wheel->addFuzzySet(Right_Slow);
	Right_Wheel->addFuzzySet(Right_Medium);
	Right_Wheel->addFuzzySet(Right_Fast);

	fuzzy->addFuzzyOutput(Left_Wheel);
	fuzzy->addFuzzyOutput(Right_Wheel);

  // Правило 1
	FuzzyRuleAntecedent *ifFrontNormalAndRearNormal = new FuzzyRuleAntecedent();
	ifFrontNormalAndRearNormal->joinWithAND(Front_Normal, Rear_Normal);
	FuzzyRuleConsequent *thenLeftFastAndRightFast = new FuzzyRuleConsequent();
	thenLeftFastAndRightFast->addOutput(Left_Fast);
	thenLeftFastAndRightFast->addOutput(Right_Fast);
	FuzzyRule *fuzzyRule1 = new FuzzyRule(1, ifFrontNormalAndRearNormal, thenLeftFastAndRightFast);
	fuzzy->addFuzzyRule(fuzzyRule1);

	// Правило 2
	FuzzyRuleAntecedent *ifFrontVeryCloseAndRearVeryClose = new FuzzyRuleAntecedent();
	ifFrontVeryCloseAndRearVeryClose->joinWithAND(Front_Very_Close, Rear_Very_Close);
	FuzzyRuleConsequent *thenLeftSlowAndRightFast = new FuzzyRuleConsequent();
	thenLeftSlowAndRightFast->addOutput(Left_Slow);
	thenLeftSlowAndRightFast->addOutput(Right_Fast);
	FuzzyRule *fuzzyRule2 = new FuzzyRule(2, ifFrontVeryCloseAndRearVeryClose, thenLeftSlowAndRightFast);
	fuzzy->addFuzzyRule(fuzzyRule2);
  
  // Правило 3
	FuzzyRuleAntecedent *ifFrontVeryFarAndRearVeryFar = new FuzzyRuleAntecedent();
	ifFrontVeryFarAndRearVeryFar->joinWithAND(Front_Very_Far, Rear_Very_Far);
	FuzzyRuleConsequent *thenLeftFastAndRightSlow = new FuzzyRuleConsequent();
	thenLeftFastAndRightSlow->addOutput(Left_Fast);
	thenLeftFastAndRightSlow->addOutput(Right_Slow);
	FuzzyRule *fuzzyRule3 = new FuzzyRule(3, ifFrontVeryFarAndRearVeryFar, thenLeftFastAndRightSlow);
	fuzzy->addFuzzyRule(fuzzyRule3);

	// Правило 4
	FuzzyRuleAntecedent *ifFrontCloseAndRearVeryClose = new FuzzyRuleAntecedent();
	ifFrontCloseAndRearVeryClose->joinWithAND(Front_Close, Rear_Very_Close);
	FuzzyRuleConsequent *thenLeftMediumAndRightMedium = new FuzzyRuleConsequent();
	thenLeftMediumAndRightMedium->addOutput(Left_Medium);
	thenLeftMediumAndRightMedium->addOutput(Right_Medium);
	FuzzyRule *fuzzyRule4 = new FuzzyRule(4, ifFrontCloseAndRearVeryClose, thenLeftMediumAndRightMedium);
	fuzzy->addFuzzyRule(fuzzyRule4);

  // Правило 5
	FuzzyRuleAntecedent *ifFrontNormalAndRearClose = new FuzzyRuleAntecedent();
	ifFrontNormalAndRearClose->joinWithAND(Front_Normal, Rear_Close);
	FuzzyRuleConsequent *thenLeftFastAndRightMedium = new FuzzyRuleConsequent();
	thenLeftFastAndRightMedium->addOutput(Left_Fast);
	thenLeftFastAndRightMedium->addOutput(Right_Medium);
	FuzzyRule *fuzzyRule5 = new FuzzyRule(5, ifFrontNormalAndRearClose, thenLeftFastAndRightMedium);
	fuzzy->addFuzzyRule(fuzzyRule5);

	// Правило 6
	FuzzyRuleAntecedent *ifFrontFarAndRearNormal = new FuzzyRuleAntecedent();
	ifFrontFarAndRearNormal->joinWithAND(Front_Far, Rear_Normal);
	/*FuzzyRuleConsequent *thenLeftFastAndRightMedium = new FuzzyRuleConsequent();
	thenLeftFastAndRightMedium->addOutput(Fast);
	thenLeftFastAndRightMedium->addOutput(Medium);*/
	FuzzyRule *fuzzyRule6 = new FuzzyRule(6, ifFrontFarAndRearNormal, thenLeftFastAndRightMedium);
	fuzzy->addFuzzyRule(fuzzyRule6);

	// Правило 7
	FuzzyRuleAntecedent *ifFrontFarAndRearVeryFar = new FuzzyRuleAntecedent();
	ifFrontFarAndRearVeryFar->joinWithAND(Front_Far, Rear_Very_Far);
	/*FuzzyRuleConsequent *thenLeftMediumAndRightMedium = new FuzzyRuleConsequent();
	thenLeftMediumAndRightMedium->addOutput(Medium);
	thenLeftMediumAndRightMedium->addOutput(Medium);*/
	FuzzyRule *fuzzyRule7 = new FuzzyRule(7, ifFrontFarAndRearVeryFar, thenLeftMediumAndRightMedium);
	fuzzy->addFuzzyRule(fuzzyRule7);

	// Правило 8
	FuzzyRuleAntecedent *ifFrontNormalAndRearFar = new FuzzyRuleAntecedent();
	ifFrontNormalAndRearFar->joinWithAND(Front_Normal, Rear_Far);
	FuzzyRuleConsequent *thenLeftMediumAndRightFast = new FuzzyRuleConsequent();
	thenLeftMediumAndRightFast->addOutput(Left_Medium);
	thenLeftMediumAndRightFast->addOutput(Right_Fast);
	FuzzyRule *fuzzyRule8 = new FuzzyRule(8, ifFrontNormalAndRearFar, thenLeftMediumAndRightFast);
	fuzzy->addFuzzyRule(fuzzyRule8);

	// Правило 9
	FuzzyRuleAntecedent *ifFrontVeryCloseAndRearClose = new FuzzyRuleAntecedent();
	ifFrontVeryCloseAndRearClose->joinWithAND(Front_Very_Close, Rear_Close);
	/*FuzzyRuleConsequent *thenLeftSlowAndRightFast = new FuzzyRuleConsequent();
	thenLeftSlowAndRightFast->addOutput(Slow);
	thenLeftSlowAndRightFast->addOutput(Fast);*/
	FuzzyRule *fuzzyRule9 = new FuzzyRule(9, ifFrontVeryCloseAndRearClose, thenLeftSlowAndRightFast);
	fuzzy->addFuzzyRule(fuzzyRule9);

	// Правило 10
	FuzzyRuleAntecedent *ifFrontCloseAndRearClose = new FuzzyRuleAntecedent();
	ifFrontCloseAndRearClose->joinWithAND(Front_Close, Rear_Close);
	/*FuzzyRuleConsequent *thenLeftMediumAndRightFast = new FuzzyRuleConsequent();
	thenLeftMediumAndRightFast->addOutput(Medium);
	thenLeftMediumAndRightFast->addOutput(Fast);*/
	FuzzyRule *fuzzyRule10 = new FuzzyRule(10, ifFrontCloseAndRearClose, thenLeftMediumAndRightFast);
	fuzzy->addFuzzyRule(fuzzyRule10);

	// Правило 11
	FuzzyRuleAntecedent *ifFrontCloseAndRearNormal = new FuzzyRuleAntecedent();
	ifFrontCloseAndRearNormal->joinWithAND(Front_Close, Rear_Normal);
	/*FuzzyRuleConsequent *thenLeftMediumAndRightFast = new FuzzyRuleConsequent();
	thenLeftMediumAndRightFast->addOutput(Medium);
	thenLeftMediumAndRightFast->addOutput(Fast);*/
	FuzzyRule *fuzzyRule11 = new FuzzyRule(11, ifFrontCloseAndRearNormal, thenLeftMediumAndRightFast);
	fuzzy->addFuzzyRule(fuzzyRule11);

	// Правило 12
	FuzzyRuleAntecedent *ifFrontFarAndRearFar = new FuzzyRuleAntecedent();
	ifFrontFarAndRearFar->joinWithAND(Front_Far, Rear_Far);
	/*FuzzyRuleConsequent *thenLeftFastAndRightMedium = new FuzzyRuleConsequent();
	thenLeftFastAndRightMedium->addOutput(Fast);
	thenLeftFastAndRightMedium->addOutput(Medium);*/
	FuzzyRule *fuzzyRule12 = new FuzzyRule(12, ifFrontFarAndRearFar, thenLeftFastAndRightMedium);
	fuzzy->addFuzzyRule(fuzzyRule12);

	// Правило 13
	FuzzyRuleAntecedent *ifFrontVeryFarAndRearFar = new FuzzyRuleAntecedent();
	ifFrontVeryFarAndRearFar->joinWithAND(Front_Very_Far, Rear_Far);
	/*FuzzyRuleConsequent *thenLeftFastAndRightSlow = new FuzzyRuleConsequent();
	thenLeftFastAndRightSlow->addOutput(Fast);
	thenLeftFastAndRightSlow->addOutput(Slow);*/
	FuzzyRule *fuzzyRule13 = new FuzzyRule(13, ifFrontVeryFarAndRearFar, thenLeftFastAndRightSlow);
	fuzzy->addFuzzyRule(fuzzyRule13);

	// Правило 14
	FuzzyRuleAntecedent *ifFrontVeryCloseAndRearNormal = new FuzzyRuleAntecedent();
	ifFrontVeryCloseAndRearNormal->joinWithAND(Front_Very_Close, Rear_Normal);
	/*FuzzyRuleConsequent *thenLeftSlowAndRightFast = new FuzzyRuleConsequent();
	thenLeftSlowAndRightFast->addOutput(Slow);
	thenLeftSlowAndRightFast->addOutput(Fast);*/
	FuzzyRule *fuzzyRule14 = new FuzzyRule(14, ifFrontVeryCloseAndRearNormal, thenLeftSlowAndRightFast);
	fuzzy->addFuzzyRule(fuzzyRule14);

	// Правило 15
	FuzzyRuleAntecedent *ifFrontCloseAndRearFar = new FuzzyRuleAntecedent();
	ifFrontCloseAndRearFar->joinWithAND(Front_Close, Rear_Far);
	/*FuzzyRuleConsequent *thenLeftMediumAndRightFast = new FuzzyRuleConsequent();
	thenLeftMediumAndRightFast->addOutput(Medium);
	thenLeftMediumAndRightFast->addOutput(Fast);*/
	FuzzyRule *fuzzyRule15 = new FuzzyRule(15, ifFrontCloseAndRearFar, thenLeftMediumAndRightFast);
	fuzzy->addFuzzyRule(fuzzyRule15);

	// Правило 16
	FuzzyRuleAntecedent *ifFrontNormalAndRearVeryClose = new FuzzyRuleAntecedent();
	ifFrontNormalAndRearVeryClose->joinWithAND(Front_Normal, Rear_Very_Close);
	/*FuzzyRuleConsequent *thenLeftFastAndRightMedium = new FuzzyRuleConsequent();
	thenLeftFastAndRightMedium->addOutput(Fast);
	thenLeftFastAndRightMedium->addOutput(Medium);*/
	FuzzyRule *fuzzyRule16 = new FuzzyRule(16, ifFrontNormalAndRearVeryClose, thenLeftFastAndRightMedium);
	fuzzy->addFuzzyRule(fuzzyRule16);

	// Правило 17
	FuzzyRuleAntecedent *ifFrontNormalAndRearVeryFar = new FuzzyRuleAntecedent();
	ifFrontNormalAndRearVeryFar->joinWithAND(Front_Normal, Rear_Very_Far);
	/*FuzzyRuleConsequent *thenLeftMediumAndRightFast = new FuzzyRuleConsequent();
	thenLeftMediumAndRightFast->addOutput(Medium);
	thenLeftMediumAndRightFast->addOutput(Fast);*/
	FuzzyRule *fuzzyRule17 = new FuzzyRule(17, ifFrontNormalAndRearVeryFar, thenLeftMediumAndRightFast);
	fuzzy->addFuzzyRule(fuzzyRule17);

	// Правило 18
	FuzzyRuleAntecedent *ifFrontFarAndRearClose = new FuzzyRuleAntecedent();
	ifFrontFarAndRearClose->joinWithAND(Front_Far, Rear_Close);
	/*FuzzyRuleConsequent *thenLeftFastAndRightMedium = new FuzzyRuleConsequent();
	thenLeftFastAndRightMedium->addOutput(Fast);
	thenLeftFastAndRightMedium->addOutput(Medium);*/
	FuzzyRule *fuzzyRule18 = new FuzzyRule(18, ifFrontFarAndRearClose, thenLeftFastAndRightMedium);
	fuzzy->addFuzzyRule(fuzzyRule18);

	// Правило 19
	FuzzyRuleAntecedent *ifFrontVeryFarAndRearNormal = new FuzzyRuleAntecedent();
	ifFrontVeryFarAndRearNormal->joinWithAND(Front_Very_Far, Rear_Normal);
	/*FuzzyRuleConsequent *thenLeftFastAndRightSlow = new FuzzyRuleConsequent();
	thenLeftFastAndRightSlow->addOutput(Fast);
	thenLeftFastAndRightSlow->addOutput(Slow);*/
	FuzzyRule *fuzzyRule19 = new FuzzyRule(19, ifFrontVeryFarAndRearNormal, thenLeftFastAndRightSlow);
	fuzzy->addFuzzyRule(fuzzyRule19);
	
}

void loop() {
  // Приклад вхідних значень (замініть на дані з ваших датчиків)
  int frontDistance = 12;
    int rearDistance = 0;
		// Printing something
		Serial.println("\n\n\nEntrance: ");
		Serial.print("\tFront Distance to Wall: ");
		Serial.println(frontDistance);
		Serial.print("\tRear Distance to Wall: ");
		Serial.println(rearDistance);
		fuzzy->setInput(1, frontDistance);
		fuzzy->setInput(2, rearDistance);
		fuzzy->fuzzify();
		float leftWheel = fuzzy->defuzzify(1);
		float rightWheel = fuzzy->defuzzify(2);
		Serial.println("Result:");
		Serial.print("\tLeft Wheel: ");
		Serial.println(leftWheel);
		Serial.print("\tRight Wheel: ");
		Serial.println(rightWheel);
		delay(5000);
}