#include <stdio.h>
#include <stdbool.h>
#include <stdlib.h>

#include "Fuzzy.h"

void setup() {
    
    Serial.begin(115200);
    Fuzzy *fuzzy = new Fuzzy();

   /* ultrassonico Esquerda */
    FuzzyInput * ultraEsquerda = new FuzzyInput(1); //indice da entrada;
    
    FuzzySet * ultraEsquerda_P = new FuzzySet(0, 0, 12.5, 25);
    ultraEsquerda->addFuzzySet(ultraEsquerda_P);
    FuzzySet * ultraEsquerda_M  = new FuzzySet(12.5, 25, 25, 37.5);
    ultraEsquerda->addFuzzySet(ultraEsquerda_M );
    FuzzySet * ultraEsquerda_L = new FuzzySet(25, 37.5, 50, 50);
    ultraEsquerda->addFuzzySet(ultraEsquerda_L);
	  fuzzy->addFuzzyInput(ultraEsquerda); 	
	
    /* ultrassonico Direita */
    FuzzyInput * ultraDireita = new FuzzyInput(2); //indice da entrada;
    
    FuzzySet * ultraDireita_P = new FuzzySet(0, 0, 12.5, 25);
    ultraDireita->addFuzzySet(ultraDireita_P);
    FuzzySet * ultraDireita_M  = new FuzzySet(12.5, 25, 25, 37.5);
    ultraDireita->addFuzzySet(ultraDireita_M );
    FuzzySet * ultraDireita_L = new FuzzySet(25, 37.5, 50, 50);
    ultraDireita->addFuzzySet(ultraDireita_L);
	  fuzzy->addFuzzyInput(ultraDireita);   	
	
    /* angular */
    FuzzyOutput * angular = new FuzzyOutput(1);
    
    FuzzySet *angular_E = new FuzzySet(-90,-90, -45, 0);
    angular->addFuzzySet(angular_E);

    FuzzySet *angular_PE = new FuzzySet(-45, -22.5, -22.5, 0);
    angular->addFuzzySet(angular_PE);
    
    FuzzySet *angular_C = new FuzzySet(-1, 0, 0, 1);
    angular->addFuzzySet(angular_C);

    FuzzySet *angular_PD = new FuzzySet(0, 22.5, 22.5, 45);
    angular->addFuzzySet(angular_PD);

    FuzzySet *angular_D = new FuzzySet(0, 45, 90, 90);
    angular->addFuzzySet(angular_D);

	fuzzy->addFuzzyOutput(angular);    	
	

	/* Regra 1 */
FuzzyRuleAntecedent *ifUltraDireitaPAndUltraEsquerdaP = new FuzzyRuleAntecedent();
ifUltraDireitaPAndUltraEsquerdaP->joinWithAND(ultraDireita_P, ultraEsquerda_P);

FuzzyRuleConsequent *thenAngularIsAngular_C = new FuzzyRuleConsequent();
thenAngularIsAngular_C->addOutput(angular_C);
FuzzyRule *fuzzyRule1 = new FuzzyRule(1, ifUltraDireitaPAndUltraEsquerdaP, thenAngularIsAngular_C);
fuzzy->addFuzzyRule(fuzzyRule1);

/* Regra 2 */
FuzzyRuleAntecedent *ifUltraDireitaPAndUltraEsquerdaM = new FuzzyRuleAntecedent();
ifUltraDireitaPAndUltraEsquerdaM->joinWithAND(ultraDireita_P, ultraEsquerda_M);

FuzzyRuleConsequent *thenAngularIsAngular_PD = new FuzzyRuleConsequent();
thenAngularIsAngular_PD->addOutput(angular_PD);
FuzzyRule *fuzzyRule2 = new FuzzyRule(2, ifUltraDireitaPAndUltraEsquerdaM, thenAngularIsAngular_PD);
fuzzy->addFuzzyRule(fuzzyRule2);

/* Regra 3 */
FuzzyRuleAntecedent *ifUltraDireitaPAndUltraEsquerdaL = new FuzzyRuleAntecedent();
ifUltraDireitaPAndUltraEsquerdaL->joinWithAND(ultraDireita_P, ultraEsquerda_L);

FuzzyRuleConsequent *thenAngularIsAngular_D = new FuzzyRuleConsequent();
thenAngularIsAngular_D->addOutput(angular_D);
FuzzyRule *fuzzyRule3 = new FuzzyRule(3, ifUltraDireitaPAndUltraEsquerdaL, thenAngularIsAngular_D);
fuzzy->addFuzzyRule(fuzzyRule3);

/* Regra 4 */
FuzzyRuleAntecedent *ifUltraDireitaMAndUltraEsquerdaP = new FuzzyRuleAntecedent();
ifUltraDireitaMAndUltraEsquerdaP->joinWithAND(ultraDireita_M, ultraEsquerda_P);

FuzzyRuleConsequent *thenAngularIsAngular_PE = new FuzzyRuleConsequent();
thenAngularIsAngular_PE->addOutput(angular_PE);
FuzzyRule *fuzzyRule4 = new FuzzyRule(4, ifUltraDireitaMAndUltraEsquerdaP, thenAngularIsAngular_PE);
fuzzy->addFuzzyRule(fuzzyRule4);

/* Regra 5 */
FuzzyRuleAntecedent *ifUltraDireitaMAndUltraEsquerdaM = new FuzzyRuleAntecedent();
ifUltraDireitaMAndUltraEsquerdaM->joinWithAND(ultraDireita_M, ultraEsquerda_M);

FuzzyRuleConsequent *thenAngularIsAngular_C_2 = new FuzzyRuleConsequent();
thenAngularIsAngular_C_2->addOutput(angular_C);
FuzzyRule *fuzzyRule5 = new FuzzyRule(5, ifUltraDireitaMAndUltraEsquerdaM, thenAngularIsAngular_C_2);
fuzzy->addFuzzyRule(fuzzyRule5);

/* Regra 6 */
FuzzyRuleAntecedent *ifUltraDireitaMAndUltraEsquerdaL = new FuzzyRuleAntecedent();
ifUltraDireitaMAndUltraEsquerdaL->joinWithAND(ultraDireita_M, ultraEsquerda_L);

FuzzyRuleConsequent *thenAngularIsAngular_D_2 = new FuzzyRuleConsequent();
thenAngularIsAngular_D_2->addOutput(angular_D);
FuzzyRule *fuzzyRule6 = new FuzzyRule(6, ifUltraDireitaMAndUltraEsquerdaL, thenAngularIsAngular_D_2);
fuzzy->addFuzzyRule(fuzzyRule6);

/* Regra 7 */
FuzzyRuleAntecedent *ifUltraDireitaLAndUltraEsquerdaP = new FuzzyRuleAntecedent();
ifUltraDireitaLAndUltraEsquerdaP->joinWithAND(ultraDireita_L, ultraEsquerda_P);

FuzzyRuleConsequent *thenAngularIsAngular_E = new FuzzyRuleConsequent();
thenAngularIsAngular_E->addOutput(angular_E);
FuzzyRule *fuzzyRule7 = new FuzzyRule(7, ifUltraDireitaLAndUltraEsquerdaP, thenAngularIsAngular_E);
fuzzy->addFuzzyRule(fuzzyRule7);

/* Regra 8 */
FuzzyRuleAntecedent *ifUltraDireitaLAndUltraEsquerdaM = new FuzzyRuleAntecedent();
ifUltraDireitaLAndUltraEsquerdaM->joinWithAND(ultraDireita_L, ultraEsquerda_M);

FuzzyRuleConsequent *thenAngularIsAngular_E_2 = new FuzzyRuleConsequent();
thenAngularIsAngular_E_2->addOutput(angular_E);
FuzzyRule *fuzzyRule8 = new FuzzyRule(8, ifUltraDireitaLAndUltraEsquerdaM, thenAngularIsAngular_E_2);
fuzzy->addFuzzyRule(fuzzyRule8);

/* Regra 9 */
FuzzyRuleAntecedent *ifUltraDireitaLAndUltraEsquerdaL = new FuzzyRuleAntecedent();
ifUltraDireitaLAndUltraEsquerdaL->joinWithAND(ultraDireita_L, ultraEsquerda_L);

FuzzyRuleConsequent *thenAngularIsAngular_C_3 = new FuzzyRuleConsequent();
thenAngularIsAngular_C_3->addOutput(angular_C);
FuzzyRule *fuzzyRule9 = new FuzzyRule(9, ifUltraDireitaLAndUltraEsquerdaL, thenAngularIsAngular_C_3);
fuzzy->addFuzzyRule(fuzzyRule9);
	
	
	 //				   
    fuzzy->setInput(1, 7); //temp=7 graus celsius
	  fuzzy->setInput(2, 2);	//velocidade do vendo = 22 nós
    fuzzy->fuzzify();
    float output = fuzzy->defuzzify(1); 
    Serial.println(output);
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}