/**
 * C library
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "ultra.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"


/**
* ultrassonico
*/
#define MAX_DISTANCE_CM 500 // 5m max
#define TRIGGER_GPIO ((gpio_num_t)17)
#define ECHO_GPIO ((gpio_num_t)16)

/**
 * Define a posição do servo motor;
 */
float targetPosition = 200;  

/*
  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.00; 

/**
 * 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.
*/

float filter( float sample )
{
    static float result; 
    result = coefficient*sample + (1-coefficient)*result;
    return result;
}

/*
  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;
    }      
}


static float distance = 0; 

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 {
          distance = distance * 1000;
            if(distance >= 400) distance = 400;
            printf("Distance: %0.04f mm\n", distance);

        }
            

        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

/*
    task de controle;
*/
void task_result(void * pvParameter) 
{
    set_filter_coefficient(coefficient);  

    for(;;) 
    {
        float currentPosition = filter(distance);
        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;
        */
        previousError = error;

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

}

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


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

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


}

void loop(void) {
  vTaskDelay(5000/portTICK_PERIOD_MS);
}