#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include "driver/ledc.h"
#include "esp_err.h"
#include "esp_log.h"
#include "esp_rom_sys.h"

#include "evse_pilot.h"
#include "adc121s021_dma.h"
#include "board_config.h"

#define PILOT_PWM_TIMER         LEDC_TIMER_0
#define PILOT_PWM_CHANNEL       LEDC_CHANNEL_0
#define PILOT_PWM_SPEED_MODE    LEDC_LOW_SPEED_MODE
#define PILOT_PWM_DUTY_RES      LEDC_TIMER_10_BIT
#define PILOT_PWM_MAX_DUTY      1023

#define NUM_PILOT_SAMPLES       100
#define MAX_SAMPLE_ATTEMPTS     1000
#define PILOT_EXTREME_PERCENT   10  // 10% superior e inferior

// ADC121S021 setup
#define ADC121_VREF_MV          3300   // AJUSTE conforme Vref do seu hardware!
#define ADC121_MAX              4095   // 12 bits

static const char *TAG = "evse_pilot";

// Memoização de estado para evitar comandos/logs desnecessários
static int last_pilot_level = -1;
static uint32_t last_pwm_duty = 0;

// Função para converter leitura bruta do ADC para mV
static int adc_raw_to_mv(uint16_t raw) {
    return (raw * ADC121_VREF_MV) / ADC121_MAX;
}

void pilot_init(void)
{
    // PWM (LEDC) configuração
    ledc_timer_config_t ledc_timer = {
        .speed_mode = PILOT_PWM_SPEED_MODE,
        .timer_num = PILOT_PWM_TIMER,
        .duty_resolution = PILOT_PWM_DUTY_RES,
        .freq_hz = 1000,
        .clk_cfg = LEDC_AUTO_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

    ledc_channel_config_t ledc_channel = {
        .speed_mode = PILOT_PWM_SPEED_MODE,
        .channel = PILOT_PWM_CHANNEL,
        .timer_sel = PILOT_PWM_TIMER,
        .intr_type = LEDC_INTR_DISABLE,
        .gpio_num = board_config.pilot_pwm_gpio,
        .duty = 0,
        .hpoint = 0
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
    ESP_ERROR_CHECK(ledc_stop(PILOT_PWM_SPEED_MODE, PILOT_PWM_CHANNEL, 0));
    //ESP_ERROR_CHECK(ledc_fade_func_install(0));

    // Inicializa ADC121S021 externo
    adc121s021_dma_init();
}

void pilot_set_level(bool level)
{
    if (last_pilot_level == level) return; // só muda se necessário
    last_pilot_level = level;

    ESP_LOGI(TAG, "Set level %d", level);
    ledc_stop(PILOT_PWM_SPEED_MODE, PILOT_PWM_CHANNEL, level ? 1 : 0);
    last_pwm_duty = 0; // PWM parado
}

void pilot_set_amps(uint16_t amps)
{
    if (amps < 6 || amps > 80) {
        ESP_LOGE(TAG, "Invalid ampere value: %d A (valid: 6–80 A)", amps);
        return;
    }

    uint32_t duty_percent;

    if (amps <= 51) {
        duty_percent = (amps * 10) / 6;  // Duty (%) = Amps / 0.6
    } else {
        duty_percent = (amps * 10) / 25 + 64;  // Duty (%) = (Amps / 2.5) + 64
    }

    if (duty_percent > 100) duty_percent = 100;

    uint32_t duty = (PILOT_PWM_MAX_DUTY * duty_percent) / 100;

    if (last_pilot_level == 0 && last_pwm_duty == duty) return;
    last_pilot_level = 0;
    last_pwm_duty = duty;

    ESP_LOGI(TAG, "Pilot set: %d A → %d/%d (≈ %d%% duty)",
         amps, (int)duty, PILOT_PWM_MAX_DUTY, (int)duty_percent);

    ledc_set_duty(PILOT_PWM_SPEED_MODE, PILOT_PWM_CHANNEL, duty);
    ledc_update_duty(PILOT_PWM_SPEED_MODE, PILOT_PWM_CHANNEL);
}

bool pilot_get_state(void) {
    // true se estamos em 12V fixo; false se PWM ou -12V
    // Se quiser diferenciar PWM, guarde um flag quando chamar set_amps.
    return (last_pilot_level == 1) && (last_pwm_duty == 0);
}



static int compare_int(const void *a, const void *b) {
    return (*(int *)a - *(int *)b);
}

static int select_low_median_qsort(int *src, int n, int percent) {
    int k = (n * percent) / 100;
    if (k == 0) k = 1;
    int *copy = alloca(n * sizeof(int));
    memcpy(copy, src, n * sizeof(int));
    qsort(copy, n, sizeof(int), compare_int);
    return copy[k / 2];
}

static int select_high_median_qsort(int *src, int n, int percent) {
    int k = (n * percent) / 100;
    if (k == 0) k = 1;
    int *copy = alloca(n * sizeof(int));
    memcpy(copy, src, n * sizeof(int));
    qsort(copy, n, sizeof(int), compare_int);
    return copy[n - k + (k / 2)];
}

void pilot_measure(pilot_voltage_t *up_voltage, bool *down_voltage_n12)
{
    ESP_LOGD(TAG, "pilot_measure");

    int samples[NUM_PILOT_SAMPLES];
    int collected = 0, attempts = 0;
    uint16_t adc_sample = 0;

    // Lê samples usando ADC121S021 externo
    while (collected < NUM_PILOT_SAMPLES && attempts < MAX_SAMPLE_ATTEMPTS) {
        adc_sample = 0;
        if (adc121s021_dma_get_sample(&adc_sample)) {
            samples[collected++] = adc_sample;
            esp_rom_delay_us(10);
        } else {
            esp_rom_delay_us(100);
            attempts++;
        }
    }

    if (collected < NUM_PILOT_SAMPLES) {
        ESP_LOGW(TAG, "Timeout on sample read (%d/%d)", collected, NUM_PILOT_SAMPLES);
        *up_voltage = PILOT_VOLTAGE_1;
        *down_voltage_n12 = false;
        return;
    }

    int high_raw = select_high_median_qsort(samples, collected, PILOT_EXTREME_PERCENT);
    int low_raw  = select_low_median_qsort(samples, collected, PILOT_EXTREME_PERCENT);

    int high_mv = adc_raw_to_mv(high_raw);
    int low_mv  = adc_raw_to_mv(low_raw);

    // Aplica thresholds definidos em board_config (em mV)
    if (high_mv >= board_config.pilot_down_threshold_12)
        *up_voltage = PILOT_VOLTAGE_12;
    else if (high_mv >= board_config.pilot_down_threshold_9)
        *up_voltage = PILOT_VOLTAGE_9;
    else if (high_mv >= board_config.pilot_down_threshold_6)
        *up_voltage = PILOT_VOLTAGE_6;
    else if (high_mv >= board_config.pilot_down_threshold_3)
        *up_voltage = PILOT_VOLTAGE_3;
    else
        *up_voltage = PILOT_VOLTAGE_1;

    *down_voltage_n12 = (low_mv <= board_config.pilot_down_threshold_n12);

    ESP_LOGD(TAG, "Final: up_voltage=%d, down_voltage_n12=%d", *up_voltage, *down_voltage_n12);
}