/**
 * C library
 */
#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>

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

// definições de potenciômetros de KP/KI/KD
#define potKp 34
#define potKi 35
#define potKd 32
#define potServo 33

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

/**
 * Config. PID;
 */
const float dt = 0.02; // intervalo em ms
float Kp = 0;
float Ki = 0;
float Kd = 0;

/**
 * Config. coeficiente do filtro exponencial (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 = 0; // inicializa result na primeira chamada
    result = coefficient * sample + (1 - coefficient) * result;
    return result;
}
float filterkd(float sample)
{
    static float result = 0; // inicializa result na primeira chamada
    result = coefficient * sample + (1 - coefficient) * result;
    return result;
}
float filterkp(float sample)
{
    static float result = 0; // inicializa result na primeira chamada
    result = coefficient * sample + (1 - coefficient) * result;
    return result;
}
float filterki(float sample)
{
    static float result = 0; // inicializa result na primeira chamada
    result = coefficient * sample + (1 - coefficient) * result;
    return result;
}
float filterservo(float sample)
{
    static float result = 0; // inicializa result na primeira chamada
    result = coefficient * sample + (1 - coefficient) * result;
    return result;
}
/*
    task de controle;
*/
void vtask_motor(void *pvParameter)
{
    set_filter_coefficient(coefficient);

    /**
    * Inicializa servo motor;
    * Posiciona servo motor na posição central;
    */
    myservo.attach(servoPin);
    myservo.write(0);

    

    for (;;)
    {
        int valorPotKp = 0;
        int valorPotKi = 0;
        int valorPotKd = 0;
        int valorPotServo = 0;

        valorPotKp =filterkp (analogRead(potKp));
        valorPotKi =filterki (analogRead(potKi));
        valorPotKd = filterkd (analogRead(potKd));
        valorPotServo =filterservo (analogRead(potServo));

        Kp = valorPotKp / 4096.0; // Divisão por 4096.0 para garantir a divisão de float
        Ki = valorPotKi / 4096.0; // Divisão por 4096.0 para garantir a divisão de float
        Kd = valorPotKd / 4096.0; // Divisão por 4096.0 para garantir a divisão de float

       /* Serial.print("Proporcional: ");
        Serial.println(Kp);
        */
        delay(100);
       /* Serial.print("Integral: ");
        Serial.println(Ki);
        */
        delay(100);
       /* Serial.print("Derivativo: ");
        Serial.println(Kd);
*/
        targetPosition = valorPotServo;
        targetPosition = targetPosition * 360 / 4096;
       /* Serial.print("SERVO: ");
        Serial.println(targetPosition);
*/
        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.print("currentPosition: ");
        Serial.println(currentPosition);
        Serial.print("output: ");
        Serial.println(output);
        Serial.printf("Position = %.2f, Output = %.2f\n",
         currentPosition, output);
        delay(dt * 1000);
    }
}

void setup(void)
{

    pinMode(potKp, INPUT);
    pinMode(potKd, INPUT);
    pinMode(potKi, INPUT);
    pinMode(potServo, INPUT);

    Serial.begin(115200);

    if (xTaskCreate(vtask_motor, "task_motor", 1024 * 5, NULL, 2, NULL) != pdPASS)
    {
        Serial.println("Error - Não foi possível alocar task_motor.\r\n");
        return;
    }
}

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