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

#include "Fuzzy.h"

#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include "ultra.h"
#include <esp_err.h>

#define MAX_TIMEOUT 500 //ms

#define TRIGGER_GPIO_1 ((gpio_num_t)17)
#define ECHO_GPIO_1 ((gpio_num_t)16)

#define TRIGGER_GPIO_2 ((gpio_num_t)18)
#define ECHO_GPIO_2 ((gpio_num_t)5)



float distance1;
float distance2;
float angulo; 

Fuzzy *fuzzy = new Fuzzy();

void task_ultrasonic1(void *pvParameters)
{
    ultrasonic_sensor_t sensor = {
        .trigger_pin = TRIGGER_GPIO_1,
        .echo_pin = ECHO_GPIO_1
    };

    ultrasonic_init(&sensor);

    while (true)
    {
        
        esp_err_t res = ultrasonic_measure(&sensor, MAX_TIMEOUT, &distance1);
        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. distance1 too big)\n");
                    break;
                default:
                    printf("%s\n", esp_err_to_name(res));
            }
        }
        else
            printf("Distance1: %0.04f m\n", distance1);

        vTaskDelay(pdMS_TO_TICKS(500));

    }
}



void task_ultrasonic2(void *pvParameters)
{
    ultrasonic_sensor_t sensor = {
        .trigger_pin = TRIGGER_GPIO_2,
        .echo_pin = ECHO_GPIO_2
    };

    ultrasonic_init(&sensor);

    while (true)
    {
        
      
        esp_err_t res = ultrasonic_measure(&sensor, MAX_TIMEOUT, &distance2);
        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. distance2 too big)\n");
                    break;
                default:
                    printf("%s\n", esp_err_to_name(res));
            }
        }
        else
            printf("Distance2: %0.04f m\n", distance2);

        vTaskDelay(pdMS_TO_TICKS(500));
      
    }
}

void setup() {
    
    Serial.begin(115200);
    


  /* ultrassonico Direita */
    FuzzyInput * ultraDireita = new FuzzyInput(1); //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);  




   /* ultrassonico Esquerda */
    FuzzyInput * ultraEsquerda = new FuzzyInput(2); //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); 	
	
     	
	
    /* 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: ultraEsquerda_P e ultraDireita_P => angular_C
    FuzzyRuleAntecedent *ifUltraEsquerdaPAndUltraDireitaP = new FuzzyRuleAntecedent();
    ifUltraEsquerdaPAndUltraDireitaP->joinWithAND(ultraEsquerda_P, ultraDireita_P);
    FuzzyRuleConsequent *thenAngularIsAngular_C = new FuzzyRuleConsequent();
    thenAngularIsAngular_C->addOutput(angular_C);
    FuzzyRule *fuzzyRule1 = new FuzzyRule(1, ifUltraEsquerdaPAndUltraDireitaP, thenAngularIsAngular_C);
    fuzzy->addFuzzyRule(fuzzyRule1);

    // Regra 2: ultraEsquerda_M e ultraDireita_P => angular_PD
    FuzzyRuleAntecedent *ifUltraEsquerdaMAndUltraDireitaP = new FuzzyRuleAntecedent();
    ifUltraEsquerdaMAndUltraDireitaP->joinWithAND(ultraEsquerda_M, ultraDireita_P);
    FuzzyRuleConsequent *thenAngularIsAngular_PD = new FuzzyRuleConsequent();
    thenAngularIsAngular_PD->addOutput(angular_PD);
    FuzzyRule *fuzzyRule2 = new FuzzyRule(2, ifUltraEsquerdaMAndUltraDireitaP, thenAngularIsAngular_PD);
    fuzzy->addFuzzyRule(fuzzyRule2);

    // Regra 3: ultraEsquerda_L e ultraDireita_P => angular_D
    FuzzyRuleAntecedent *ifUltraEsquerdaLAndUltraDireitaP = new FuzzyRuleAntecedent();
    ifUltraEsquerdaLAndUltraDireitaP->joinWithAND(ultraEsquerda_L, ultraDireita_P);
    FuzzyRuleConsequent *thenAngularIsAngular_D = new FuzzyRuleConsequent();
    thenAngularIsAngular_D->addOutput(angular_D);
    FuzzyRule *fuzzyRule3 = new FuzzyRule(3, ifUltraEsquerdaLAndUltraDireitaP, thenAngularIsAngular_D);
    fuzzy->addFuzzyRule(fuzzyRule3);

    // Regra 4: ultraEsquerda_P e ultraDireita_M => angular_PE
    FuzzyRuleAntecedent *ifUltraEsquerdaPAndUltraDireitaM = new FuzzyRuleAntecedent();
    ifUltraEsquerdaPAndUltraDireitaM->joinWithAND(ultraEsquerda_P, ultraDireita_M);
    FuzzyRuleConsequent *thenAngularIsAngular_PE = new FuzzyRuleConsequent();
    thenAngularIsAngular_PE->addOutput(angular_PE);
    FuzzyRule *fuzzyRule4 = new FuzzyRule(4, ifUltraEsquerdaPAndUltraDireitaM, thenAngularIsAngular_PE);
    fuzzy->addFuzzyRule(fuzzyRule4);

    // Regra 5: ultraEsquerda_M e ultraDireita_M => angular_C
    FuzzyRuleAntecedent *ifUltraEsquerdaMAndUltraDireitaM = new FuzzyRuleAntecedent();
    ifUltraEsquerdaMAndUltraDireitaM->joinWithAND(ultraEsquerda_M, ultraDireita_M);
    FuzzyRuleConsequent *thenAngularIsAngular_C_2 = new FuzzyRuleConsequent();
    thenAngularIsAngular_C_2->addOutput(angular_C);
    FuzzyRule *fuzzyRule5 = new FuzzyRule(5, ifUltraEsquerdaMAndUltraDireitaM, thenAngularIsAngular_C_2);
    fuzzy->addFuzzyRule(fuzzyRule5);

    // Regra 6: ultraEsquerda_L e ultraDireita_M => angular_D
    FuzzyRuleAntecedent *ifUltraEsquerdaLAndUltraDireitaM = new FuzzyRuleAntecedent();
    ifUltraEsquerdaLAndUltraDireitaM->joinWithAND(ultraEsquerda_L, ultraDireita_M);
    FuzzyRuleConsequent *thenAngularIsAngular_D_2 = new FuzzyRuleConsequent();
    thenAngularIsAngular_D_2->addOutput(angular_D);
    FuzzyRule *fuzzyRule6 = new FuzzyRule(6, ifUltraEsquerdaLAndUltraDireitaM, thenAngularIsAngular_D_2);
    fuzzy->addFuzzyRule(fuzzyRule6);

    // Regra 7: ultraEsquerda_P e ultraDireita_L => angular_E
    FuzzyRuleAntecedent *ifUltraEsquerdaPAndUltraDireitaL = new FuzzyRuleAntecedent();
    ifUltraEsquerdaPAndUltraDireitaL->joinWithAND(ultraEsquerda_P, ultraDireita_L);
    FuzzyRuleConsequent *thenAngularIsAngular_E = new FuzzyRuleConsequent();
    thenAngularIsAngular_E->addOutput(angular_E);
    FuzzyRule *fuzzyRule7 = new FuzzyRule(7, ifUltraEsquerdaPAndUltraDireitaL, thenAngularIsAngular_E);
    fuzzy->addFuzzyRule(fuzzyRule7);

    // Regra 8: ultraEsquerda_M e ultraDireita_L => angular_E
    FuzzyRuleAntecedent *ifUltraEsquerdaMAndUltraDireitaL = new FuzzyRuleAntecedent();
    ifUltraEsquerdaMAndUltraDireitaL->joinWithAND(ultraEsquerda_M, ultraDireita_L);
    FuzzyRuleConsequent *thenAngularIsAngular_E_2 = new FuzzyRuleConsequent();
    thenAngularIsAngular_E_2->addOutput(angular_E);
    FuzzyRule *fuzzyRule8 = new FuzzyRule(8, ifUltraEsquerdaMAndUltraDireitaL, thenAngularIsAngular_E_2);
    fuzzy->addFuzzyRule(fuzzyRule8);

    // Regra 9: ultraEsquerda_L e ultraDireita_L => angular_C
    FuzzyRuleAntecedent *ifUltraEsquerdaLAndUltraDireitaL = new FuzzyRuleAntecedent();
    ifUltraEsquerdaLAndUltraDireitaL->joinWithAND(ultraEsquerda_L, ultraDireita_L);
    FuzzyRuleConsequent *thenAngularIsAngular_C_3 = new FuzzyRuleConsequent();
    thenAngularIsAngular_C_3->addOutput(angular_C);
    FuzzyRule *fuzzyRule9 = new FuzzyRule(9, ifUltraEsquerdaLAndUltraDireitaL, thenAngularIsAngular_C_3);
    fuzzy->addFuzzyRule(fuzzyRule9);
	
	
	 //				   
    fuzzy->setInput(1, distance1); // ultrassonico Esquerda
	  fuzzy->setInput(2, distance2);	// ultrassonico Direta
    fuzzy->fuzzify();
    float output = fuzzy->defuzzify(1); 
    //float angulo = fuzzy->defuzzify(1); 

    xTaskCreate(task_ultrasonic1, "task_ultrasonic1", configMINIMAL_STACK_SIZE * 3, NULL, 5, NULL);

    xTaskCreate(task_ultrasonic2, "task_ultrasonic2", configMINIMAL_STACK_SIZE * 3, NULL, 5, NULL);


    //Serial.println(output);
}

void loop() {
  // put your main code here, to run repeatedly:
  vTaskDelay(pdMS_TO_TICKS(500));

    fuzzy->setInput(1, 12.5); // ultrassonico Esquerda
	  fuzzy->setInput(2, 37.5);	// ultrassonico Direta
    fuzzy->fuzzify();
    float output = fuzzy->defuzzify(1);
    Serial.println(output);
  
  //Serial.println(angulo);
  delay(100); // this speeds up the simulation
}