#include "ClientSensor.h"
#include "secrets.h"

#include "nvs_flash.h"
#include "nvs.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_netif.h"
#include "esp_log.h"

#include <inttypes.h>
#include <esp_now.h>
#include <esp_sleep.h>
#include <sys/time.h>

volatile bool rtc_synced_with_server = false;
volatile bool response_received      = false;
volatile bool send_process_complete  = false;
volatile bool send_response_process_complete = false;

uint8_t config_request_attempts = 0;


// ----------------------------------------------------------------------------
// Helper
// ----------------------------------------------------------------------------
void printMac(const char *TAG, const uint8_t* mac) {
    if (!mac) return;
    ESP_LOGI(TAG, "%02X:%02X:%02X:%02X:%02X:%02X",
             mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
}

// ----------------------------------------------------------------------------
// WiFi handling
// ----------------------------------------------------------------------------
int32_t get_WiFiChannel() {
    wifi_scan_config_t scan_config = {};
    ESP_ERROR_CHECK(esp_wifi_scan_start(&scan_config, true)); // blocking scan

    uint16_t ap_count = 0;
    ESP_ERROR_CHECK(esp_wifi_scan_get_ap_num(&ap_count));
    if (ap_count == 0) return 0;

    wifi_ap_record_t *ap_list = (wifi_ap_record_t*)malloc(sizeof(wifi_ap_record_t) * ap_count);
    if (!ap_list) return 0;

    ESP_ERROR_CHECK(esp_wifi_scan_get_ap_records(&ap_count, ap_list));

    int32_t channel = 0;
    for (int i = 0; i < ap_count; i++) {
        if (strcmp((char*)ap_list[i].ssid, WIFI_SSID) == 0) {
            channel = ap_list[i].primary;
            break;
        }
    }

    free(ap_list);
    return channel;
}

void init_WiFi() {
    // Initialize NVS
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    // Initialize TCP/IP stack and default event loop
    ESP_ERROR_CHECK(esp_netif_init());
    ESP_ERROR_CHECK(esp_event_loop_create_default());

    // Initialize Wi-Fi driver
    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));
    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
    ESP_ERROR_CHECK(esp_wifi_start());

    // Scan and set channel
    int32_t channel = get_WiFiChannel();
    if (channel > 0) {
        esp_wifi_set_promiscuous(true);
        ESP_ERROR_CHECK(esp_wifi_set_channel(channel, WIFI_SECOND_CHAN_NONE));
        esp_wifi_set_promiscuous(false);
    }
}


// ----------------------------------------------------------------------------
// Peers
// ----------------------------------------------------------------------------

void print_SensorConfig(const SensorConfig_s& cfg) {
    static const char *TAG = "print_SensorConfig";

    ESP_LOGI(TAG, "SensorConfig for '%s'", cfg.sensor_name);
    ESP_LOGI(TAG, "  is_active: %s", cfg.is_active ? "true" : "false");
    ESP_LOGI(TAG, "  pin: %d", cfg.pin);
    ESP_LOGI(TAG, "  min_moisture: %d", cfg.min_moisture);
    ESP_LOGI(TAG, "  notification_name: %s", cfg.notification_name);
    ESP_LOGI(TAG, "  pumping_delay_sec: %d", cfg.pumping_delay_sec);
    ESP_LOGI(TAG, "  pumping_duration: %d", cfg.pumping_duration);
    ESP_LOGI(TAG, "  pump_mac: "); printMac(TAG, cfg.pump_mac);
    ESP_LOGI(TAG, "  delay_ms: %d", cfg.delay_ms);
    ESP_LOGI(TAG, "  use_time: %s", cfg.use_time ? "true" : "false");
    ESP_LOGI(TAG, "  time_count: %u", cfg.time_count);
    ESP_LOGI(TAG, "  last_sensor_read: %lld", cfg.last_sensor_read);
    ESP_LOGI(TAG, "  next_sensor_read: %lld", cfg.next_sensor_read);

    if (cfg.time_count > 0) {
        char buf[128];
        int offset = 0;

        offset += snprintf(buf + offset, sizeof(buf) - offset, "  times: ");
        for (uint8_t i = 0; i < cfg.time_count; i++) {
            offset += snprintf(buf + offset, sizeof(buf) - offset, "%u ", cfg.times[i]);
            if (offset >= sizeof(buf)) break;
        }

        ESP_LOGI(TAG, "%s", buf);
    }
}

// ----------------------------------------------------------------------------
// Config to flash
// ----------------------------------------------------------------------------

bool load_SensorConfigs() {
    static const char *TAG = "load_SensorConfigs";

    ESP_LOGI(TAG, "Loading sensor configs from NVS...");

    nvs_handle_t handle;
    esp_err_t err = nvs_open("sensors", NVS_READONLY, &handle);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to open NVS namespace: %d", err);
        return false;
    }

    size_t sc_size = SENSOR_CONFIG_COUNT * sizeof(SensorConfig_s);
    size_t stored_size = sc_size;
    err = nvs_get_blob(handle, "configs", sensor_configs, &stored_size);
    if (err == ESP_OK && stored_size == sc_size) {
        ESP_LOGI(TAG, "Config read successfully.");
        nvs_close(handle);
        return true;
    } else {
        ESP_LOGE(TAG, "No valid stored config found.");
        nvs_close(handle);
        return false;
    }
}

void save_SensorConfigs() {
    static const char *TAG = "save_SensorConfigs";

    ESP_LOGI(TAG, "Saving sensor configs to NVS...");

    nvs_handle_t handle;
    esp_err_t err = nvs_open("sensors", NVS_READWRITE, &handle);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to open NVS namespace: %d", err);
        return;
    }

    size_t sc_size = SENSOR_CONFIG_COUNT * sizeof(SensorConfig_s);
    err = nvs_set_blob(handle, "configs", sensor_configs, sc_size);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Failed to write blob to NVS: %d", err);
    } else {
        nvs_commit(handle);
        ESP_LOGI(TAG, "Save complete.");
    }

    nvs_close(handle);
}

void restore_SensorConfigs() {
    static const char *TAG = "restore_SensorConfigs";

    ESP_LOGI(TAG, "Trying to load old config!");

    if (load_SensorConfigs()) {
        ESP_LOGI(TAG, "Sensor configs restored from flash.");
    } else {
        ESP_LOGE(TAG, "No stored configs found, using defaults.");
        for (size_t i = 0; i < SENSOR_CONFIG_COUNT; i++) {
            memcpy(sensor_configs[i].pump_mac, PEER_PUMP, 6);
        }
    }
}


// ----------------------------------------------------------------------------
// ESP-NOW handling
// ----------------------------------------------------------------------------

void on_DataSent(const wifi_tx_info_t* info, esp_now_send_status_t status) {
    static const char *TAG = "on_DataSent";

    ESP_LOGI(TAG, "Packet to ");
    printMac(TAG, info->des_addr);

    if (status == ESP_NOW_SEND_SUCCESS)
        ESP_LOGI(TAG, " queued successfully");
    else
        ESP_LOGE(TAG, " failed to queue!");
}

void on_DataRecv(const esp_now_recv_info *info, const uint8_t *data, int len) {
    static const char *TAG = "on_DataRecv";

    if (!info || !data || len <= 0) return;

    // --- Filter by source MAC ---
    const uint8_t *src = info->src_addr;

    if (memcmp(src, PEER_SERVER, 6) != 0) {
        ESP_LOGW(TAG, "Ignoring message: unknown sender");
        ESP_LOGW(TAG, "%02X:%02X:%02X:%02X:%02X:%02X",
                 src[0], src[1], src[2], src[3], src[4], src[5]);
        return;
    }

    // --- Handle message ---
    char msg[256];
    memcpy(msg, data, len);
    msg[len] = '\0';

    ESP_LOGI(TAG, "Received from server: %s", msg);

    if (strncmp(msg, "time:", 5) == 0) {
        int64_t server_unix = strtoll(msg + 5, nullptr, 10);
        struct timeval tv = { .tv_sec = server_unix, .tv_usec = 0 };
        settimeofday(&tv, nullptr);
        rtc_synced_with_server = true;
        ESP_LOGI(TAG, "System time updated from server.");

        // Save epoch + uptime to NVS
        int64_t stored_epoch_ref = 0;
        int64_t stored_uptime_ref = 0;
        stored_epoch_ref  = server_unix;
        stored_uptime_ref = esp_timer_get_time() / 1000000LL;
        save_time_reference(stored_epoch_ref, stored_uptime_ref); // persist to flash
    }


    ESP_LOGI(TAG, "Setting send_process_complete to TRUE");
    send_process_complete = true;
}


void on_DataRecvResponse(const esp_now_recv_info *info, const uint8_t *data, int len) {
    static const char *TAG = "on_DataRecvResponse";

    if (!info || !data || len <= 0) return;

    // --- Filter by source MAC ---
    const uint8_t *src = info->src_addr;
    bool from_server = memcmp(src, PEER_SERVER, 6) == 0;
    bool from_pump = false;

    for (uint8_t i = 0; i < SENSOR_CONFIG_COUNT; i++) {
        if (memcmp(sensor_configs[i].pump_mac, src, 6) == 0) {
            from_pump = true;
            break;
        }
    }

    if (!from_server && !from_pump) {
        ESP_LOGW(TAG, "Ignoring message: unknown sender");
        return;
    }

    char msg[256];
    int copylen = len < (int)sizeof(msg) - 1 ? len : (int)sizeof(msg) - 1;
    memcpy(msg, data, copylen);
    msg[copylen] = '\0';

    ESP_LOGI(TAG, "Response received: %s", msg);

    if (from_pump) {
        ESP_LOGI(TAG, "Received from Pump: %s", msg);
        if (strncmp(msg, "thanks", 6) == 0) {
            response_received = true;
            ESP_LOGI(TAG, "Setting send_response_process_complete to TRUE");
            send_response_process_complete = true;
        } else {
            ESP_LOGW(TAG, "Unrecognized message");
        }
        return;
    }

    const char *time_ptr = strstr(msg, "time:");
    if (time_ptr) {
        time_t server_unix = strtoul(time_ptr + 5, nullptr, 10);
        struct timeval tv = { .tv_sec = server_unix, .tv_usec = 0 };
        settimeofday(&tv, nullptr);
        rtc_synced_with_server = true;
        response_received = true;
        ESP_LOGI(TAG, "System time updated from server (Response).");
    }

    const char *cfg_ptr = msg;
    while ((cfg_ptr = strstr(cfg_ptr, "cfg:")) != nullptr) {
        cfg_ptr += 4;
        const char *end = strchr(cfg_ptr, ',');
        size_t len_cfg = end ? (size_t)(end - cfg_ptr) : strlen(cfg_ptr);
        if (len_cfg >= sizeof(g_pending_sensor_configs[0])) len_cfg = sizeof(g_pending_sensor_configs[0]) - 1;

        char new_cfg[32];
        memcpy(new_cfg, cfg_ptr, len_cfg);
        new_cfg[len_cfg] = '\0';

        bool exists = false;
        for (uint8_t i = 0; i < g_pending_sensor_configs_count; i++) {
            if (strcmp(g_pending_sensor_configs[i], new_cfg) == 0) {
                exists = true;
                break;
            }
        }

        if (!exists && g_pending_sensor_configs_count < 4) {
            strncpy(g_pending_sensor_configs[g_pending_sensor_configs_count], new_cfg, sizeof(g_pending_sensor_configs[0]) - 1);
            g_pending_sensor_configs[g_pending_sensor_configs_count][sizeof(g_pending_sensor_configs[0]) - 1] = '\0';
            g_pending_sensor_configs_count++;
            ESP_LOGI(TAG, "Added new config: %s", new_cfg);
        }

        if (!end) break;
        cfg_ptr = end;
    }

    ESP_LOGI(TAG, "Setting send_response_process_complete to TRUE");
    send_response_process_complete = true;
}

void print_AllPeers() {
    static const char *TAG = "print_AllPeers";

    esp_now_peer_num_t peerCount;
    esp_err_t res = esp_now_get_peer_num(&peerCount);
    if (res != ESP_OK) {
        ESP_LOGE(TAG, "Failed to get peer count");
        return;
    }

    ESP_LOGI(TAG, "Total peers: %d", peerCount.total_num);

    for (int i = 0; i < peerCount.total_num; i++) {
        esp_now_peer_info_t peerInfo;
        res = esp_now_fetch_peer(i, &peerInfo);
        printMac(TAG, peerInfo.peer_addr);
        if (res == ESP_OK) {
            ESP_LOGI(TAG, "Peer is active");
        } else {
            ESP_LOGE(TAG, "Failed to fetch peer");
        }
    }
}

void add_Peer(const uint8_t* addr, int32_t channel) {
    static const char *TAG = "print_AllPeers";

    ESP_LOGI(TAG, "add_Peer: ");
    printMac(TAG, addr);
    if (!addr) {
        ESP_LOGE(TAG, "ERROR: null MAC address!");
        return;
    }
    for (int i = 0; i < 6; i++) {
        if (addr[i] != 0) continue;
        else {
            ESP_LOGE(TAG, "ERROR: MAC address is zero!");
            return;
        }
    }

    if (esp_now_is_peer_exist(addr)) {
        ESP_LOGI(TAG, "Peer already exists, skipping:");
        return;
    }

    esp_now_peer_info_t peer{};
    memcpy(peer.peer_addr, addr, 6);
    peer.ifidx = WIFI_IF_STA;
    peer.channel = channel;
    peer.encrypt = false;

    esp_err_t res = esp_now_add_peer(&peer);
    if (res == ESP_OK) {
        ESP_LOGI(TAG, "Peer added");
    } else {
        ESP_LOGE(TAG, "Failed to add");
        ESP_LOGE(TAG, " (ESP_ERR: %d)", res);
    }
}

void ensure_Peers() {
    int32_t channel = get_WiFiChannel();

    for (size_t i=0; i<SENSOR_CONFIG_COUNT; i++)
        add_Peer(sensor_configs[i].pump_mac, channel);

    add_Peer(PEER_SERVER, channel);

    print_AllPeers();
}

void init_EspNow() {
    static const char *TAG = "init_EspNow";

    if (esp_now_init() != ESP_OK) {
        ESP_LOGE(TAG, "ESP NOW init failed");
        while (true);
    }
    esp_now_register_send_cb(on_DataSent);
    ensure_Peers();
}

bool send_Message(const uint8_t* peer, const char* message) {
    static const char *TAG = "send_Message";

    ESP_LOGI(TAG, "send_Message to");
    printMac(TAG, peer);
    ESP_LOGI(TAG, "  %s", message);
    if (!esp_now_is_peer_exist(peer)) {
        ESP_LOGE(TAG, "ERROR: peer doese not exist!");
        return false;
    }
    esp_err_t result = esp_now_send(peer, (uint8_t*)message, strlen(message));
    return result == ESP_NOW_SEND_SUCCESS;
}

bool send_AndWaitResponse(const uint8_t* peer, const char* message, uint32_t timeout_ms = 5000) {
    static const char *TAG = "send_AndWaitResponse";

    send_response_process_complete = false;
    response_received = false;

    esp_now_register_recv_cb(on_DataRecvResponse);

    if (!send_Message(peer, message)) {
        ESP_LOGE(TAG, "Failed to queue message for sending!");
        return false;
    }

    unsigned long start = millis();
    while (!response_received && millis() - start < timeout_ms) {
        vTaskDelay(pdMS_TO_TICKS(10));
    }

    if (!response_received) {
        ESP_LOGE(TAG, "No Response received from ");
        printMac(TAG, peer);
        return false;
    }

    return true;
}

// ----------------------------------------------------------------------------
// Time handling
// ----------------------------------------------------------------------------

#define TIME_NAMESPACE "time_ref"

void save_time_reference(int64_t epoch, int64_t uptime) {
    nvs_handle_t nvs;
    if (nvs_open(TIME_NAMESPACE, NVS_READWRITE, &nvs) == ESP_OK) {
        nvs_set_i64(nvs, "epoch", epoch);
        nvs_set_i64(nvs, "uptime", uptime);
        nvs_commit(nvs);
        nvs_close(nvs);
    }
}

void load_time_reference(int64_t *epoch, int64_t *uptime) {
    nvs_handle_t nvs;
    int64_t e = 0, u = 0;
    if (nvs_open(TIME_NAMESPACE, NVS_READONLY, &nvs) == ESP_OK) {
        nvs_get_i64(nvs, "epoch", &e);
        nvs_get_i64(nvs, "uptime", &u);
        nvs_close(nvs);
    }
    if (epoch) *epoch = e;
    if (uptime) *uptime = u;
}

int64_t get_CurrentUnix() {
    struct timeval tv;
    gettimeofday(&tv, NULL);

    return tv.tv_sec;
}

int64_t get_CurrentUnixMs() {
    return get_CurrentUnix() * 1000;
}

bool request_TimeSync(uint32_t timeout_ms) {
    const uint8_t* server_mac = PEER_SERVER;
    unsigned long start = millis();
    esp_now_register_recv_cb(on_DataRecv);
    send_Message(server_mac, "time?");
    while (millis() - start < timeout_ms) {
        if (rtc_synced_with_server) return true;
        vTaskDelay(pdMS_TO_TICKS(50));
    }
    return false;
}

void setup_Timezone() {
    setenv("TZ", "CET-1CEST,M3.5.0,M10.5.0/3", 1);
    tzset();
}

void get_LocalTime(int *hour, int *minute) {
    time_t unix_time = get_CurrentUnix();
    struct tm tm_time;
    localtime_r(&unix_time, &tm_time);
    *hour = tm_time.tm_hour;
    *minute = tm_time.tm_min;
}

uint16_t get_CurrentMinutes() {
    int hour, minute;
    get_LocalTime(&hour, &minute);
    return hour * 60 + minute;
}

// ----------------------------------------------------------------------------
// Deep sleep scheduler
// ----------------------------------------------------------------------------


bool is_TimeToReadSensor(const SensorConfig_s& cfg) {
    static const char *TAG = "is_TimeToReadSensor";

    ESP_LOGI(TAG, "check %s", cfg.sensor_name);

    if (!cfg.is_active) {
        ESP_LOGW(TAG, "Sensor is inactive!");
        return false;
    }
    ESP_LOGI(TAG, "  %lld <= %lld ", cfg.next_sensor_read, get_CurrentUnix());
    return (cfg.next_sensor_read <= get_CurrentUnix());
}

int64_t get_NextSensorReadFor(const char* sensor_name) {
    int64_t  unix_now    = get_CurrentUnix();
    uint16_t min_current = get_CurrentMinutes();

    for (size_t i = 0; i < SENSOR_CONFIG_COUNT; i++) {
        SensorConfig_s &cfg = sensor_configs[i];

        if (!cfg.is_active) continue;
        if (strcmp(cfg.sensor_name, sensor_name) != 0) continue;

        int64_t last_read = cfg.last_sensor_read > 0 && cfg.last_sensor_read < unix_now
                          ? cfg.last_sensor_read
                          : unix_now;

        int64_t next_time = INT64_MAX;

        if (cfg.use_time && cfg.time_count > 0) {
            uint32_t best_delta_min = 24 * 60;
            for (size_t j = 0; j < cfg.time_count; j++) {
                int16_t delta = (int16_t)cfg.times[j] - (int16_t)min_current;
                if (delta <= 0) delta += 24 * 60;
                if ((uint32_t)delta < best_delta_min)
                    best_delta_min = delta;
            }
            next_time = (unix_now - (unix_now % 60)) + (int64_t)best_delta_min * 60;
        } else if (cfg.delay_ms > 0) {
            next_time = last_read + (cfg.delay_ms / 1000);
        }

        if (next_time != INT64_MAX) {
            return next_time;
        }
    }

    return -1;
}

int64_t get_NextSensorReadsUnixTime(const uint8_t* mac) {
    int64_t unix_next = INT64_MAX;

    for (size_t i = 0; i < SENSOR_CONFIG_COUNT; i++) {
        SensorConfig_s &cfg = sensor_configs[i];
        if (!cfg.is_active) continue;
        if (mac != nullptr && memcmp(mac, cfg.pump_mac, 6) != 0) continue;


        int64_t next_time = get_NextSensorReadFor(cfg.sensor_name);

        if (next_time > 0 && next_time < unix_next)
            unix_next = next_time;
    }

    return (unix_next == INT64_MAX) ? -1 : unix_next;
}

void set_AllNextSensorReads() {
    for (size_t i = 0; i < SENSOR_CONFIG_COUNT; i++) {
        SensorConfig_s &cfg = sensor_configs[i];
        if (!cfg.is_active) continue;
        cfg.next_sensor_read = get_NextSensorReadFor(cfg.sensor_name);

        ESP_LOGI("set_AllNextSensorReads", "next read updated: %lld", cfg.next_sensor_read);
    }
}


void sleep_UntilNextSend(int64_t next_read) {
    static const char *TAG = "sleep_UntilNextSend";

    int64_t now_sec = get_CurrentUnix();

    int64_t delta_sec = next_read - now_sec;
    if (delta_sec <= 0) delta_sec = 0;

    uint32_t min_delay_ms = delta_sec > UINT32_MAX / 1000
                          ? UINT32_MAX
                          : (uint32_t)(delta_sec * 1000);


    // Prevent sleeping if next send is within g_min_sleep_time
    if (min_delay_ms < g_min_sleep_time) {
        ESP_LOGI(TAG, "Delaying for %u ms (no deep sleep)", min_delay_ms);
        ESP_LOGI(TAG, "######################");
        vTaskDelay(pdMS_TO_TICKS(min_delay_ms));
        return;
    }

    uint32_t prep_time_ms = g_setup_duration + g_wakeup_earlier_sensor;
    uint32_t sleep_time_ms = (min_delay_ms > prep_time_ms)
                           ? (min_delay_ms - prep_time_ms)
                           : 0;


    save_SensorConfigs();

    ESP_LOGI(TAG, "Sleeping for %u ms (setup margin: %u ms)",
               sleep_time_ms, prep_time_ms);
    ESP_LOGI(TAG, "######################");

    esp_sleep_enable_timer_wakeup((uint64_t)sleep_time_ms * 1000ULL);
    esp_deep_sleep_start();
}


// ----------------------------------------------------------------------------
// Request sensor config from server via ESP-NOW
// ----------------------------------------------------------------------------
volatile
bool sensor_config_received     = false;
char sensor_config_message[256] = { 0 };

void on_DataRecvSensorConfig(const esp_now_recv_info* info, const uint8_t* data, int len) {
    static const char *TAG = "on_DataRecvSensorConfig";

    if (!info || !data || len <= 0) return;
    printMac(TAG, info->src_addr);
    if (memcmp(info->src_addr, PEER_SERVER, 6) != 0) return;

    int copylen = len < (int)sizeof(sensor_config_message) - 1 ? len : (int)sizeof(sensor_config_message) - 1;
    memcpy(sensor_config_message, data, copylen);
    sensor_config_message[copylen] = '\0';

    ESP_LOGI(TAG, "%s", sensor_config_message);
    if (strncmp(sensor_config_message, "cfg:", 4) != 0) return;

    sensor_config_received = true;
}

bool request_SensorConfig(const char* sensor_name) {
    static const char *TAG = "request_SensorConfig";

    ESP_LOGI(TAG, "Getting sensor config for: %s", sensor_name);
    const uint8_t* server_mac = PEER_SERVER;

    sensor_config_received = false;
    memset(sensor_config_message, 0, sizeof(sensor_config_message));

    esp_now_register_recv_cb(on_DataRecvSensorConfig);

    char msg[64];
    snprintf(msg, sizeof(msg), "get_config:%s", sensor_name);

    if (!send_Message(server_mac, msg)) {
        ESP_LOGE(TAG, "Failed to send get_config request");
        return false;
    }

    unsigned long start = millis();
    const uint32_t max_wait = 10000;

    while (!sensor_config_received && millis() - start < max_wait) {
        vTaskDelay(pdMS_TO_TICKS(50));
    }

    if (!sensor_config_received) {
        ESP_LOGE(TAG, "No config reply received (timeout)");
        return false;
    }

    if (strncmp(sensor_config_message, "cfg:", 4) != 0) {
        ESP_LOGE(TAG, "Invalid config response: %s", sensor_config_message);
        return false;
    }

    char* ptr = sensor_config_message + 4;
    char* tokens[20];
    uint8_t count_token = 0;
    char* tok = strtok(ptr, ",");
    while (tok && count_token < 20) {
        tokens[count_token++] = tok;
        tok = strtok(nullptr, ",");
    }

    if (count_token == 0) return false;

    ESP_LOGI(TAG, "Config received for %s:", tokens[0]);
    for (uint8_t i = 1; i < count_token; i++) {
        ESP_LOGI(TAG, "  %u: %s", i, tokens[i]);
    }


    if (count_token < 9) {
        ESP_LOGE(TAG, "Incomplete config data");
        return false;
    }

    SensorConfig_s *cfg = NULL;
    for (size_t i = 0; i < SENSOR_CONFIG_COUNT; i++) {
        if (strcmp(sensor_configs[i].sensor_name, tokens[0]) == 0) {
            cfg = &sensor_configs[i];
            break;
        }
    }

    if (!cfg) {
        ESP_LOGE(TAG, "Unknown sensor name in config: %s", tokens[0]);
        return false;
    }

    // basic fields
    strncpy(cfg->sensor_name, tokens[0], sizeof(cfg->sensor_name)-1);
    cfg->sensor_name[sizeof(cfg->sensor_name)-1] = '\0';
    cfg->is_active = atoi(tokens[1]) != 0;
    cfg->pin = atoi(tokens[2]);
    cfg->min_moisture = atoi(tokens[3]);
    strncpy(cfg->notification_name, tokens[4], sizeof(cfg->notification_name)-1);
    cfg->notification_name[sizeof(cfg->notification_name)-1] = '\0';
    cfg->pumping_delay_sec = atoi(tokens[5]);
    cfg->pumping_duration = atoi(tokens[6]);
    /*
    uint8_t new_mac[6];
    sscanf(tokens[7], "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
                &new_mac[0], &new_mac[1], &new_mac[2], &new_mac[3], &new_mac[4], &new_mac[5]);
                */
    cfg->delay_ms = atoi(tokens[7]);
    cfg->use_time = atoi(tokens[8]) != 0;

    // times
    cfg->time_count = 0;
    if (cfg->use_time && count_token > 9 && strncmp(tokens[9], "times=", 6) == 0) {
        char *time_str = tokens[9] + 6;
        char *time_tok = strtok(time_str, "|");
        while (time_tok && cfg->time_count < 8) {
            cfg->times[cfg->time_count++] = (uint16_t)atoi(time_tok);
            time_tok = strtok(NULL, "|");
        }
    }

    if (cfg->last_sensor_read == 0) {
        ESP_LOGI(TAG, "RESETTING last_sensor_read");
        cfg->last_sensor_read = get_CurrentUnix();
    }

    cfg->next_sensor_read = get_NextSensorReadFor(cfg->sensor_name);
    config_request_attempts = 0;
    return true;
}

void process_PendingSensorConfigs() {
    static const char *TAG = "process_PendingSensorConfigs";

    config_request_attempts = 0;
    ESP_LOGI(TAG, "NEW CONFIGS: %d", g_pending_sensor_configs_count);

    size_t config_id = 0;
    bool save_new_configs = false;
    while (config_id < g_pending_sensor_configs_count) {
        ESP_LOGI(TAG, "SENSOR: %s", g_pending_sensor_configs[config_id]);

        bool success = false;
        for (config_request_attempts = 0;
                config_request_attempts < config_request_attempts_max;
                config_request_attempts++) {

            ESP_LOGI(TAG, "Requesting config attempt %u...", config_request_attempts + 1);
            if (request_SensorConfig(g_pending_sensor_configs[config_id])) {
                success = true;
                break;
            }
            vTaskDelay(pdMS_TO_TICKS(2000));
        }

        if (success) {
            ESP_LOGI(TAG, "Config %s processed successfully, removing.",
                    g_pending_sensor_configs[config_id]);
            if (config_id >= g_pending_sensor_configs_count) return;
            for (size_t i = config_id; i < g_pending_sensor_configs_count - 1; i++)
                strcpy(g_pending_sensor_configs[i], g_pending_sensor_configs[i + 1]);
            g_pending_sensor_configs_count--;
            save_new_configs = true;
        } else {
            ESP_LOGE(TAG, "Config request failed repeatedly, aborting loop.");
            break;
        }
    }

    if (save_new_configs) save_SensorConfigs();
}

void get_AllSensorConfigs() {
    g_pending_sensor_configs_count = 0;

    for (size_t i = 0; i < SENSOR_CONFIG_COUNT; ++i) {
        strncpy(
            g_pending_sensor_configs[g_pending_sensor_configs_count],
            sensor_configs[i].sensor_name,
            sizeof(g_pending_sensor_configs[0]) - 1
        );

        // reset last_sensor_read
        sensor_configs[i].last_sensor_read = get_CurrentUnix();

        g_pending_sensor_configs[g_pending_sensor_configs_count][sizeof(g_pending_sensor_configs[0]) - 1] = '\0';

        ++g_pending_sensor_configs_count;
    }


    process_PendingSensorConfigs();
    if (g_pending_sensor_configs_count == 0) {
        ESP_LOGI("get_AllSensorConfigs", "All init configs received");
        all_init_configs_received = true;
    }
}

// ----------------------------------------------------------------------------
// Send Data to peers
// ----------------------------------------------------------------------------


void send_DataToPumpAndServer(size_t config_id, uint32_t value, bool shall_pump) {
    static const char *TAG = "send_DataToPumpAndServer";

    SensorConfig_s& cfg = sensor_configs[config_id];
    ESP_LOGI(TAG, "%s", cfg.sensor_name);

    int pumping_duration = shall_pump ? cfg.pumping_duration : 0;

    char message[64];

    ESP_LOGI(TAG, "Sending to Pump: "); printMac(TAG, cfg.pump_mac);
    snprintf(message, sizeof(message), "p:%lld:%s:%d:%d:%lld",
            get_CurrentUnix(),
            cfg.notification_name,
            cfg.pumping_delay_sec,
            pumping_duration,
            get_NextSensorReadsUnixTime(cfg.pump_mac));

    // START MESSAGE PUMP
    bool reached_pump = false;

    // try three times..
    for (int attempt = 1; attempt <= 3; attempt++) {
        if (send_AndWaitResponse(cfg.pump_mac, message)) {
            reached_pump = true;
            break;
        }

        ESP_LOGW(TAG, "No response from Pump (attempt %d/3)", attempt);

        if (attempt < 3)
            vTaskDelay(pdMS_TO_TICKS(5000));
    }

    if (!reached_pump) {
        ESP_LOGE(TAG, "Pump did not respond after 3 attempts!");
        send_response_process_complete = true;
    }
    // END MESSAGE PUMP

    while (!send_response_process_complete)
            vTaskDelay(pdMS_TO_TICKS(100));

    ESP_LOGI(TAG, "Sending to Server: "); printMac(TAG, PEER_SERVER);
    snprintf(message, sizeof(message), "%s:%lu:%lld:%d",
            cfg.sensor_name,
            value,
            cfg.last_sensor_read,
            reached_pump);
    send_response_process_complete = false;
    if (!send_AndWaitResponse(PEER_SERVER, message)) {
        ESP_LOGE(TAG, "No response from server!");
        send_response_process_complete = true;
    }
    while (!send_response_process_complete)
            vTaskDelay(pdMS_TO_TICKS(100));
}


void send_NextSensorReadToPeer(const uint8_t* mac, int64_t unix_time) {
    static const char *TAG = "send_NextSensorReadToPeer";

    char message[64];

    snprintf(message, sizeof(message), "next_read:%lld", unix_time);

    ESP_LOGI(TAG, "Sending to peer: ");
    printMac(TAG, mac);
    if (!send_Message(mac, message)) {
        ESP_LOGE(TAG, "Delivery failed or peer offline!");
    }
}