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

#include "Fuzzy.h"

#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <sys/time.h>
#include "pcf8574.h"
#include "hd44780.h"
#include "i2cdev.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

/**
 * FreeRTOS
 */
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

/**
 * ESP32;
 */
#include "esp_err.h"
#include "esp_log.h"

/**
 * Servo lib;
 */
#include <ESP32Servo.h>



#define SDA_GPIO (gpio_num_t)16
#define SCL_GPIO (gpio_num_t)17
#define I2C_ADDR 0x27

static i2c_dev_t pcf8574;

char MENSAGEM[] = "SEJAM BEM VINDO";

static esp_err_t write_lcd_data(const hd44780_t *lcd, uint8_t data)
{
    return pcf8574_port_write(&pcf8574, data);
}

hd44780_t lcd = {
    .write_cb = write_lcd_data, // use callback to send data to LCD by I2C GPIO expander
    .pins = {
        .rs = 0,
        .e  = 2,
        .d4 = 4,
        .d5 = 5,
        .d6 = 6,
        .d7 = 7,
        .bl = 3
    },
    .font = HD44780_FONT_5X8,
    .lines = 4,
    .backlight = 1
};

void setup() {
    
    Serial.begin(115200);
    Fuzzy *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);
	
	 //				   
    fuzzy->setInput(1, 35); //temp=7 graus celsius
	  fuzzy->setInput(2, 0);	//velocidade do vendo = 22 nós
    fuzzy->fuzzify();
    float output = fuzzy->defuzzify(1); 
    Serial.println(output);

    //DISPLAY

    Serial.begin(115200);
  Serial.println(MENSAGEM);

  ESP_ERROR_CHECK(i2cdev_init());
  
  memset(&pcf8574, 0, sizeof(i2c_dev_t));
  ESP_ERROR_CHECK(pcf8574_init_desc(&pcf8574, 0, I2C_ADDR, SDA_GPIO, SCL_GPIO));
  
  /* inicializa o display lcd */
  hd44780_init(&lcd);
 
  /* posiciona o cursor na COLUNA X LINHA */
  hd44780_gotoxy(&lcd, 0, 0);

  /* escreve na linha (0) do display lcd */ 
  hd44780_puts(&lcd, MENSAGEM);

   //Opção-1
   //hd44780_clear(&lcd);
   hd44780_gotoxy(&lcd, 0, 1); //coluna x linha
   hd44780_puts(&lcd, "Linha-1");
   hd44780_gotoxy(&lcd, 0, 2);
   hd44780_puts(&lcd, "Linha-2");
  delay(5000);

    Serial.begin(115200);

	if(xTaskCreate(vtask_motor, "task_motor", 1024 * 5, NULL, 2, NULL) != pdPASS) {
    Serial.println("error - Nao foi possivel alocar task_motor.\r\n" );  
    return;   
  }
}

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

/**
 * Config. do servo motor;
 */
static Servo myservo;
const int servoPin = 18;  

/**
 * Define a posição do servo motor;
 */
const int targetPosition = 100;  
}
//      PID
/**
 * Config. do servo motor;
 */
static Servo myservo;
const int servoPin = 18;  

/**
 * Define a posição do servo motor;
 */
const int targetPosition = 100;  

/*
  O método de ajuste dos ganhos de um controlador PID é geralmente feito por tentativa e erro. 
  Inicialmente, os termos integral (KI) e derivativo (KD) são zerados, e o ganho proporcional (KP) 
  é aumentado até que ocorram oscilações na saída do sistema. Aumentar o ganho proporcional torna 
  o sistema mais rápido, mas é necessário cuidado para evitar instabilidades. Uma vez que o ganho 
  proporcional é ajustado para uma resposta rápida desejada, o termo integral (KI) é aumentado para 
  minimizar o erro em estado estacionário, mas isso pode aumentar o overshoot. Um certo nível de 
  overshoot é necessário para uma resposta rápida do sistema. O termo integral (KI) é então ajustado para 
  reduzir esse erro mínimo em estado estacionário. Posteriormente, o termo derivativo (KD) é aumentado 
  para melhorar a velocidade de resposta do sistema, diminuir o overshoot e aumentar a estabilidade, 
  mas isso pode tornar o sistema mais sensível ao ruído. Geralmente, é preciso fazer adequações 
  entre diferentes características do sistema para atender aos requisitos específicos de controle.
*/
/**
 * Config. PID;
 */   
const float dt = 0.02; //intevalo em s   ((0.02*1000) = 2 ms)    
/*
  A componente proporcional depende apenas da diferença entre o set point (targetPosition) e a variável 
  do processo. Essa diferença é chamada de termo de erro. O ganho proporcional (Kp ) determina a 
  relação entre a resposta de saída e o sinal de erro. Por exemplo, se o termo de erro tiver uma 
  magnitude de 10, um ganho proporcional de 5 produziria uma resposta proporcional de 50. Em geral, 
  aumentar o ganho   proporcional aumentará a velocidade da resposta do sistema de controle. 
  Porém, se o ganho proporcional for muito grande, a variável do processo começará a oscilar. Se Kp aumentar ainda mais, 
  as oscilações se tornarão maiores e o sistema se tornará instável e poderá até oscilar fora de controle.
*/
const float Kp = 0.5;  

/*
  O componente Ki integral soma o termo de erro ao longo do tempo. O resultado é que mesmo um pequeno 
  termo de erro fará com que o componente integral aumente lentamente. A resposta integral aumentará 
  continuamente ao longo do tempo, a menos que o erro seja zero, então o efeito é levar o erro de 
  estado estacionário a zero. O erro de estado estacionário é a diferença final entre a variável do 
  processo e o ponto de ajuste. Um fenômeno chamado encerramento integral ocorre quando a ação integral 
  satura um controlador sem que o controlador conduza o sinal de erro para zero.
*/
const float Ki = 0.1;        
       
/*
  O componente Kd derivativo faz com que a produção diminua se a variável do processo estiver aumentando 
  rapidamente. A resposta derivada é proporcional à taxa de mudança da variável do processo. 
  Aumentar o parâmetro do tempo derivativo (Td ) fará com que o sistema de controle reaja mais 
  fortemente às mudanças no termo de erro e aumentará a velocidade da resposta geral do sistema 
  de controle. A maioria dos sistemas de controle práticos utilizam um tempo derivativo 
  muito pequeno (Td ) , porque a Resposta Derivativa é altamente sensível ao ruído no sinal 
  variável do processo. Se o sinal de feedback do sensor for ruidoso ou se a taxa do circuito de 
  controle for muito lenta, a resposta derivada pode tornar o sistema de controle instável.
*/
const float Kd = 0.01; 
/**
 * Config. coeficiente do filtro exponential (de 0 a 1);
 * 1 -> não se aplica o filtro; 
 * 0 -> utiliza o valor anterior;
 */   
static float coefficient = 1; 

static float integral = 0;
static float previousError = 0;
/*
  O filtro exponencial opera aplicando uma média ponderada exponencial 
  aos dados de entrada. Em vez de atribuir pesos iguais a todos os pontos 
  de dados, ele dá mais peso aos dados mais recentes e reduz o peso 
  à medida que os dados ficam mais antigos. Isso significa que o filtro 
  exponencial é mais sensível às mudanças recentes nos dados do que aos 
  valores históricos.
*/
void set_filter_coefficient ( float newCoefficient )
{
    if (( newCoefficient >= 0  ) && ( newCoefficient <= 1  )){
      coefficient = newCoefficient;
    }      
}
float filter( float sample )
{
    static float result; 
    result = coefficient*sample + (1-coefficient)*result;
    return result;
}

/*
    task de controle;
*/
void vtask_motor(void * pvParameter) 
{
    set_filter_coefficient(coefficient);
    
    /**
    * Initcializa servo motor;
    * Posiciona servo motor na posição central;
    */
    myservo.attach(servoPin);
    myservo.write(0); 

    for(;;) 
    {
        float currentPosition = filter(myservo.read());
        float error = targetPosition - currentPosition;

        /**
        * Calcula os componentes do controlador PID;
        */
        float proportional = Kp * error;
        integral += Ki * error * dt;
        float derivative = Kd * (error - previousError) / dt;
        /**
        * Calcula a saída do controlador PID;
        */
        float output = proportional + integral + derivative;

        /**
        * Aplica a saída do controlador PID ao servo motor;
        */
        myservo.write(currentPosition + output);
        previousError = error;

        Serial.printf("Position = %.2f, Output = %.2f\n", 
              currentPosition, output);
  
        delay(dt * 1000); 
    }
}