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

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

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

/**
* 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 = 100;  

/**
 * Config. PID;
 */   
const float dt = 0.02; //intevalo em s  varredura ((0.02*1000) = 2 ms)    
const float Kp = 35;          //derivativa comumente em 0(zero)
const float Ki = 32;          //ganho do bloco de integração
const float Kd = 33;          //

/**
 * Config. coeficiente do filtro exponential (de 0 a 1);
 * 1 -> não se aplica o filtro; 
 * 0 -> utiliza o valor anterior;
 */   
static float coefficient = 0.8; 

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

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

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

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

float Var_targetPosition = 0.0;
float Var_Kp = 0.0;
float Var_Ki = 0.0;
float Var_Kd = 0.0;

float set_point_distance = 2.00; 
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 * 100;
              if(distance >= 400)
                distance = 400;
              printf("Distance: %0.04f mm\n", distance);
        }
            

        vTaskDelay(pdMS_TO_TICKS(500));
    }
}


void vtask_pot(void * pvParameter)
{
	
	for(;;)
	{
		targetPosition = set_point_distance;    //((float)analogRead(34) * 360.0/4095.0);
		Var_Kp = 0.5;                           //((float)analogRead(35) * 1.0/4095.0);
		Var_Ki = 0.1;                      //((float)analogRead(32) * 1.0/4095.0);
		Var_Kd = 0.01;                          //((float)analogRead(33) * 1.0/4095.0);

    /*Serial.println("pot1 - " + String(Var_targetPosition));
    Serial.println("pot2 - " + String(Var_Kp));
    Serial.println("pot3 - " + String(Var_Ki));
    Serial.println("pot4 - " + String(Var_Kd));
    Serial.println("---------------------------------------");*/

		delay(dt * 1000); 
	}
}

/*
    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(distance);
        float error = targetPosition - currentPosition;
		
        
		/**
        * Calcula os componentes do controlador PID;
        */
        float proportional = Var_Kp * error;
        integral += Var_Ki * error * dt;
        float derivative = Var_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, erro = %.2f\n", 
              currentPosition, output, previousError);
		
        delay(dt * 1000); //dt tempo de varredura
    }

}

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

  pinMode(34, INPUT_PULLUP); 
  pinMode(35, INPUT_PULLUP);
  pinMode(32, INPUT_PULLUP);
  pinMode(33, INPUT_PULLUP);

	if(xTaskCreate(vtask_motor, "task_motor", 1024 * 5, NULL, 2, NULL) != pdPASS) {
    Serial.println("error - Nao foi possivel alocar task_motor.\r\n" );  
    return;   
  }
  if(xTaskCreate(vtask_pot, "task_pot", 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);
}