/* JSN-SR04T Ultrasonic Distance Sensor Driver Implementation */

#include "jsn_sr04_driver.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "rom/ets_sys.h"

static const char *TAG = "JSN_SR04";

/* Configuração do sensor */
static jsn_sr04_config_t sensor_config = {0};
static bool initialized = false;

/* Constantes físicas */
#define SPEED_OF_SOUND_CM_US    0.0343  /* Velocidade do som: 343 m/s = 0.0343 cm/µs */
#define TRIGGER_PULSE_US        10      /* Duração do pulso trigger em µs */
#define MIN_WAIT_BETWEEN_MS     60      /* Tempo mínimo entre leituras em ms */

/* Timeout padrão baseado na distância máxima */
#define DEFAULT_MAX_DISTANCE_CM 400
#define CALC_TIMEOUT_US(dist_cm) ((dist_cm * 2 * 100) / 343 + 1000)

static int64_t last_read_time = -MIN_WAIT_BETWEEN_MS * 1000;

/**
 * @brief Aguarda mudança de nível no pino ECHO
 * 
 * @param level Nível esperado (0 ou 1)
 * @param timeout_us Timeout em microsegundos
 * @return Tempo decorrido em µs, ou -1 se timeout
 */
static int64_t wait_for_echo_level(int level, uint32_t timeout_us)
{
    int64_t start = esp_timer_get_time();
    
    while (gpio_get_level(sensor_config.echo_gpio) != level) {
        if ((esp_timer_get_time() - start) > timeout_us) {
            return -1;
        }
        ets_delay_us(1);
    }
    
    return esp_timer_get_time() - start;
}

/**
 * @brief Envia pulso de trigger
 */
static void send_trigger_pulse(void)
{
    /* Garante que trigger está LOW */
    gpio_set_level(sensor_config.trigger_gpio, 0);
    ets_delay_us(2);
    
    /* Pulso HIGH de 10µs */
    gpio_set_level(sensor_config.trigger_gpio, 1);
    ets_delay_us(TRIGGER_PULSE_US);
    gpio_set_level(sensor_config.trigger_gpio, 0);
}

esp_err_t jsn_sr04_init(const jsn_sr04_config_t *config)
{
    if (config == NULL) {
        ESP_LOGE(TAG, "Config is NULL");
        return ESP_ERR_INVALID_ARG;
    }
    
    if (initialized) {
        ESP_LOGW(TAG, "Already initialized, reinitializing...");
        jsn_sr04_deinit();
    }
    
    /* Salva configuração */
    sensor_config = *config;
    
    /* Define timeout padrão se não especificado */
    if (sensor_config.max_distance_cm == 0) {
        sensor_config.max_distance_cm = DEFAULT_MAX_DISTANCE_CM;
    }
    
    if (sensor_config.timeout_us == 0) {
        sensor_config.timeout_us = CALC_TIMEOUT_US(sensor_config.max_distance_cm);
    }
    
    /* Configura GPIO TRIGGER como saída */
    gpio_config_t trigger_conf = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_OUTPUT,
        .pin_bit_mask = (1ULL << sensor_config.trigger_gpio),
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .pull_up_en = GPIO_PULLUP_DISABLE,
    };
    
    esp_err_t ret = gpio_config(&trigger_conf);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to configure TRIGGER GPIO %d", sensor_config.trigger_gpio);
        return ret;
    }
    
    /* Configura GPIO ECHO como entrada */
    gpio_config_t echo_conf = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_INPUT,
        .pin_bit_mask = (1ULL << sensor_config.echo_gpio),
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .pull_up_en = GPIO_PULLUP_DISABLE,
    };
    
    ret = gpio_config(&echo_conf);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to configure ECHO GPIO %d", sensor_config.echo_gpio);
        return ret;
    }
    
    /* Inicializa trigger em LOW */
    gpio_set_level(sensor_config.trigger_gpio, 0);
    
    initialized = true;
    
    ESP_LOGI(TAG, "JSN-SR04T initialized - TRIGGER: GPIO%d, ECHO: GPIO%d, Max: %d cm",
             sensor_config.trigger_gpio, sensor_config.echo_gpio, 
             sensor_config.max_distance_cm);
    
    /* Aguarda sensor estabilizar */
    vTaskDelay(pdMS_TO_TICKS(100));
    
    return ESP_OK;
}

esp_err_t jsn_sr04_read(jsn_sr04_reading_t *reading)
{
    if (!initialized) {
        ESP_LOGE(TAG, "Sensor not initialized");
        return ESP_ERR_INVALID_STATE;
    }
    
    if (reading == NULL) {
        ESP_LOGE(TAG, "Reading pointer is NULL");
        return ESP_ERR_INVALID_ARG;
    }
    
    /* Inicializa leitura como inválida */
    reading->valid = false;
    reading->distance_cm = 0.0f;
    reading->echo_time_us = 0;
    
    /* Verifica intervalo mínimo entre leituras */
    int64_t current_time = esp_timer_get_time();
    int64_t time_since_last = current_time - last_read_time;
    
    if (time_since_last < (MIN_WAIT_BETWEEN_MS * 1000)) {
        int64_t wait_us = (MIN_WAIT_BETWEEN_MS * 1000) - time_since_last;
        ESP_LOGD(TAG, "Waiting %lld us before next reading", wait_us);
        ets_delay_us((uint32_t)wait_us);
    }
    
    /* Desabilita interrupções para timing preciso */
    portDISABLE_INTERRUPTS();
    
    /* Envia pulso de trigger */
    send_trigger_pulse();
    
    /* Aguarda ECHO ir para HIGH (início do pulso) */
    int64_t wait_high = wait_for_echo_level(1, sensor_config.timeout_us);
    if (wait_high < 0) {
        portENABLE_INTERRUPTS();
        ESP_LOGW(TAG, "Timeout waiting for ECHO HIGH");
        return ESP_ERR_TIMEOUT;
    }
    
    /* Mede duração do pulso HIGH */
    int64_t start_time = esp_timer_get_time();
    int64_t pulse_duration = wait_for_echo_level(0, sensor_config.timeout_us);
    
    portENABLE_INTERRUPTS();
    
    if (pulse_duration < 0) {
        ESP_LOGW(TAG, "Timeout waiting for ECHO LOW");
        return ESP_ERR_TIMEOUT;
    }
    
    /* Atualiza timestamp da última leitura */
    last_read_time = esp_timer_get_time();
    
    /* Calcula distância
     * Fórmula: distância = (tempo_echo * velocidade_som) / 2
     * Dividido por 2 porque o som vai e volta
     */
    reading->echo_time_us = (uint32_t)pulse_duration;
    reading->distance_cm = (pulse_duration * SPEED_OF_SOUND_CM_US) / 2.0f;
    
    /* Valida leitura */
    if (reading->distance_cm < 2.0f || reading->distance_cm > sensor_config.max_distance_cm) {
        ESP_LOGD(TAG, "Distance out of range: %.2f cm", reading->distance_cm);
        reading->valid = false;
        return ESP_ERR_INVALID_RESPONSE;
    }
    
    reading->valid = true;
    
    ESP_LOGI(TAG, "Distance: %.2f cm (echo: %u µs)", 
             reading->distance_cm, reading->echo_time_us);
    
    return ESP_OK;
}

esp_err_t jsn_sr04_get_distance(float *distance_cm)
{
    if (distance_cm == NULL) {
        return ESP_ERR_INVALID_ARG;
    }
    
    jsn_sr04_reading_t reading;
    esp_err_t ret = jsn_sr04_read(&reading);
    
    if (ret == ESP_OK && reading.valid) {
        *distance_cm = reading.distance_cm;
        return ESP_OK;
    }
    
    return ret;
}

esp_err_t jsn_sr04_deinit(void)
{
    if (!initialized) {
        return ESP_OK;
    }
    
    /* Reseta GPIOs */
    gpio_reset_pin(sensor_config.trigger_gpio);
    gpio_reset_pin(sensor_config.echo_gpio);
    
    initialized = false;
    
    ESP_LOGI(TAG, "JSN-SR04T deinitialized");
    
    return ESP_OK;
}