#include <stdio.h>
#include <string.h> 

#include <sys/time.h>
#include "pcf8574.h"
#include "hd44780.h"
#include "i2cdev.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)

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

static i2c_dev_t pcf8574;
static float distance = 0; 

char MENSAGEM[] = "SEJAM BEM VINDOS";

/*
*	Parâmetros do controle PID 
*/
float targetPosition = 50.0; // Valor de referência (de 0 a 100%)
const float Kp = 0.1; // Ganho proporcional
const float Ki = 0.01; // Ganho integral
const float Kd = 0.001; // Ganho derivativo
const float dt = 0.02; //intevalo em s   ((0.02*1000) = 2 ms)

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

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

/*
*	Pinos usados no LCD
*/
hd44780_t lcd = {
    .write_cb = write_lcd_data,
    .pins = {
        .rs = 0,
        .e  = 2,
        .d4 = 4,
        .d5 = 5,
        .d6 = 6,
        .d7 = 7,
        .bl = 3
    },
    .font = HD44780_FONT_5X8,
    .lines = 4,
    .backlight = 1
};

/*
*	O filtro exponencial opera aplicando uma média ponderada exponencial
*/
float filter( float sample )
{
    static float result; 
    result = coefficient*sample + (1-coefficient)*result;
    return result;
}

void set_filter_coefficient ( float newCoefficient )
{
    if (( newCoefficient >= 0  ) && ( newCoefficient <= 1  )){
      coefficient = newCoefficient;
    }      
}


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

}

/*
    task lcd
*/
void task_lcd(void * pvParameter) 
{
  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));
  hd44780_init(&lcd);
  hd44780_gotoxy(&lcd, 0, 0);
  

  for(;;)
  {
      hd44780_puts(&lcd, MENSAGEM);
    vTaskDelay(pdMS_TO_TICKS(100));
  }

}

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

	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) {
	/* Simulação da leitura do sensor de nível (substitua pelo seu código real)
    valor_atual = analogRead(18) * 100.0 / 400.0; // Pino D18 (ECHO)
    // Leitura dos potenciômetros (substitua pelos seus códigos reais)
    double pot_referencia = analogRead(33) / 1023.0 * 100.0; // Pino D33
    double pot_valvula = analogRead(32) / 1023.0 * 100.0; // Pino D32
    // Atualiza a saída do controle PID
    double saida_pid = calcular_saida_pid(valor_atual);
    char linha2[17];
    snprintf(linha2, sizeof(linha2), "Ref: %.1f%%", setpoint_referencia);
    hd44780_gotoxy(&lcd, 0, 1);
    hd44780_puts(&lcd, linha2);
    char linha3[17];
    snprintf(linha3, sizeof(linha3), "PID: %.1f%%", saida_pid);
    hd44780_gotoxy(&lcd, 0, 2);
    hd44780_puts(&lcd, linha3);
    char linha4[17];
    snprintf(linha4, sizeof(linha4), "Nível: %.1f%%", valor_atual);
    hd44780_gotoxy(&lcd, 0, 3);
    hd44780_puts(&lcd, linha4);
    delay(100);*/

}