#include <zephyr/kernel.h>
#include <zephyr/settings/settings.h>
#include <zephyr/logging/log.h>
#include <zephyr/device.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/drivers/adc.h>
#include <lib/valve.h>

LOG_MODULE_REGISTER(valve, LOG_LEVEL_DBG);

// ADC configuration for MULTISENSE (PA0)
static const struct device *adc_dev = DEVICE_DT_GET(DT_NODELABEL(adc1));
static const struct adc_channel_cfg adc_channel_cfg = {
    .gain = ADC_GAIN_1,
    .reference = ADC_REF_INTERNAL,  // STM32 only supports internal ref (1.2V)
    .acquisition_time = ADC_ACQ_TIME_DEFAULT,  // Use default acquisition time
    .channel_id = 1, // ADC1_IN1 (PA0)
    .differential = 0,
};

static const struct valve_gpios valve_gpios = {
    .in0 = GPIO_DT_SPEC_GET(DT_NODELABEL(vnd7050aj), in0_gpios),
    .in1 = GPIO_DT_SPEC_GET(DT_NODELABEL(vnd7050aj), in1_gpios),
    .rst = GPIO_DT_SPEC_GET(DT_NODELABEL(vnd7050aj), rst_gpios),
    .sen = GPIO_DT_SPEC_GET(DT_NODELABEL(vnd7050aj), sen_gpios),
    .s0 = GPIO_DT_SPEC_GET(DT_NODELABEL(vnd7050aj), s0_gpios),
    .s1 = GPIO_DT_SPEC_GET(DT_NODELABEL(vnd7050aj), s1_gpios),
};

static enum valve_state current_state = VALVE_STATE_CLOSED;
static enum valve_movement current_movement = VALVE_MOVEMENT_IDLE;
static uint16_t max_opening_time_s = 60;
static uint16_t max_closing_time_s = 60;
static struct k_work_delayable valve_work;

static void valve_work_handler(struct k_work *work)
{
    gpio_pin_set_dt(&valve_gpios.in0, 0);
    gpio_pin_set_dt(&valve_gpios.in1, 0);
    gpio_pin_set_dt(&valve_gpios.rst, 0);

    if (current_movement == VALVE_MOVEMENT_OPENING) {
        LOG_INF("Valve finished opening");
    } else if (current_movement == VALVE_MOVEMENT_CLOSING) {
        current_state = VALVE_STATE_CLOSED;
        LOG_INF("Valve finished closing");
    }
    current_movement = VALVE_MOVEMENT_IDLE;
}

void valve_init(void)
{
    k_work_init_delayable(&valve_work, valve_work_handler);
    settings_load_one("valve/max_open_time", &max_opening_time_s, sizeof(max_opening_time_s));
    settings_load_one("valve/max_close_time", &max_closing_time_s, sizeof(max_closing_time_s));

    // Initialize ADC for MULTISENSE
    if (!device_is_ready(adc_dev)) {
        LOG_ERR("ADC device not ready");
        return;
    }

    int ret = adc_channel_setup(adc_dev, &adc_channel_cfg);
    if (ret < 0) {
        LOG_ERR("Could not setup ADC channel (%d)", ret);
        return;
    }

    gpio_pin_configure_dt(&valve_gpios.in0, GPIO_OUTPUT_INACTIVE); // IN0 control pin - output, deactivate
    gpio_pin_configure_dt(&valve_gpios.in1, GPIO_OUTPUT_INACTIVE); // IN1 control pin - output, deactivate
    gpio_pin_configure_dt(&valve_gpios.rst, GPIO_OUTPUT_INACTIVE);  // Keep VND7050AJ in reset
    gpio_pin_configure_dt(&valve_gpios.sen, GPIO_OUTPUT_INACTIVE);  // Sensor enable pin - output, inactive
    // S0 and S1 pins are used for selecting the valve state, they are initially inactive
    // and will be set to active when the valve is opened or closed.
    gpio_pin_configure_dt(&valve_gpios.s0, GPIO_OUTPUT_INACTIVE);  // S0 select pin - output
    gpio_pin_configure_dt(&valve_gpios.s1, GPIO_OUTPUT_INACTIVE);  // S1 select pin - output
    
    LOG_INF("Valve initialized: max_open=%us, max_close=%us", max_opening_time_s, max_closing_time_s);
}

void valve_open(void)
{
    if (current_state == VALVE_STATE_CLOSED) {
        gpio_pin_set_dt(&valve_gpios.rst, 1);
        gpio_pin_set_dt(&valve_gpios.in1, 0);
        gpio_pin_set_dt(&valve_gpios.in0, 1);
        current_state = VALVE_STATE_OPEN;
        current_movement = VALVE_MOVEMENT_OPENING;
        k_work_schedule(&valve_work, K_MSEC(max_opening_time_s * 1000 * 0.9));
    }
}

void valve_close(void)
{
    if (current_state == VALVE_STATE_OPEN) {
        gpio_pin_set_dt(&valve_gpios.rst, 1);
        gpio_pin_set_dt(&valve_gpios.in0, 0);
        gpio_pin_set_dt(&valve_gpios.in1, 1);
        current_movement = VALVE_MOVEMENT_CLOSING;
        k_work_schedule(&valve_work, K_MSEC(max_closing_time_s * 1000 * 0.9));
    }
}

void valve_stop(void)
{
    k_work_cancel_delayable(&valve_work);
    current_movement = VALVE_MOVEMENT_IDLE;
}

enum valve_state valve_get_state(void) { return current_state; }
enum valve_movement valve_get_movement(void) { return current_movement; }
uint16_t valve_get_motor_current(void) { return (current_movement != VALVE_MOVEMENT_IDLE) ? 150 : 10; }

uint16_t valve_get_supply_voltage(void)
{
    LOG_INF("=== ADC TEST MODE - PA0 LAB SUPPLY TEST ===");
    LOG_INF("Connect lab supply to PA0. Recommended: 1.0V");
    LOG_INF("Expected raw value for 1.0V: ~2007 (using 2.048V VREFBUF)");
    LOG_INF("ADC range: 0-2.048V (STM32G431 VREFBUF internal reference)");
    LOG_INF("");
    
    // No VND7050AJ configuration - pure ADC test
    // Just make sure pins are in safe state
    gpio_pin_configure_dt(&valve_gpios.rst, GPIO_OUTPUT);
    gpio_pin_configure_dt(&valve_gpios.sen, GPIO_OUTPUT); 
    gpio_pin_configure_dt(&valve_gpios.s0, GPIO_OUTPUT);
    gpio_pin_configure_dt(&valve_gpios.s1, GPIO_OUTPUT);
    gpio_pin_configure_dt(&valve_gpios.in0, GPIO_OUTPUT);
    gpio_pin_configure_dt(&valve_gpios.in1, GPIO_OUTPUT);
    
    // Set all VND7050AJ pins LOW for safety
    gpio_pin_set_dt(&valve_gpios.rst, 0);
    gpio_pin_set_dt(&valve_gpios.s0, 0);
    gpio_pin_set_dt(&valve_gpios.s1, 0);
    gpio_pin_set_dt(&valve_gpios.sen, 0);
    gpio_pin_set_dt(&valve_gpios.in0, 0);
    gpio_pin_set_dt(&valve_gpios.in1, 0);
    
    LOG_INF("VND7050AJ disabled - all pins LOW");
    LOG_INF("PA0 is now isolated for lab supply testing");
    k_msleep(100);
    
    // Setup simple ADC sequence
    int16_t buf;
    struct adc_sequence sequence = {
        .buffer = &buf,
        .buffer_size = sizeof(buf),
        .channels = BIT(adc_channel_cfg.channel_id),
        .resolution = 12,
    };
    
    LOG_INF("Starting continuous ADC readings every 500ms...");
    
    // Continuous monitoring loop with improved stability
    int reading_count = 0;
    int32_t samples[10];  // Buffer for averaging
    
    while (1) {
        // Take multiple samples and average them for stability
        int valid_samples = 0;
        int32_t sum = 0;
        
        for (int i = 0; i < 10; i++) {
            k_msleep(50);  // Longer delay between samples for stability
            int adc_ret = adc_read(adc_dev, &sequence);
            
            if (adc_ret == 0 && buf > 100) {  // Filter out near-zero readings (floating input)
                samples[i] = buf;
                sum += buf;
                valid_samples++;
            } else {
                LOG_WRN("Sample %d invalid: raw=%d, ret=%d", i, buf, adc_ret);
                samples[i] = 0;  // Mark as invalid
            }
        }
        
        if (valid_samples > 0) {
            // Calculate average
            int32_t avg_raw = sum / valid_samples;
            
            // Calculate voltage using the correct VREFBUF reference (2.048V)
            int32_t pa0_mv = (avg_raw * 2048) / 4096;  // Using 2.048V VREFBUF
            
            // Calculate standard deviation to show stability
            int32_t variance = 0;
            for (int i = 0; i < valid_samples; i++) {
                int32_t diff = samples[i] - avg_raw;
                variance += diff * diff;
            }
            int32_t std_dev = (valid_samples > 1) ? variance / (valid_samples - 1) : 0;
            
            // Find min/max for this sample set
            int32_t min_raw = samples[0], max_raw = samples[0];
            for (int i = 1; i < valid_samples; i++) {
                if (samples[i] < min_raw) min_raw = samples[i];
                if (samples[i] > max_raw) max_raw = samples[i];
            }
            
            LOG_INF("Reading %d: avg_raw=%d (%dmV) | range=%d-%d | std_dev=%d | samples=%d/10", 
                    reading_count, (int)avg_raw, (int)pa0_mv,
                    (int)min_raw, (int)max_raw, (int)std_dev, valid_samples);
        } else {
            LOG_ERR("Reading %d: All ADC samples failed", reading_count);
        }
        
        reading_count++;
        k_msleep(400);  // Wait before next reading set
    }
    
    return 0;  // Never reached
}

void valve_set_max_open_time(uint16_t seconds) { max_opening_time_s = seconds; settings_save_one("valve/max_open_time", &max_opening_time_s, sizeof(max_opening_time_s)); }
void valve_set_max_close_time(uint16_t seconds) { max_closing_time_s = seconds; settings_save_one("valve/max_close_time", &max_closing_time_s, sizeof(max_closing_time_s)); }
uint16_t valve_get_max_open_time(void) { return max_opening_time_s; }
uint16_t valve_get_max_close_time(void) { return max_closing_time_s; }