#include "Fuzzy.h"

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

float sensorReadLux(int pin)
{
    return analogRead(pin);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
    

    Fuzzy *fuzzy = new Fuzzy();

    /* temperatura */
    FuzzyInput * temperatura = new FuzzyInput(1);
    
    FuzzySet * temperaturafrio = new FuzzySet(0, 0, 10, 20);
    temperatura->addFuzzySet(temperaturafrio);
    
    FuzzySet * temperaturanormal = new FuzzySet(10, 20, 20, 30);
    temperatura->addFuzzySet(temperaturanormal);
    
    FuzzySet * temperaturaquente = new FuzzySet(20, 30, 30, 40);
    temperatura->addFuzzySet(temperaturaquente);
    
    FuzzySet * temperaturamuitoquente = new FuzzySet(30, 40, 50, 50);
    temperatura->addFuzzySet(temperaturamuitoquente);
    
	fuzzy->addFuzzyInput(temperatura);    	
	


    /* numero de pessoas */
    FuzzyInput * npessoas = new FuzzyInput(2);
    
    FuzzySet * npessoasmuitopequeno = new FuzzySet(0, 0, 10, 20);
    npessoas->addFuzzySet(npessoasmuitopequeno);
    
    FuzzySet * npessoaspequeno = new FuzzySet(10, 20, 20, 30);
    npessoas->addFuzzySet(npessoaspequeno);
    
    FuzzySet * npessoasmedio = new FuzzySet(20, 30, 30, 40);
    npessoas->addFuzzySet(npessoasmedio);
    
    FuzzySet * npessoasgrande = new FuzzySet(30, 40, 40, 50);
    npessoas->addFuzzySet(npessoasgrande);
    
    FuzzySet * npessoasmuitogrande = new FuzzySet(40, 50, 60, 60);
    npessoas->addFuzzySet(npessoasmuitogrande);
    
    fuzzy->addFuzzyInput(npessoas);    	
	

    /* potencia */
    FuzzyOutput * potencia = new FuzzyOutput(1);
    
    FuzzySet *potenciamuitobaixa = new FuzzySet(0, 0, 0, 25);
    potencia->addFuzzySet(potenciamuitobaixa);
    
    FuzzySet *potenciabaixa = new FuzzySet(0, 25, 25, 50);
    potencia->addFuzzySet(potenciabaixa);
    
    FuzzySet *potenciamedia = new FuzzySet(25, 50, 50, 75);
    potencia->addFuzzySet(potenciamedia);
    
    FuzzySet *potenciaalta = new FuzzySet(50, 75, 75, 100);
    potencia->addFuzzySet(potenciaalta);
	
	FuzzySet *potenciamuitoalta = new FuzzySet(75, 100, 100, 100);
    potencia->addFuzzySet(potenciamuitoalta);	
			    
	fuzzy->addFuzzyOutput(potencia);    	
	

		/*
	1. Se a TEMPERATURA for FRIA e o NÚMERO DE PESSOAS for MUITO PEQUENO, então POTÊNCIA é MUITO BAIXA.
	*/
	FuzzyRuleAntecedent *ifTempFrioAndPessoasMuitoPequeno = new FuzzyRuleAntecedent();
	ifTempFrioAndPessoasMuitoPequeno->joinWithAND(temperaturafrio, npessoasmuitopequeno);
	
	FuzzyRuleConsequent *thenPotenciaMuitoBaixa = new FuzzyRuleConsequent();
	thenPotenciaMuitoBaixa->addOutput(potenciamuitobaixa);
	
	FuzzyRule *fuzzyRule1 = new FuzzyRule(1, ifTempFrioAndPessoasMuitoPequeno, thenPotenciaMuitoBaixa);
	fuzzy->addFuzzyRule(fuzzyRule1);
	
	/*
	2. Se a TEMPERATURA for FRIA e o NÚMERO DE PESSOAS for PEQUENO, então POTÊNCIA é MUITO BAIXA.
	*/
	FuzzyRuleAntecedent *ifTempFrioAndPessoasPequeno = new FuzzyRuleAntecedent();
	ifTempFrioAndPessoasPequeno->joinWithAND(temperaturafrio, npessoaspequeno);
	
	FuzzyRule *fuzzyRule2 = new FuzzyRule(2, ifTempFrioAndPessoasPequeno, thenPotenciaMuitoBaixa);
	fuzzy->addFuzzyRule(fuzzyRule2);
	
	/*
	3. Se a TEMPERATURA for FRIA e o NÚMERO DE PESSOAS for MÉDIO, então POTÊNCIA é MUITO BAIXA.
	*/
	FuzzyRuleAntecedent *ifTempFrioAndPessoasMedio = new FuzzyRuleAntecedent();
	ifTempFrioAndPessoasMedio->joinWithAND(temperaturafrio, npessoasmedio);
	
	FuzzyRule *fuzzyRule3 = new FuzzyRule(3, ifTempFrioAndPessoasMedio, thenPotenciaMuitoBaixa);
	fuzzy->addFuzzyRule(fuzzyRule3);
	
	/*
	4. Se a TEMPERATURA for FRIA e o NÚMERO DE PESSOAS for GRANDE, então POTÊNCIA é BAIXA.
	*/
	FuzzyRuleAntecedent *ifTempFrioAndPessoasGrande = new FuzzyRuleAntecedent();
	ifTempFrioAndPessoasGrande->joinWithAND(temperaturafrio, npessoasgrande);
	
	FuzzyRuleConsequent *thenPotenciaBaixa = new FuzzyRuleConsequent();
	thenPotenciaBaixa->addOutput(potenciabaixa);
	
	FuzzyRule *fuzzyRule4 = new FuzzyRule(4, ifTempFrioAndPessoasGrande, thenPotenciaBaixa);
	fuzzy->addFuzzyRule(fuzzyRule4);
	
	/*
	5. Se a TEMPERATURA for FRIA e o NÚMERO DE PESSOAS for MUITO GRANDE, então POTÊNCIA é BAIXA.
	*/
	FuzzyRuleAntecedent *ifTempFrioAndPessoasMuitoGrande = new FuzzyRuleAntecedent();
	ifTempFrioAndPessoasMuitoGrande->joinWithAND(temperaturafrio, npessoasmuitogrande);
	
	FuzzyRule *fuzzyRule5 = new FuzzyRule(5, ifTempFrioAndPessoasMuitoGrande, thenPotenciaBaixa);
	fuzzy->addFuzzyRule(fuzzyRule5);
	
	/*
	6. Se a TEMPERATURA for NORMAL e o NÚMERO DE PESSOAS for MUITO PEQUENO, então POTÊNCIA é BAIXA.
	*/
	FuzzyRuleAntecedent *ifTempNormalAndPessoasMuitoPequeno = new FuzzyRuleAntecedent();
	ifTempNormalAndPessoasMuitoPequeno->joinWithAND(temperaturanormal, npessoasmuitopequeno);
	
	FuzzyRule *fuzzyRule6 = new FuzzyRule(6, ifTempNormalAndPessoasMuitoPequeno, thenPotenciaBaixa);
	fuzzy->addFuzzyRule(fuzzyRule6);
	
	/*
	7. Se a TEMPERATURA for NORMAL e o NÚMERO DE PESSOAS for PEQUENO, então POTÊNCIA é BAIXA.
	*/
	FuzzyRuleAntecedent *ifTempNormalAndPessoasPequeno = new FuzzyRuleAntecedent();
	ifTempNormalAndPessoasPequeno->joinWithAND(temperaturanormal, npessoaspequeno);
	
	FuzzyRule *fuzzyRule7 = new FuzzyRule(7, ifTempNormalAndPessoasPequeno, thenPotenciaBaixa);
	fuzzy->addFuzzyRule(fuzzyRule7);
	
	/*
	8. Se a TEMPERATURA for NORMAL e o NÚMERO DE PESSOAS for MÉDIO, então POTÊNCIA é MÉDIA.
	*/
	FuzzyRuleAntecedent *ifTempNormalAndPessoasMedio = new FuzzyRuleAntecedent();
	ifTempNormalAndPessoasMedio->joinWithAND(temperaturanormal, npessoasmedio);
	
	FuzzyRuleConsequent *thenPotenciaMedia = new FuzzyRuleConsequent();
	thenPotenciaMedia->addOutput(potenciamedia);
	
	FuzzyRule *fuzzyRule8 = new FuzzyRule(8, ifTempNormalAndPessoasMedio, thenPotenciaMedia);
	fuzzy->addFuzzyRule(fuzzyRule8);
	
	/*
	9. Se a TEMPERATURA for NORMAL e o NÚMERO DE PESSOAS for GRANDE, então POTÊNCIA é ALTA.
	*/
	FuzzyRuleAntecedent *ifTempNormalAndPessoasGrande = new FuzzyRuleAntecedent();
	ifTempNormalAndPessoasGrande->joinWithAND(temperaturanormal, npessoasgrande);
	
	FuzzyRuleConsequent *thenPotenciaAlta = new FuzzyRuleConsequent();
	thenPotenciaAlta->addOutput(potenciaalta);
	
	FuzzyRule *fuzzyRule9 = new FuzzyRule(9, ifTempNormalAndPessoasGrande, thenPotenciaAlta);
	fuzzy->addFuzzyRule(fuzzyRule9);
	
	/*
	10. Se a TEMPERATURA for NORMAL e o NÚMERO DE PESSOAS for MUITO GRANDE, então POTÊNCIA é ALTA.
	*/
	FuzzyRuleAntecedent *ifTempNormalAndPessoasMuitoGrande = new FuzzyRuleAntecedent();
	ifTempNormalAndPessoasMuitoGrande->joinWithAND(temperaturanormal, npessoasmuitogrande);
	
	FuzzyRule *fuzzyRule10 = new FuzzyRule(10, ifTempNormalAndPessoasMuitoGrande, thenPotenciaAlta);
	fuzzy->addFuzzyRule(fuzzyRule10);

	
		/*
	11. Se a TEMPERATURA for QUENTE e o NÚMERO DE PESSOAS for MUITO PEQUENO, então POTÊNCIA é BAIXA.
	*/
	FuzzyRuleAntecedent *ifTempQuenteAndPessoasMuitoPequeno = new FuzzyRuleAntecedent();
	ifTempQuenteAndPessoasMuitoPequeno->joinWithAND(temperaturaquente, npessoasmuitopequeno);
	
	FuzzyRuleConsequent *thenpotenciabaixa = new FuzzyRuleConsequent();
	thenPotenciaBaixa->addOutput(potenciabaixa);
	
	FuzzyRule *fuzzyRule11 = new FuzzyRule(11, ifTempQuenteAndPessoasMuitoPequeno, thenpotenciabaixa);
	fuzzy->addFuzzyRule(fuzzyRule11);
	
	/*
	12. Se a TEMPERATURA for QUENTE e o NÚMERO DE PESSOAS for PEQUENO, então POTÊNCIA é MÉDIA.
	*/
	FuzzyRuleAntecedent *ifTempQuenteAndPessoasPequeno = new FuzzyRuleAntecedent();
	ifTempQuenteAndPessoasPequeno->joinWithAND(temperaturaquente, npessoaspequeno);
	
	FuzzyRuleConsequent *thenpotenciamedia = new FuzzyRuleConsequent();
	thenPotenciaMedia->addOutput(potenciamedia);
	
	FuzzyRule *fuzzyRule12 = new FuzzyRule(12, ifTempQuenteAndPessoasPequeno, thenpotenciamedia);
	fuzzy->addFuzzyRule(fuzzyRule12);
	
	/*
	13. Se a TEMPERATURA for QUENTE e o NÚMERO DE PESSOAS for MÉDIO, então POTÊNCIA é ALTA.
	*/
	FuzzyRuleAntecedent *ifTempQuenteAndPessoasMedio = new FuzzyRuleAntecedent();
	ifTempQuenteAndPessoasMedio->joinWithAND(temperaturaquente, npessoasmedio);
	
	FuzzyRuleConsequent *thenpotenciaalta = new FuzzyRuleConsequent();
	thenPotenciaAlta->addOutput(potenciaalta);
	
	FuzzyRule *fuzzyRule13 = new FuzzyRule(13, ifTempQuenteAndPessoasMedio, thenpotenciaalta);
	fuzzy->addFuzzyRule(fuzzyRule13);
	
	/*
	14. Se a TEMPERATURA for QUENTE e o NÚMERO DE PESSOAS for GRANDE, então POTÊNCIA é ALTA.
	*/
	FuzzyRuleAntecedent *ifTempQuenteAndPessoasGrande = new FuzzyRuleAntecedent();
	ifTempQuenteAndPessoasGrande->joinWithAND(temperaturaquente, npessoasgrande);
	
	FuzzyRule *fuzzyRule14 = new FuzzyRule(14, ifTempQuenteAndPessoasGrande, thenPotenciaAlta);
	fuzzy->addFuzzyRule(fuzzyRule14);
	
	/*
	15. Se a TEMPERATURA for QUENTE e o NÚMERO DE PESSOAS for MUITO GRANDE, então POTÊNCIA é MUITO ALTA.
	*/
	FuzzyRuleAntecedent *ifTempQuenteAndPessoasMuitoGrande = new FuzzyRuleAntecedent();
	ifTempQuenteAndPessoasMuitoGrande->joinWithAND(temperaturaquente, npessoasmuitogrande);
	
	FuzzyRuleConsequent *thenPotenciaMuitoAlta = new FuzzyRuleConsequent();
	thenPotenciaMuitoAlta->addOutput(potenciamuitoalta);
	
	FuzzyRule *fuzzyRule15 = new FuzzyRule(15, ifTempQuenteAndPessoasMuitoGrande, thenPotenciaMuitoAlta);
	fuzzy->addFuzzyRule(fuzzyRule15);
	
	/*
	16. Se a TEMPERATURA for MUITO QUENTE e o NÚMERO DE PESSOAS for MUITO PEQUENO, então POTÊNCIA é MÉDIA.
	*/
	FuzzyRuleAntecedent *ifTempMuitoQuenteAndPessoasMuitoPequeno = new FuzzyRuleAntecedent();
	ifTempMuitoQuenteAndPessoasMuitoPequeno->joinWithAND(temperaturamuitoquente, npessoasmuitopequeno);
	
	FuzzyRule *fuzzyRule16 = new FuzzyRule(16, ifTempMuitoQuenteAndPessoasMuitoPequeno, thenPotenciaMedia);
	fuzzy->addFuzzyRule(fuzzyRule16);
	
	/*
	17. Se a TEMPERATURA for MUITO QUENTE e o NÚMERO DE PESSOAS for PEQUENO, então POTÊNCIA é MÉDIA.
	*/
	FuzzyRuleAntecedent *ifTempMuitoQuenteAndPessoasPequeno = new FuzzyRuleAntecedent();
	ifTempMuitoQuenteAndPessoasPequeno->joinWithAND(temperaturamuitoquente, npessoaspequeno);
	
	FuzzyRule *fuzzyRule17 = new FuzzyRule(17, ifTempMuitoQuenteAndPessoasPequeno, thenPotenciaMedia);
	fuzzy->addFuzzyRule(fuzzyRule17);
	
	/*
	18. Se a TEMPERATURA for MUITO QUENTE e o NÚMERO DE PESSOAS for MÉDIO, então POTÊNCIA é ALTA.
	*/
	FuzzyRuleAntecedent *ifTempMuitoQuenteAndPessoasMedio = new FuzzyRuleAntecedent();
	ifTempMuitoQuenteAndPessoasMedio->joinWithAND(temperaturamuitoquente, npessoasmedio);
	
	FuzzyRule *fuzzyRule18 = new FuzzyRule(18, ifTempMuitoQuenteAndPessoasMedio, thenPotenciaAlta);
	fuzzy->addFuzzyRule(fuzzyRule18);
	
	/*
	19. Se a TEMPERATURA for MUITO QUENTE e o NÚMERO DE PESSOAS for GRANDE, então POTÊNCIA é MUITO ALTA.
	*/
	FuzzyRuleAntecedent *ifTempMuitoQuenteAndPessoasGrande = new FuzzyRuleAntecedent();
	ifTempMuitoQuenteAndPessoasGrande->joinWithAND(temperaturamuitoquente, npessoasgrande);
	
	FuzzyRule *fuzzyRule19 = new FuzzyRule(19, ifTempMuitoQuenteAndPessoasGrande, thenPotenciaMuitoAlta);
	fuzzy->addFuzzyRule(fuzzyRule19);
	
	/*
	20. Se a TEMPERATURA for MUITO QUENTE e o NÚMERO DE PESSOAS for MUITO GRANDE, então POTÊNCIA é MUITO ALTA.
	*/
	FuzzyRuleAntecedent *ifTempMuitoQuenteAndPessoasMuitoGrande = new FuzzyRuleAntecedent();
	ifTempMuitoQuenteAndPessoasMuitoGrande->joinWithAND(temperaturamuitoquente, npessoasmuitogrande);
	
	FuzzyRule *fuzzyRule20 = new FuzzyRule(20, ifTempMuitoQuenteAndPessoasMuitoGrande, thenPotenciaMuitoAlta);
	fuzzy->addFuzzyRule(fuzzyRule20);


  // set output
  pinMode(12,OUTPUT);  //ldr1
  pinMode(14,OUTPUT);  //ldr2
  float old = 0; 
  int mval1[20]; 
  int mval2[20]; 
  int index1 = 0; 
  int index2 = 0; 
 	for (;;) 
    {
        static float out = 0;
        int val1 = map(sensorReadLux(15), 0, 4095, 0, 50);            
        int val2 = map(sensorReadLux(2), 0, 4095, 0, 60);   
        
        if(index1 < 20) {mval1[index1++] = val1;}
        if(index2 < 20) {mval2[index2++] = val2;}  

        if(index1 >= 20 && index2 >= 20) 
        {
            index1=0;
            index2=0;

            /*long newAvg1 = 0;
            {
                int count = sizeof(mval1) / sizeof(int);
            
                for(int i = 0; i < count; i++){
                    newAvg1 += mval1[i];
                }
                newAvg1 /= 20;
            }

            /*long newAvg2 = 0;
            {
                int count = sizeof(mval2) / sizeof(int);
            
                for(int i = 0; i < count; i++){
                    newAvg2 += mval2[i];
                }
                newAvg2 /= 20;
            }    */

           // if((newAvg1-newAvg2+out) <= -75) fuzzy->setInput(1, -75); else
           // if((newAvg1-newAvg2+out) >= 75) fuzzy->setInput(1, 75); else
            fuzzy->setInput(1, val1);
            fuzzy->setInput(2, val2);

            		
            fuzzy->fuzzify();
            out = fuzzy->defuzzify(1);

            if(1)
            {
                Serial.print("Temperatura: ");
                Serial.println(val1);
                Serial.print("N Pessoas ");
                Serial.println(val2);
                Serial.print("Result: ");
                Serial.println(out);
            }
                
            set_position(out);
 
        }

    }   
}

void set_position(int step)
{
   static int step_old = 0; 
       if((step - step_old) < 0) digitalWrite(14, LOW);
        if((step - step_old) > 0) digitalWrite(14, HIGH);
    digitalWrite(12, LOW);
   for(int i = 0; i < abs(step - step_old)*2; ++i )
   {
       int state = digitalRead(12);
       digitalWrite(12, !state);
       delay(15);
   }
   step_old = step;
}
void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}