#include <stdio.h>
#include <stdbool.h>
#include <stdlib.h>
#include "ultra.h"
#include "Fuzzy.h"

#define MAX_DISTANCE_CM 500 // 5m max
#define TRIGGER_GPIO ((gpio_num_t)17)
#define ECHO_GPIO ((gpio_num_t)16)
#define BTN_UP 5
#define BTN_DOWN 4
#define PWM_PIN 18
#define PWM_FREQUENCIA 5000
#define PWM_CHANNEL  0
#define PWM_RESOLUCAO 8 

int qtd_pessoas = 0; 
float distance = 0; 
long distancia_m = 0;

Fuzzy * fuzzy; 
void task_button(void*pvParameters) 
{
   pinMode(BTN_UP, INPUT_PULLUP);
   pinMode(BTN_DOWN, INPUT_PULLUP); 
    for(;;) 
    {
      if(digitalRead(BTN_UP) == LOW) {
         if(qtd_pessoas < 30) {
             qtd_pessoas++;
             Serial.println(qtd_pessoas);
         }
         delay(300);  //ms
      }
      if(digitalRead(BTN_DOWN) == LOW) {
         if(qtd_pessoas > 0) {
             qtd_pessoas--;
             Serial.println(qtd_pessoas);
         }
         delay(300);  //ms
      }

      delay(10); //necessário para que esta task seja bloqueada em algum momento
    }
}

void task_ultrasonic(void *pvParameters)
{
    ultrasonic_sensor_t sensor = {
        .trigger_pin = TRIGGER_GPIO,
        .echo_pin = ECHO_GPIO
    };

    ultrasonic_init(&sensor);

    while (true)
    {   
        esp_err_t res = ultrasonic_measure(&sensor, MAX_DISTANCE_CM, &distance);
        if (res != ESP_OK)
        {
            printf("Error %d: ", res);
            switch (res)
            {
                case ESP_ERR_ULTRASONIC_PING:
                    printf("Cannot ping (device is in invalid state)\n");
                    break;
                case ESP_ERR_ULTRASONIC_PING_TIMEOUT:
                    printf("Ping timeout (no device found)\n");
                    break;
                case ESP_ERR_ULTRASONIC_ECHO_TIMEOUT:
                    printf("Echo timeout (i.e. distance too big)\n");
                    break;
                default:
                    printf("%s\n", esp_err_to_name(res));
            }
        }
        else {
              printf("Distance: %0.04f m\n", distance);
              if(distance > 0.40) distance = 0.40; 
              distancia_m = map( (int)(distance*100), 0, 0.40*100, -5, 35);
        }
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

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

    /* ---- temperatura ---- */
	/*
		aloca espaço na memoria para conter o indice da primeira entrada
		utilizando a classe FuzzyInput
	*/
    FuzzyInput * temperatura = new FuzzyInput(1); 
    
	/*
		fornece os valores para zonas para montagem do grafico utilizando 
		a classe FuzzySet
	*/
    FuzzySet * temp_muito_baixa = new FuzzySet(0, 0, 5, 25);  //muito baixo
    temperatura->addFuzzySet(temp_muito_baixa);
	
    FuzzySet * temp_baixa = new FuzzySet(5, 25, 25, 45); //baixo
    temperatura->addFuzzySet(temp_baixa);
	
    FuzzySet * temp_media = new FuzzySet(25, 45, 45, 75); //médio
    temperatura->addFuzzySet(temp_media);
	
    FuzzySet * temp_alta = new FuzzySet(50, 65, 65, 85); //alto
    temperatura->addFuzzySet(temp_alta);
	
    FuzzySet * temp_muito_alta = new FuzzySet(65, 85, 100, 100); //muito alto
    temperatura->addFuzzySet(temp_muito_alta);
	
	//o objeto adiciona a primeira variável 
	fuzzy->addFuzzyInput(temperatura);    	

    /* ---- n° pessoas ---- */
    FuzzyInput * n_pessoas = new FuzzyInput(2);
    
    FuzzySet * n_pequeno = new FuzzySet(0, 0, 10, 25); //pequeno
    n_pessoas->addFuzzySet(n_pequeno);
	
    FuzzySet * n_medio = new FuzzySet(10, 25, 25, 40); //médio
    n_pessoas->addFuzzySet(n_medio);
	
    FuzzySet * n_grande = new FuzzySet(25, 40, 60, 60); //grande
    n_pessoas->addFuzzySet(n_grande);
	
    fuzzy->addFuzzyInput(n_pessoas);    	
	

    /* ---- potencia do A/C ---- */
    FuzzyOutput * potencia_ac = new FuzzyOutput(1);
    
    FuzzySet *pot_muito_baixa = new FuzzySet(0, 0, 20, 40); //muito baixa
    potencia_ac->addFuzzySet(pot_muito_baixa);
	
    FuzzySet *pot_baixa = new FuzzySet(20, 40, 40, 60); //baixa
    potencia_ac->addFuzzySet(pot_baixa);
	
    FuzzySet *pot_media = new FuzzySet(40, 60, 60, 80); //médio
    potencia_ac->addFuzzySet(pot_media);
	
    FuzzySet *pot_alta = new FuzzySet(60, 80, 100, 100); //alta
    potencia_ac->addFuzzySet(pot_alta);
    			    
	fuzzy->addFuzzyOutput(potencia_ac);    	
	

	/*
	1. If temperatura estiver MUITO BAIXA e PEQUENO grupo de pessoas. 
	*/ 
    FuzzyRuleAntecedent *ifMB_P = new FuzzyRuleAntecedent();
    ifMB_P->joinWithAND(temp_muito_baixa, n_pequeno);

    FuzzyRuleConsequent *pot_MB1 = new FuzzyRuleConsequent();
    pot_MB1->addOutput(pot_muito_baixa);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule1 = new FuzzyRule(1, ifMB_P, pot_MB1);
    fuzzy->addFuzzyRule(fuzzyRule1);
	//====================================================================================
	
    /*
	2. If temperatura estiver MUITO BAIXA e MÉDIO grupo de pessoas.
	*/	
    FuzzyRuleAntecedent *ifMB_M = new FuzzyRuleAntecedent();
    ifMB_M->joinWithAND(temp_muito_baixa, n_medio);

    FuzzyRuleConsequent *pot_MB2 = new FuzzyRuleConsequent();
    pot_MB2->addOutput(pot_muito_baixa);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule2 = new FuzzyRule(2, ifMB_M, pot_MB2);
    fuzzy->addFuzzyRule(fuzzyRule2);	
	//====================================================================================
	
    /*
	3. If temperatura estiver MUITO BAIXA e GRANDE grupo de pessoas.
	*/	
    FuzzyRuleAntecedent *ifMB_G = new FuzzyRuleAntecedent();
    ifMB_G->joinWithAND(temp_muito_baixa, n_grande);

    FuzzyRuleConsequent *pot_MB3 = new FuzzyRuleConsequent();
    pot_MB3->addOutput(pot_muito_baixa);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule3 = new FuzzyRule(3, ifMB_G, pot_MB3);
    fuzzy->addFuzzyRule(fuzzyRule3);
	//====================================================================================

	/*
	4. If temperatura estiver BAIXO e PEQUENO grupo de pessoas.
	*/
    FuzzyRuleAntecedent *ifB_P = new FuzzyRuleAntecedent();
    ifB_P->joinWithAND(temp_baixa, n_pequeno);

    FuzzyRuleConsequent *pot_MB4 = new FuzzyRuleConsequent();
    pot_MB4->addOutput(pot_muito_baixa);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule4 = new FuzzyRule(4, ifB_P, pot_MB4);
    fuzzy->addFuzzyRule(fuzzyRule4);	
	//====================================================================================

	/*
	5. If temperatura estiver BAIXO e MÉDIO grupo de pessoas.
	*/	
    FuzzyRuleAntecedent *ifB_M = new FuzzyRuleAntecedent();
    ifB_M->joinWithAND(temp_baixa, n_medio);

    FuzzyRuleConsequent *pot_B5 = new FuzzyRuleConsequent();
    pot_B5->addOutput(pot_baixa);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule5 = new FuzzyRule(5, ifB_M, pot_B5);
    fuzzy->addFuzzyRule(fuzzyRule5);
	//====================================================================================
	
	/*
	6. If temperatura estiver BAIXO e GRANDE grupo de pessoas.
	*/	
    FuzzyRuleAntecedent *ifB_G = new FuzzyRuleAntecedent();
    ifB_G->joinWithAND(temp_baixa, n_grande);

    FuzzyRuleConsequent *pot_B6 = new FuzzyRuleConsequent();
    pot_B6->addOutput(pot_baixa);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule6 = new FuzzyRule(6, ifB_G, pot_B6);
    fuzzy->addFuzzyRule(fuzzyRule6);
	//====================================================================================
	
	/*
	7. If temperatura estiver MÉDIO e PEQUENO grupo de pessoas.
	*/	
    FuzzyRuleAntecedent *ifM_P = new FuzzyRuleAntecedent();
    ifM_P->joinWithAND(temp_media, n_pequeno);

    FuzzyRuleConsequent *pot_M7 = new FuzzyRuleConsequent();
    pot_M7->addOutput(pot_media);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule7 = new FuzzyRule(7, ifM_P, pot_M7);
    fuzzy->addFuzzyRule(fuzzyRule7);
	//====================================================================================
	
	/*
	8. If temperatura estiver MÉDIO e MÉDIO grupo de pessoas.
	*/	
    FuzzyRuleAntecedent *ifM_M = new FuzzyRuleAntecedent();
    ifM_M->joinWithAND(temp_media, n_medio);

    FuzzyRuleConsequent *pot_M8 = new FuzzyRuleConsequent();
    pot_M8->addOutput(pot_media);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule8 = new FuzzyRule(8, ifM_M, pot_M8);
    fuzzy->addFuzzyRule(fuzzyRule8);
	//====================================================================================
	
	/*
	9. If temperatura estiver MÉDIO e GRANDE grupo de pessoas.
	*/
    FuzzyRuleAntecedent *ifM_G = new FuzzyRuleAntecedent();
    ifM_G->joinWithAND(temp_media, n_grande);

    FuzzyRuleConsequent *pot_M9 = new FuzzyRuleConsequent();
    pot_M9->addOutput(pot_media);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule9 = new FuzzyRule(9, ifM_G, pot_M9);
    fuzzy->addFuzzyRule(fuzzyRule9);
	//====================================================================================
	
	/*
	10. If temperatura estiver ALTO e PEQUENO grupo de pessoas.
	*/
    FuzzyRuleAntecedent *ifA_P = new FuzzyRuleAntecedent();
    ifA_P->joinWithAND(temp_alta, n_pequeno);

    FuzzyRuleConsequent *pot_M10 = new FuzzyRuleConsequent();
    pot_M10->addOutput(pot_media);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule10 = new FuzzyRule(10, ifA_P, pot_M10);
    fuzzy->addFuzzyRule(fuzzyRule10);
	//====================================================================================
	
	/*
	11. If temperatura estiver ALTO e MÉDIO grupo de pessoas.
	*/	
    FuzzyRuleAntecedent *ifA_M = new FuzzyRuleAntecedent();
    ifA_M->joinWithAND(temp_alta, n_medio);

    FuzzyRuleConsequent *pot_A11 = new FuzzyRuleConsequent();
    pot_A11->addOutput(pot_alta);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule11 = new FuzzyRule(11, ifA_M, pot_A11);
    fuzzy->addFuzzyRule(fuzzyRule11);
	//====================================================================================
	
	/*
	12. If temperatura estiver ALTO e GRANDE grupo de pessoas.
	*/	
    FuzzyRuleAntecedent *ifA_G = new FuzzyRuleAntecedent();
    ifA_G->joinWithAND(temp_alta, n_medio);

    FuzzyRuleConsequent *pot_A12 = new FuzzyRuleConsequent();
    pot_A12->addOutput(pot_alta);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule12 = new FuzzyRule(12, ifA_G, pot_A12);
    fuzzy->addFuzzyRule(fuzzyRule12);
	//====================================================================================
	
	/*
	13. If temperatura estiver MUITO ALTO e PEQUENO grupo de pessoas.
	*/	
	FuzzyRuleAntecedent *ifMA_P = new FuzzyRuleAntecedent();
    ifMA_P->joinWithAND(temp_alta, n_medio);

    FuzzyRuleConsequent *pot_A13 = new FuzzyRuleConsequent();
    pot_A13->addOutput(pot_alta);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule13 = new FuzzyRule(13, ifMA_P, pot_A13);
    fuzzy->addFuzzyRule(fuzzyRule12);
	//====================================================================================
	
	/*
	14. If temperatura estiver MUITO ALTO e MÉDIO grupo de pessoas.
	*/
	FuzzyRuleAntecedent *ifMA_M = new FuzzyRuleAntecedent();
    ifMA_M->joinWithAND(temp_alta, n_medio);

    FuzzyRuleConsequent *pot_A14 = new FuzzyRuleConsequent();
    pot_A14->addOutput(pot_alta);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule14 = new FuzzyRule(14, ifMA_M, pot_A14);
    fuzzy->addFuzzyRule(fuzzyRule12);
	//====================================================================================
	
	/*
	15. If temperatura estiver MUITO ALTO e GRANDE grupo de pessoas.
	*/
	FuzzyRuleAntecedent *ifMA_G = new FuzzyRuleAntecedent();
    ifMA_G->joinWithAND(temp_alta, n_medio);

    FuzzyRuleConsequent *pot_A15 = new FuzzyRuleConsequent();
    pot_A15->addOutput(pot_alta);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule15 = new FuzzyRule(15, ifMA_G, pot_A15);
    fuzzy->addFuzzyRule(fuzzyRule12);
	//====================================================================================
	
    xTaskCreate(task_ultrasonic, "task_ultrasonic", configMINIMAL_STACK_SIZE * 3, NULL, 5, NULL);
    xTaskCreate(task_button, "task_button", configMINIMAL_STACK_SIZE * 3, NULL, 5, NULL);



ledcSetup(PWM_CHANNEL, PWM_FREQUENCIA, PWM_RESOLUCAO);
ledcAttachPin(PWM_PIN, PWM_CHANNEL);

}

void loop() 
{

    fuzzy->setInput(1, distancia_m); //temp=7 graus celsius
	  fuzzy->setInput(2, qtd_pessoas);	//velocidade do vendo = 22 nós
    fuzzy->fuzzify();
    float output = fuzzy->defuzzify(1); 
    
    int pwm_value = (int)map( (int)(output), -25, 35, 0, 100);
    Serial.printf("out=%.4f, dist=%d, qtd_p=%d, pwm=%d\n", output, distancia_m, qtd_pessoas, pwm_value);
    ledcWrite(PWM_CHANNEL, pwm_value);

    delay(50);
}