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

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

Fuzzy * fuzzy; 
void task_button(void*pvParameters) 
{
   #define BTN_UP 5
   #define BTN_DOWN 4
   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 */
    FuzzyInput * temperatura = new FuzzyInput(1); //indice da entrada;
    
    FuzzySet * temperaturacold = new FuzzySet(-5, -5, 0, 10);
    temperatura->addFuzzySet(temperaturacold);
    FuzzySet * temperaturacool = new FuzzySet(0, 10, 10, 20);
    temperatura->addFuzzySet(temperaturacool);
    FuzzySet * temperaturawarm = new FuzzySet(10, 20, 20, 30);
    temperatura->addFuzzySet(temperaturawarm);
    FuzzySet * temperaturahot = new FuzzySet(20, 30, 35, 35);
    temperatura->addFuzzySet(temperaturahot);
	fuzzy->addFuzzyInput(temperatura);    	
	
    /* velocidade do vento */
    FuzzyInput * windspeed = new FuzzyInput(2);
    
    FuzzySet * windspeedlow = new FuzzySet(0, 0, 5, 17.5);
    windspeed->addFuzzySet(windspeedlow);
    FuzzySet * windspeedgentle = new FuzzySet(2.5, 15, 15, 27.5);
    windspeed->addFuzzySet(windspeedgentle);
    FuzzySet * windspeedhigh = new FuzzySet(12.5, 25, 30, 30);
    windspeed->addFuzzySet(windspeedhigh);
    fuzzy->addFuzzyInput(windspeed);    	
	
    /* WIND CHILL */
    FuzzyOutput * windchill = new FuzzyOutput(1);
    
    FuzzySet *windchillsevere = new FuzzySet(-40, -25, -25, -10);
    windchill->addFuzzySet(windchillsevere);
    FuzzySet *windchillbad = new FuzzySet(-25, -10, -10, 5);
    windchill->addFuzzySet(windchillbad);
    FuzzySet *windchillbearable = new FuzzySet(-10, 5, 5, 20);
    windchill->addFuzzySet(windchillbearable);
    FuzzySet *windchillmild = new FuzzySet(5, 20, 20, 35);
    windchill->addFuzzySet(windchillmild);
    FuzzySet *windchillunnoticeable = new FuzzySet(20, 35, 35, 50);
    windchill->addFuzzySet(windchillunnoticeable);			    
	fuzzy->addFuzzyOutput(windchill);    	
	

	/*
	1. If TEMPERATURE is COLD and WIND SPEED is LOW, then WIND
	CHILL is BEARABLE.
	*/
    FuzzyRuleAntecedent *ifTempColdAndWindLow = new FuzzyRuleAntecedent();
    ifTempColdAndWindLow->joinWithAND(temperaturacold, windspeedlow);

    FuzzyRuleConsequent *thenWindchillIsBearable = new FuzzyRuleConsequent();
    thenWindchillIsBearable->addOutput(windchillbearable);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule1 = new FuzzyRule(1, ifTempColdAndWindLow, thenWindchillIsBearable);
    fuzzy->addFuzzyRule(fuzzyRule1);

    /*
	2. If TEMPERATURE is COLD and WIND SPEED is GENTLE, then WIND
	CHILL is BAD.
	*/	
    FuzzyRuleAntecedent *ifTempColdAndWindGentle = new FuzzyRuleAntecedent();
    ifTempColdAndWindGentle->joinWithAND(temperaturacold, windspeedgentle);

    FuzzyRuleConsequent *thenWindchillIsBad = new FuzzyRuleConsequent();
    thenWindchillIsBad->addOutput(windchillbad);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule2 = new FuzzyRule(2, ifTempColdAndWindGentle, thenWindchillIsBad);
    fuzzy->addFuzzyRule(fuzzyRule2);	

    /*
		3. If TEMPERATURE is COLD and WIND SPEED is HIGH, then WIND
		CHILL is SEVERE.
	*/	
    FuzzyRuleAntecedent *ifTempColdAndWindHigh = new FuzzyRuleAntecedent();
    ifTempColdAndWindHigh->joinWithAND(temperaturacold, windspeedhigh);

    FuzzyRuleConsequent *thenWindchillIsSevere = new FuzzyRuleConsequent();
    thenWindchillIsSevere->addOutput(windchillsevere);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule3 = new FuzzyRule(3, ifTempColdAndWindHigh, thenWindchillIsSevere);
    fuzzy->addFuzzyRule(fuzzyRule3);

	/*
	4. If TEMPERATURE is COOL and WIND SPEED is LOW, then WIND
	CHILL is MILD.
	*/
    FuzzyRuleAntecedent *ifTempCoolAndWindLow = new FuzzyRuleAntecedent();
    ifTempCoolAndWindLow->joinWithAND(temperaturacool, windspeedlow);

    FuzzyRuleConsequent *thenWindchillIsMild = new FuzzyRuleConsequent();
    thenWindchillIsMild->addOutput(windchillmild);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule4 = new FuzzyRule(4, ifTempCoolAndWindLow, thenWindchillIsMild);
    fuzzy->addFuzzyRule(fuzzyRule4);	

	/*
	5. If TEMPERATURE is COOL and WIND SPEED is GENTLE, then WIND
	CHILL is BEARABLE.
	*/	
    FuzzyRuleAntecedent *ifTempCoolAndWindGentle = new FuzzyRuleAntecedent();
    ifTempCoolAndWindGentle->joinWithAND(temperaturacool, windspeedgentle);

    FuzzyRuleConsequent *thenWindchillIsBearable5 = new FuzzyRuleConsequent();
    thenWindchillIsBearable5->addOutput(windchillbearable);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule5 = new FuzzyRule(5, ifTempCoolAndWindGentle, thenWindchillIsBearable5);
    fuzzy->addFuzzyRule(fuzzyRule5);
	
	/*
	6. If TEMPERATURE is COOL and WIND SPEED is HIGH, then WIND
	CHILL is BAD.
	*/	
    FuzzyRuleAntecedent *ifTempCoolAndWindHigh = new FuzzyRuleAntecedent();
    ifTempCoolAndWindHigh->joinWithAND(temperaturacool, windspeedhigh);

    FuzzyRuleConsequent *thenWindchillIsBad6 = new FuzzyRuleConsequent();
    thenWindchillIsBad6->addOutput(windchillbad);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule6 = new FuzzyRule(6, ifTempCoolAndWindHigh, thenWindchillIsBad6);
    fuzzy->addFuzzyRule(fuzzyRule6);

	/*
	7. If TEMPERATURE is WARM and WIND SPEED is LOW, then WIND
	CHILL is UNNOTICEABLE.
	*/	

    FuzzyRuleAntecedent *ifTempWarmAndWindLow = new FuzzyRuleAntecedent();
    ifTempWarmAndWindLow->joinWithAND(temperaturawarm, windspeedlow);

    FuzzyRuleConsequent *thenWindchillIsUnnoticeable = new FuzzyRuleConsequent();
    thenWindchillIsUnnoticeable->addOutput(windchillunnoticeable);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule7 = new FuzzyRule(7, ifTempWarmAndWindLow, thenWindchillIsUnnoticeable);
    fuzzy->addFuzzyRule(fuzzyRule7);
	
	/*
	8. If TEMPERATURE is WARM and WIND SPEED is GENTLE, then WIND
	CHILL is MILD.
	*/	
    FuzzyRuleAntecedent *ifTempWarmAndWindGentle = new FuzzyRuleAntecedent();
    ifTempWarmAndWindGentle->joinWithAND(temperaturawarm, windspeedgentle);

    FuzzyRuleConsequent *thenWindchillIsMild8 = new FuzzyRuleConsequent();
    thenWindchillIsMild8->addOutput(windchillmild);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule8 = new FuzzyRule(8, ifTempWarmAndWindGentle, thenWindchillIsMild8);
    fuzzy->addFuzzyRule(fuzzyRule8);

	/*
	9. If TEMPERATURE is WARM and WIND SPEED is HIGH, then WIND
	CHILL is BEARABLE.
	*/
	
    FuzzyRuleAntecedent *ifTempWarmAndWindHigh = new FuzzyRuleAntecedent();
    ifTempWarmAndWindHigh->joinWithAND(temperaturawarm, windspeedhigh);

    FuzzyRuleConsequent *thenWindchillIsBearable9 = new FuzzyRuleConsequent();
    thenWindchillIsBearable9->addOutput(windchillbearable);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule9 = new FuzzyRule(9, ifTempWarmAndWindHigh, thenWindchillIsBearable9);
    fuzzy->addFuzzyRule(fuzzyRule9);

	/*
	10. If TEMPERATURE is HOT and WIND SPEED is LOW, then WIND CHILL
	is UNNOTICEABLE.
	*/
    FuzzyRuleAntecedent *ifTempHotAndWindLow = new FuzzyRuleAntecedent();
    ifTempHotAndWindLow->joinWithAND(temperaturahot, windspeedlow);

    FuzzyRuleConsequent *thenWindchillIsUnnoticeable10 = new FuzzyRuleConsequent();
    thenWindchillIsUnnoticeable10->addOutput(windchillunnoticeable);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule10 = new FuzzyRule(10, ifTempHotAndWindLow, thenWindchillIsUnnoticeable10);
    fuzzy->addFuzzyRule(fuzzyRule10);
	
	/*
	11. If TEMPERATURE is HOT and WIND SPEED is GENTLE, then WIND
	CHILL is UNNOTICEABLE.
	*/	
    FuzzyRuleAntecedent *ifTempHotAndWindGentle = new FuzzyRuleAntecedent();
    ifTempHotAndWindGentle->joinWithAND(temperaturahot, windspeedgentle);

    FuzzyRuleConsequent *thenWindchillIsUnnoticeable11 = new FuzzyRuleConsequent();
    thenWindchillIsUnnoticeable11->addOutput(windchillunnoticeable);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule11 = new FuzzyRule(11, ifTempHotAndWindGentle, thenWindchillIsUnnoticeable11);
    fuzzy->addFuzzyRule(fuzzyRule11);

	/*
	12. If TEMPERATURE is HOT and WIND SPEED is HIGH, then WIND CHILL
	is MILD.
	*/	
    FuzzyRuleAntecedent *ifTempHotAndWindHigh = new FuzzyRuleAntecedent();
    ifTempHotAndWindHigh->joinWithAND(temperaturahot, windspeedhigh);

    FuzzyRuleConsequent *thenWindchillIsMild12 = new FuzzyRuleConsequent();
    thenWindchillIsMild12->addOutput(windchillmild);
    //INDICE DA REGRA
    FuzzyRule *fuzzyRule12 = new FuzzyRule(12, ifTempHotAndWindHigh, thenWindchillIsMild12);
    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);

#define PWM_PIN 18
#define PWM_FREQUENCIA 5000
#define PWM_CHANNEL  0
#define PWM_RESOLUCAO 8 

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);
}