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

/**
 * Lcd lib;
 */
#include <sys/time.h>
#include "pcf8574.h"
#include "hd44780.h"
#include "i2cdev.h"

/**
 * Config do display lcd;
 */
#define SDA_GPIO (gpio_num_t)21
#define SCL_GPIO (gpio_num_t)22
#define I2C_ADDR 0x27

static i2c_dev_t pcf8574;

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

/**
 * Config. do servo motor;
 */
static Servo myservo;
const int servoPin = 18;   
/**
 * Define a posição do servo motor;
 */
int targetPosition = 30;
float currentPosition;
float output; 

/**
 * Config. PID;
 * 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.
*/ 
const float dt = 0.02; //intevalo em 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.
*/
float Kp;

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

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

/**
 * Definição dos pinos p/ pot. de ajuste do PID
 */
const int ajusteKp = 32;
const int ajusteKi = 33;
const int ajusteKd = 34;
const int ajusteSetpoint = 35;

/**
 * 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_sp( float sample )
{
    static float result; 
    result = coefficient*sample + (1-coefficient)*result;
    return result;
}
float filter_kp( float sample )
{
    static float result; 
    result = coefficient*sample + (1-coefficient)*result;
    return result;
}
float filter_ki( float sample )
{
    static float result; 
    result = coefficient*sample + (1-coefficient)*result;
    return result;
}
float filter_kd( float sample )
{
    static float result; 
    result = coefficient*sample + (1-coefficient)*result;
    return result;
}
float filter_pv( float sample )
{
    static float result; 
    result = coefficient*sample + (1-coefficient)*result;
    return result;
}

/*
 * Task de coleta de dados dos potênciometros
 */
void vTask_adjust(void * pvParameter)
{
  char sv_sp[20];
  char pid_out[20];
  char pid_k[20];

  for(;;)
  {

    targetPosition = analogRead(ajusteSetpoint);
    targetPosition = targetPosition * 180 /4095;
    targetPosition = filter_sp(targetPosition);
    sprintf(sv_sp, "SV: %d  PV: %.2f", targetPosition, currentPosition);

    /*
     * Set Value x Present Value
     */
    hd44780_gotoxy(&lcd, 0, 1);
    hd44780_puts(&lcd, sv_sp);

    /*
     * PID Output
     */
    sprintf(pid_out, "PID Output: %.2f", output);
    hd44780_gotoxy(&lcd, 0, 2);
    hd44780_puts(&lcd, pid_out);   
    

    Kp = analogRead(ajusteKp);
    Kp /= 4095.;
    Kp = filter_kp(Kp);

    Ki = analogRead(ajusteKi);
    Ki /= 4095.;
    Ki = filter_ki(Ki);

    Kd = analogRead(ajusteKd);
    Kd /= 4095.;
    Kd = filter_kd(Kd);

    /*
     * Ganhos
     */
    sprintf(pid_k, "KP:%.1f KI:%.1f KD:%.1f", Kp, Ki, Kd);
    hd44780_gotoxy(&lcd, 0, 3);
    hd44780_puts(&lcd, pid_k);

    Serial.printf("SP: %d KP: %.2f KI: %.2f KD: %.2f\n",
                  targetPosition, Kp, Ki, Kd);
    delay(100);
  }
}

/*
    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(;;) 
    {

      currentPosition = filter_pv(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;
       */
      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); 
    }

}

void setup(void) 
{
  Serial.begin(115200);
  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 */
  char msg[20] = {"Controle PID"};
  hd44780_init(&lcd);
  hd44780_clear(&lcd);
  hd44780_gotoxy(&lcd, 3, 0);
  hd44780_puts(&lcd, msg);

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

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