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

/* ────────────────────────────────  Static handles  ──────────────────────────────── */

static httpd_handle_t _http_server_handle = NULL;
static zh_pcf8574_handle_t *_lcd_handle = NULL;
static zh_160x_i2c_handle_t *_lcd_160x_handle = NULL;
static zh_encoder_handle_t *_mode_encoder_handle = NULL;
static zh_inclinometer_handle_t *_a_inclinometer_handle = NULL;
static zh_inclinometer_handle_t *_b_inclinometer_handle = NULL;
static zh_tachometer_handle_t *_a_tachometer_handle = NULL;
static zh_tachometer_handle_t *_b_tachometer_handle = NULL;
static esp_timer_handle_t _inclinometer_reset_timer_handle = {0};
static SemaphoreHandle_t _lcd_mutex = NULL;
static TaskHandle_t _a_inclinometer_task_handle = NULL;
static TaskHandle_t _b_inclinometer_task_handle = NULL;
static TaskHandle_t _a_tachometer_task_handle = NULL;
static TaskHandle_t _b_tachometer_task_handle = NULL;
static TaskHandle_t _zh_stats_task_handle = NULL;

/* ────────────────────────────────  Menu handles  ───────────────────────────────── */

static zh_menu_handle_t *_menu_handle = NULL;
static zh_menu_node_t _main_menu_tree[];

/* ────────────────────────────────  Forward declarations  ───────────────────────── */

static void _zh_wifi_init(void);
static void _zh_webserver_init(void);
static void _zh_espnow_init(void);
static void _zh_encoder_init(void);
static void _zh_menu_init(void);
static void _zh_a_inclinometer_init(void);
static void _zh_b_inclinometer_init(void);
static void _zh_a_tachometer_init(void);
static void _zh_b_tachometer_init(void);
static void _zh_pcf8574_init(void);
static void _zh_system_load(void);
static void _zh_timer_init(void);
static void _zh_encoder_event_handler(void *arg, esp_event_base_t event_base, int32_t event_id, void *event_data);
static void _zh_espnow_event_handler(void *arg, esp_event_base_t event_base, int32_t event_id, void *event_data);
static void _zh_wifi_event_handler(void *arg, esp_event_base_t event_base, int32_t event_id, void *event_data);
static void _zh_inclinometer_reset_timer_on_alarm_cb(void *arg);
static void _zh_a_inclinometer_processing_task(void *pvParameter);
static void _zh_b_inclinometer_processing_task(void *pvParameter);
static void _zh_a_tachometer_processing_task(void *pvParameter);
static void _zh_b_tachometer_processing_task(void *pvParameter);
static void _zh_stats_processing_task(void *pvParameter);

/* Menu callbacks */
static void _menu_enter_cb(void *arg);
static void _menu_exit_cb(void *arg);
static void _menu_change_cb(zh_menu_node_t *current_node, uint8_t current_index);
static void _menu_select_angle_angle(void *arg);
static void _menu_select_tacho_tacho(void *arg);
static void _menu_select_angle_tacho(void *arg);
static void _menu_select_remote(void *arg);
static void _menu_select_exit(void *arg);

/* ────────────────────────────────  State variables  ───────────────────────────── */

volatile static task_stats_t _task_stats = {0};
volatile static bool _is_reset_mode = false;
volatile static bool _is_reset_occurred = false;
volatile static uint8_t _current_menu_value = MODE_ANGLE_ANGLE; /* active mode, not menu index */
volatile static uint8_t _wifi_reconnect_retry_num = 0;
volatile static bool _is_wifi_connected = false;

/* Task-stop flags for graceful termination */
volatile static bool _stop_a_inclinometer = false;
volatile static bool _stop_b_inclinometer = false;
volatile static bool _stop_a_tachometer = false;
volatile static bool _stop_b_tachometer = false;

/* ────────────────────────────────  Menu tree definition  ──────────────────────── */

static zh_menu_node_t _main_menu_tree[] = {
    {.label = MAIN_MENU_ANGLE_ANGLE_NAME, .item_cb = &_menu_select_angle_angle},
    {.label = MAIN_MENU_TACHO_TACHO_NAME, .item_cb = &_menu_select_tacho_tacho},
    {.label = MAIN_MENU_ANGLE_TACHO_NAME, .item_cb = &_menu_select_angle_tacho},
    {.label = MAIN_MENU_REMOTE_NAME,      .item_cb = &_menu_select_remote},
    {.label = "Exit",                     .item_cb = &_menu_select_exit},
};

static zh_menu_node_t _root_menu = {
    .label = "MODE",
    .submenu = _main_menu_tree,
    .item_numbers = sizeof(_main_menu_tree) / sizeof(_main_menu_tree[0]),
};

/* ────────────────────────────────  app_main  ───────────────────────────────────── */

void app_main(void)
{
    esp_err_t err = nvs_flash_init();
    if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND)
    {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ESP_ERROR_CHECK(nvs_flash_init());
    }
    esp_event_loop_create_default();
    _zh_pcf8574_init();
    _zh_system_load();
    _zh_encoder_init();
    _zh_timer_init();
    _zh_menu_init();
    _zh_a_inclinometer_init(); /* start with default mode */
    _zh_b_inclinometer_init();
    _zh_wifi_init();
    _zh_webserver_init();
    _zh_espnow_init();
    const esp_partition_t *running = esp_ota_get_running_partition();
    esp_ota_img_states_t ota_state = {0};
    esp_ota_get_state_partition(running, &ota_state);
    if (ota_state == ESP_OTA_IMG_PENDING_VERIFY)
    {
        vTaskDelay(60000 / portTICK_PERIOD_MS);
        esp_ota_mark_app_valid_cancel_rollback();
    }
}

/* ────────────────────────────────  WiFi  ───────────────────────────────────────── */

static void _zh_wifi_init(void)
{
    esp_netif_init();
    esp_netif_create_default_wifi_ap();
    esp_netif_create_default_wifi_sta();
    wifi_init_config_t wifi_init_config = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&wifi_init_config));

    wifi_config_t wifi_ap_config = {
        .ap.ssid = WIFI_AP_SSID,
        .ap.password = WIFI_AP_PASS,
        .ap.channel = WIFI_AP_CHANNEL,
        .ap.max_connection = MAX_AP_CONNECTION,
        .ap.authmode = WIFI_AUTH_WPA2_PSK,
    };
    wifi_config_t wifi_sta_config = {
        .sta.ssid = WIFI_STA_SSID,
        .sta.password = WIFI_STA_PASS,
    };
    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_APSTA));
    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_ap_config));
    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_sta_config));
    esp_event_handler_instance_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &_zh_wifi_event_handler, NULL, NULL);
    esp_event_handler_instance_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &_zh_wifi_event_handler, NULL, NULL);
    ESP_ERROR_CHECK(esp_wifi_start());
}

/* ────────────────────────────────  Web server  ─────────────────────────────────── */

static void _zh_webserver_init(void)
{
    httpd_config_t config = HTTPD_DEFAULT_CONFIG();
    ESP_ERROR_CHECK(httpd_start(&_http_server_handle, &config));
    zh_ota_server_init(_http_server_handle, "/");
}

static void _zh_espnow_init(void)
{
    // To Do.
}

/* ────────────────────────────────  Encoder  ────────────────────────────────────── */

static void _zh_encoder_init(void)
{
    esp_event_handler_instance_register(ZH_ENCODER, ESP_EVENT_ANY_ID, &_zh_encoder_event_handler, NULL, NULL);
    zh_encoder_init_config_t config = ZH_ENCODER_INIT_CONFIG_DEFAULT();
    config.task_priority = ENCODER_TASK_PRIORITY;
    config.stack_size = ENCODER_TASK_STACK_SIZE;
    config.queue_size = 5;
    config.a_gpio_number = MODE_ENCODER_A_GPIO;
    config.b_gpio_number = MODE_ENCODER_B_GPIO;
    config.s_gpio_number = MODE_ENCODER_S_GPIO;
    config.s_gpio_debounce_time = 1000;
    config.encoder_min_value = 0;
    config.encoder_max_value = 3;
    config.encoder_step = 1;
    config.encoder_number = MODE_ENCODER_NUMBER;
    zh_encoder_init(&config, &_mode_encoder_handle);
    zh_encoder_set(&_mode_encoder_handle, 0);
}

/* ────────────────────────────────  Menu  ───────────────────────────────────────── */

static void _zh_menu_init(void)
{
    zh_menu_init_config_t config = ZH_MENU_INIT_CONFIG_DEFAULT();
    config.task_priority = MENU_TASK_PRIORITY;
    config.stack_size = MENU_TASK_STACK_SIZE;
    config.queue_size = 10;
    config.root = &_root_menu;
    config.loop = false;        /* no wrap-around, so "Exit" is at the end */
    config.viewport_rows = 1;   /* LCD 16x2: row 0 = header, row 1 = one item */
    config.enter_cb = &_menu_enter_cb;
    config.exit_cb  = &_menu_exit_cb;
    config.change_cb = &_menu_change_cb;
    ESP_ERROR_CHECK(zh_menu_init(&config, &_menu_handle));
}

/* ────────────────────────────────  PCF8574 / LCD  ──────────────────────────────── */

static void _zh_pcf8574_init(void)
{
    i2c_master_bus_handle_t _i2c_bus_handle = NULL;
    i2c_master_bus_config_t i2c_bus_config = {
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .scl_io_num = I2C_SCL_GPIO,
        .sda_io_num = I2C_SDA_GPIO,
        .glitch_ignore_cnt = 7,
        .flags.enable_internal_pullup = true,
    };
    ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_bus_config, &_i2c_bus_handle));
    zh_pcf8574_init_config_t config = ZH_PCF8574_INIT_CONFIG_DEFAULT();
    config.task_priority = EXPANDER_TASK_PRIORITY;
    config.stack_size = EXPANDER_TASK_STACK_SIZE;
    config.i2c_handle = _i2c_bus_handle;
    config.i2c_address = LCD_I2C_ADDRESS;
    zh_pcf8574_init(&config, &_lcd_handle);
    zh_160x_init(&_lcd_handle, &_lcd_160x_handle, ZH_LCD_16X2);
    _lcd_mutex = xSemaphoreCreateMutex();
    configASSERT(_lcd_mutex != NULL);
}

static void _zh_system_load(void)
{
    zh_160x_set_cursor(&_lcd_160x_handle, 0, 0);
    zh_160x_print_char(&_lcd_160x_handle, "LLC AEROTECH");
    zh_160x_set_cursor(&_lcd_160x_handle, 1, 0);
    const esp_app_desc_t *esp_app_desc = esp_app_get_description();
    zh_160x_print_char(&_lcd_160x_handle, esp_app_desc->project_name);
    zh_160x_set_cursor(&_lcd_160x_handle, 1, 8);
    zh_160x_print_char(&_lcd_160x_handle, "V");
    zh_160x_print_char(&_lcd_160x_handle, esp_app_desc->version);
    vTaskDelay(1000 / portTICK_PERIOD_MS);
    zh_160x_lcd_clear(&_lcd_160x_handle);
    zh_160x_set_cursor(&_lcd_160x_handle, 0, 0);
    zh_160x_print_char(&_lcd_160x_handle, "SYSTEM LOADING:");
    for (uint8_t i = 0; i <= 100; i += 10)
    {
        zh_160x_print_progress_bar(&_lcd_160x_handle, 1, i);
        vTaskDelay(100 / portTICK_PERIOD_MS);
    }
    vTaskDelay(500 / portTICK_PERIOD_MS);
    zh_160x_lcd_clear(&_lcd_160x_handle);
    vTaskDelay(500 / portTICK_PERIOD_MS);
    zh_160x_set_cursor(&_lcd_160x_handle, 0, 0);
    zh_160x_print_char(&_lcd_160x_handle, "MODE:");
    zh_160x_print_char(&_lcd_160x_handle, _main_menu_tree[0].label);
}

/* ────────────────────────────────  Timer  ──────────────────────────────────────── */

static void _zh_timer_init(void)
{
    const esp_timer_create_args_t timer_args = {
        .callback = &_zh_inclinometer_reset_timer_on_alarm_cb,
    };
    ESP_ERROR_CHECK(esp_timer_create(&timer_args, &_inclinometer_reset_timer_handle));
}

/* ────────────────────────────────  Task stopping helpers  ──────────────────────── */

static void _stop_all_processing_tasks(void)
{
    _stop_a_inclinometer = true;
    _stop_b_inclinometer = true;
    _stop_a_tachometer = true;
    _stop_b_tachometer = true;

    /* Give tasks up to 200 ms to exit their loops and call vTaskDelete(NULL) */
    vTaskDelay(200 / portTICK_PERIOD_MS);

    _stop_a_inclinometer = false;
    _stop_b_inclinometer = false;
    _stop_a_tachometer = false;
    _stop_b_tachometer = false;

    _a_inclinometer_task_handle = NULL;
    _b_inclinometer_task_handle = NULL;
    _a_tachometer_task_handle = NULL;
    _b_tachometer_task_handle = NULL;
}

/* ────────────────────────────────  Encoder event handler  ──────────────────────── */

static void _zh_encoder_event_handler(void *arg, esp_event_base_t event_base, int32_t event_id, void *event_data)
{
    (void)arg;
    zh_menu_status_t menu_status = {0};
    zh_menu_get(&_menu_handle, &menu_status);

    switch (event_id)
    {
    case ZH_BUTTON_EVENT:
    {
        zh_encoder_button_event_on_isr_t *button_event = (zh_encoder_button_event_on_isr_t *)event_data;
        if (button_event->button_status == ZH_ENCODER_BUTTON_STATUS_PRESSED)
        {
            esp_timer_start_once(_inclinometer_reset_timer_handle, 2000000);
            _is_reset_mode = true;
        }
        else
        {
            if (_is_reset_occurred == true)
            {
                _is_reset_occurred = false;
                break;
            }
            if (_is_reset_mode == true)
            {
                esp_timer_stop(_inclinometer_reset_timer_handle);
                _is_reset_mode = false;
            }

            if (menu_status.is_active == false)
            {
                zh_menu_set(&_menu_handle, ZH_MENU_ENTER, NULL, 0);
            }
            else if (menu_status.is_selected == false)
            {
                zh_menu_set(&_menu_handle, ZH_MENU_SELECT, NULL, 0);
            }
            else
            {
                zh_menu_set(&_menu_handle, ZH_MENU_BACK, NULL, 0);
            }
        }
        break;
    }
    case ZH_ENCODER_EVENT:
    {
        zh_encoder_event_on_isr_t *encoder_event = (zh_encoder_event_on_isr_t *)event_data;
        if (menu_status.is_active == true)
        {
            if (menu_status.is_selected == true)
            {
                zh_menu_set(&_menu_handle, ZH_MENU_SELECT, encoder_event, sizeof(zh_encoder_event_on_isr_t));
            }
            else
            {
                if (encoder_event->encoder_status == true)
                {
                    zh_menu_set(&_menu_handle, ZH_MENU_UP, NULL, 0);
                }
                else
                {
                    zh_menu_set(&_menu_handle, ZH_MENU_DOWN, NULL, 0);
                }
            }
        }
        else
        {
            zh_encoder_set(&_mode_encoder_handle, (float)_current_menu_value);
        }
        break;
    }
    default:
        break;
    }
}

/* ────────────────────────────────  Menu callbacks  ────────────────────────────── */

static void _menu_enter_cb(void *arg)
{
    (void)arg;
    xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
    zh_160x_on_cursor(&_lcd_160x_handle, true);
    xSemaphoreGive(_lcd_mutex);
}

static void _menu_exit_cb(void *arg)
{
    (void)arg;
    xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
    zh_160x_off_cursor(&_lcd_160x_handle);
    xSemaphoreGive(_lcd_mutex);
    _stop_all_processing_tasks();
    switch (_current_menu_value)
    {
    case MODE_ANGLE_ANGLE:
        _zh_a_inclinometer_init();
        _zh_b_inclinometer_init();
        break;
    case MODE_TACHO_TACHO:
        _zh_a_tachometer_init();
        _zh_b_tachometer_init();
        break;
    case MODE_ANGLE_TACHO:
        _zh_a_inclinometer_init();
        _zh_b_tachometer_init();
        break;
    case MODE_REMOTE:
        xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
        zh_160x_set_cursor(&_lcd_160x_handle, 1, 0);
        zh_160x_print_char(&_lcd_160x_handle, "NOT CONNECTED");
        xSemaphoreGive(_lcd_mutex);
        break;
    default:
        break;
    }
}

static void _menu_change_cb(zh_menu_node_t *current_node, uint8_t current_index)
{
    zh_menu_view_t view = {0};
    zh_menu_get_view(&_menu_handle, &view);

    xSemaphoreTake(_lcd_mutex, portMAX_DELAY);

    /* Header row */
    zh_160x_clear_row(&_lcd_160x_handle, 0);
    zh_160x_set_cursor(&_lcd_160x_handle, 0, 0);
    zh_160x_print_char(&_lcd_160x_handle, "MODE:");

    /* Single visible item row (viewport_rows = 1) */
    zh_160x_clear_row(&_lcd_160x_handle, 1);
    zh_160x_set_cursor(&_lcd_160x_handle, 1, 0);

    uint8_t idx = view.first_visible;
    char line[17] = {0};
    snprintf(line, sizeof(line), "%c%s",
             (idx == current_index) ? '>' : ' ',
             current_node->submenu[idx].label);
    zh_160x_print_char(&_lcd_160x_handle, line);

    /* Hidden-item indicators */
    if (view.first_visible > 0)
    {
        zh_160x_set_cursor(&_lcd_160x_handle, 1, 15);
        zh_160x_print_char(&_lcd_160x_handle, "^");
    }
    if ((uint8_t)(view.first_visible + view.visible_count) < current_node->item_numbers)
    {
        zh_160x_set_cursor(&_lcd_160x_handle, 1, 15);
        zh_160x_print_char(&_lcd_160x_handle, "v");
    }

    xSemaphoreGive(_lcd_mutex);
}

static void _menu_select_angle_angle(void *arg)
{
    (void)arg;
    _stop_all_processing_tasks();
    _current_menu_value = MODE_ANGLE_ANGLE;
    _zh_a_inclinometer_init();
    _zh_b_inclinometer_init();
    zh_menu_set(&_menu_handle, ZH_MENU_BACK, NULL, 0);
}

static void _menu_select_tacho_tacho(void *arg)
{
    (void)arg;
    _stop_all_processing_tasks();
    _current_menu_value = MODE_TACHO_TACHO;
    _zh_a_tachometer_init();
    _zh_b_tachometer_init();
    zh_menu_set(&_menu_handle, ZH_MENU_BACK, NULL, 0);
}

static void _menu_select_angle_tacho(void *arg)
{
    (void)arg;
    _stop_all_processing_tasks();
    _current_menu_value = MODE_ANGLE_TACHO;
    _zh_a_inclinometer_init();
    _zh_b_tachometer_init();
    zh_menu_set(&_menu_handle, ZH_MENU_BACK, NULL, 0);
}

static void _menu_select_remote(void *arg)
{
    (void)arg;
    _stop_all_processing_tasks();
    _current_menu_value = MODE_REMOTE;
    /* TODO: initialise ESP-NOW remote mode */
    xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
    zh_160x_set_cursor(&_lcd_160x_handle, 1, 0);
    zh_160x_print_char(&_lcd_160x_handle, "NOT CONNECTED");
    xSemaphoreGive(_lcd_mutex);
    zh_menu_set(&_menu_handle, ZH_MENU_BACK, NULL, 0);
}

static void _menu_select_exit(void *arg)
{
    (void)arg;
    zh_menu_set(&_menu_handle, ZH_MENU_BACK, NULL, 0);
}

/* ────────────────────────────────  WiFi event handler  ────────────────────────── */

static void _zh_wifi_event_handler(void *arg, esp_event_base_t event_base, int32_t event_id, void *event_data)
{
    (void)arg;
    (void)event_data;
    switch (event_id)
    {
    case WIFI_EVENT_STA_START:
        esp_wifi_connect();
        break;
    case WIFI_EVENT_STA_DISCONNECTED:
        if (_is_wifi_connected == true)
        {
            if (_zh_stats_task_handle != NULL)
            {
                vTaskDelete(_zh_stats_task_handle);
                _zh_stats_task_handle = NULL; /* critical: vTaskDelete does NOT NULL the handle */
            }
            zh_syslog_deinit();
            _is_wifi_connected = false;
        }
        if (_wifi_reconnect_retry_num < WIFI_STA_MAXIMUM_RETRY)
        {
            esp_wifi_connect();
            ++_wifi_reconnect_retry_num;
        }
        break;
    case IP_EVENT_STA_GOT_IP:
        _wifi_reconnect_retry_num = 0;
        _is_wifi_connected = true;
        zh_syslog_init_config_t config = ZH_SYSLOG_INIT_CONFIG_DEFAULT();
        snprintf(config.syslog_ip, sizeof(config.syslog_ip), "%s", SYSLOG_IP);
        zh_syslog_init(&config);
        if (_zh_stats_task_handle == NULL)
        {
            xTaskCreatePinnedToCore(&_zh_stats_processing_task, "stats_processing_task", STATS_TASK_STACK_SIZE, NULL, STATS_TASK_PRIORITY, &_zh_stats_task_handle, tskNO_AFFINITY);
        }
        break;
    default:
        break;
    }
}

/* ────────────────────────────────  Reset timer callback  ──────────────────────── */

static void _zh_inclinometer_reset_timer_on_alarm_cb(void *arg)
{
    (void)arg;
    _is_reset_mode = false;

    zh_menu_status_t menu_status = {0};
    zh_menu_get(&_menu_handle, &menu_status);
    if (menu_status.is_active == false)
    {
        _is_reset_occurred = true;
        switch (_current_menu_value)
        {
        case MODE_ANGLE_ANGLE:
            zh_inclinometer_reset(&_a_inclinometer_handle);
            zh_inclinometer_reset(&_b_inclinometer_handle);
            break;
        case MODE_ANGLE_TACHO:
            zh_inclinometer_reset(&_a_inclinometer_handle);
            break;
        default:
            break;
        }
    }
}

/* ────────────────────────────────  Processing tasks  ──────────────────────────── */

static void _zh_a_inclinometer_init(void)
{
    zh_inclinometer_init_config_t config = ZH_INCLINOMETER_INIT_CONFIG_DEFAULT();
    config.a_gpio_number = A_OPTICAL_DEVICE_A_GPIO;
    config.b_gpio_number = A_OPTICAL_DEVICE_B_GPIO;
    config.encoder_pulses = 3600;
    zh_inclinometer_init(&config, &_a_inclinometer_handle);
    xTaskCreatePinnedToCore(&_zh_a_inclinometer_processing_task, "a_inclinometer_processing_task",
                            A_INCLINOMETER_TASK_STACK_SIZE, NULL, A_INCLINOMETER_TASK_PRIORITY,
                            &_a_inclinometer_task_handle, tskNO_AFFINITY);
}

static void _zh_b_inclinometer_init(void)
{
    zh_inclinometer_init_config_t config = ZH_INCLINOMETER_INIT_CONFIG_DEFAULT();
    config.a_gpio_number = B_OPTICAL_DEVICE_A_GPIO;
    config.b_gpio_number = B_OPTICAL_DEVICE_B_GPIO;
    config.encoder_pulses = 3600;
    zh_inclinometer_init(&config, &_b_inclinometer_handle);
    xTaskCreatePinnedToCore(&_zh_b_inclinometer_processing_task, "b_inclinometer_processing_task",
                            B_INCLINOMETER_TASK_STACK_SIZE, NULL, B_INCLINOMETER_TASK_PRIORITY,
                            &_b_inclinometer_task_handle, tskNO_AFFINITY);
}

static void _zh_a_tachometer_init(void)
{
    zh_tachometer_init_config_t config = ZH_TACHOMETER_INIT_CONFIG_DEFAULT();
    config.a_gpio_number = A_OPTICAL_DEVICE_A_GPIO;
    config.b_gpio_number = A_OPTICAL_DEVICE_B_GPIO;
    config.encoder_pulses = 3600;
    zh_tachometer_init(&config, &_a_tachometer_handle);
    xTaskCreatePinnedToCore(&_zh_a_tachometer_processing_task, "a_tachometer_processing_task",
                            A_TACHOMETER_TASK_STACK_SIZE, NULL, A_TACHOMETER_TASK_PRIORITY,
                            &_a_tachometer_task_handle, tskNO_AFFINITY);
}

static void _zh_b_tachometer_init(void)
{
    zh_tachometer_init_config_t config = ZH_TACHOMETER_INIT_CONFIG_DEFAULT();
    config.a_gpio_number = B_OPTICAL_DEVICE_A_GPIO;
    config.b_gpio_number = B_OPTICAL_DEVICE_B_GPIO;
    config.encoder_pulses = 3600;
    zh_tachometer_init(&config, &_b_tachometer_handle);
    xTaskCreatePinnedToCore(&_zh_b_tachometer_processing_task, "b_tachometer_processing_task",
                            B_TACHOMETER_TASK_STACK_SIZE, NULL, B_TACHOMETER_TASK_PRIORITY,
                            &_b_tachometer_task_handle, tskNO_AFFINITY);
}

static void _zh_a_inclinometer_processing_task(void *pvParameter)
{
    (void)pvParameter;
    float prev_angle = 0.0;
    float current_angle = 0.0;

    xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
    zh_160x_set_cursor(&_lcd_160x_handle, 1, 0);
    zh_160x_print_char(&_lcd_160x_handle, "A:");
    zh_160x_print_float(&_lcd_160x_handle, current_angle, 1);
    xSemaphoreGive(_lcd_mutex);

    while (!_stop_a_inclinometer)
    {
        zh_inclinometer_get(&_a_inclinometer_handle, &current_angle);
        if (prev_angle != current_angle)
        {
            prev_angle = current_angle;
            xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
            zh_160x_set_cursor(&_lcd_160x_handle, 1, 5);
            zh_160x_print_char(&_lcd_160x_handle, "   ");
            zh_160x_set_cursor(&_lcd_160x_handle, 1, 2);
            zh_160x_print_float(&_lcd_160x_handle, current_angle, 1);
            xSemaphoreGive(_lcd_mutex);
        }
        vTaskDelay(100 / portTICK_PERIOD_MS);
        _task_stats.a_inclinometer_task_min_stack_size = (uint32_t)uxTaskGetStackHighWaterMark(NULL);
    }
    vTaskDelete(NULL);
}

static void _zh_b_inclinometer_processing_task(void *pvParameter)
{
    (void)pvParameter;
    float prev_angle = 0.0;
    float current_angle = 0.0;

    xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
    zh_160x_set_cursor(&_lcd_160x_handle, 1, 8);
    zh_160x_print_char(&_lcd_160x_handle, "B:");
    zh_160x_print_float(&_lcd_160x_handle, current_angle, 1);
    xSemaphoreGive(_lcd_mutex);

    while (!_stop_b_inclinometer)
    {
        zh_inclinometer_get(&_b_inclinometer_handle, &current_angle);
        if (prev_angle != current_angle)
        {
            prev_angle = current_angle;
            xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
            zh_160x_set_cursor(&_lcd_160x_handle, 1, 13);
            zh_160x_print_char(&_lcd_160x_handle, "   ");
            zh_160x_set_cursor(&_lcd_160x_handle, 1, 10);
            zh_160x_print_float(&_lcd_160x_handle, current_angle, 1);
            xSemaphoreGive(_lcd_mutex);
        }
        vTaskDelay(100 / portTICK_PERIOD_MS);
        _task_stats.b_inclinometer_task_min_stack_size = (uint32_t)uxTaskGetStackHighWaterMark(NULL);
    }
    vTaskDelete(NULL);
}

static void _zh_a_tachometer_processing_task(void *pvParameter)
{
    (void)pvParameter;
    uint16_t prev_rpm = 0;
    uint16_t current_rpm = 0;

    xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
    zh_160x_set_cursor(&_lcd_160x_handle, 1, 0);
    zh_160x_print_char(&_lcd_160x_handle, "A:");
    zh_160x_print_int(&_lcd_160x_handle, current_rpm);
    xSemaphoreGive(_lcd_mutex);

    while (!_stop_a_tachometer)
    {
        zh_tachometer_get(&_a_tachometer_handle, &current_rpm);
        if (prev_rpm != current_rpm)
        {
            prev_rpm = current_rpm;
            xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
            zh_160x_set_cursor(&_lcd_160x_handle, 1, 3);
            zh_160x_print_char(&_lcd_160x_handle, "    ");
            zh_160x_set_cursor(&_lcd_160x_handle, 1, 2);
            zh_160x_print_int(&_lcd_160x_handle, current_rpm);
            xSemaphoreGive(_lcd_mutex);
        }
        vTaskDelay(100 / portTICK_PERIOD_MS);
        _task_stats.a_tachometer_task_min_stack_size = (uint32_t)uxTaskGetStackHighWaterMark(NULL);
    }
    vTaskDelete(NULL);
}

static void _zh_b_tachometer_processing_task(void *pvParameter)
{
    (void)pvParameter;
    uint16_t prev_rpm = 0;
    uint16_t current_rpm = 0;

    xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
    zh_160x_set_cursor(&_lcd_160x_handle, 1, 8);
    zh_160x_print_char(&_lcd_160x_handle, "B:");
    zh_160x_print_int(&_lcd_160x_handle, current_rpm);
    xSemaphoreGive(_lcd_mutex);

    while (!_stop_b_tachometer)
    {
        zh_tachometer_get(&_b_tachometer_handle, &current_rpm);
        if (prev_rpm != current_rpm)
        {
            prev_rpm = current_rpm;
            xSemaphoreTake(_lcd_mutex, portMAX_DELAY);
            zh_160x_set_cursor(&_lcd_160x_handle, 1, 11);
            zh_160x_print_char(&_lcd_160x_handle, "    ");
            zh_160x_set_cursor(&_lcd_160x_handle, 1, 10);
            zh_160x_print_int(&_lcd_160x_handle, current_rpm);
            xSemaphoreGive(_lcd_mutex);
        }
        vTaskDelay(100 / portTICK_PERIOD_MS);
        _task_stats.b_tachometer_task_min_stack_size = (uint32_t)uxTaskGetStackHighWaterMark(NULL);
    }
    vTaskDelete(NULL);
}

static void _zh_stats_processing_task(void *pvParameter)
{
    (void)pvParameter;
    const esp_app_desc_t *esp_app_desc = esp_app_get_description();
    for (;;)
    {
        char buffer[1024] = {0};
        const zh_encoder_stats_t *encoder_stats = zh_encoder_get_stats();
        const zh_pcf8574_stats_t *pcf8574_stats = zh_pcf8574_get_stats();
        snprintf(buffer, sizeof(buffer), "A inclinometer task minimum free stack size: %lu.\n", (unsigned long)_task_stats.a_inclinometer_task_min_stack_size);
        zh_syslog_send(ZH_KERN, _task_stats.a_inclinometer_task_min_stack_size > 100 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_inclinometer", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "B inclinometer task minimum free stack size: %lu.\n", (unsigned long)_task_stats.b_inclinometer_task_min_stack_size);
        zh_syslog_send(ZH_KERN, _task_stats.b_inclinometer_task_min_stack_size > 100 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_inclinometer", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "A tachometer task minimum free stack size: %lu.\n", (unsigned long)_task_stats.a_tachometer_task_min_stack_size);
        zh_syslog_send(ZH_KERN, _task_stats.a_tachometer_task_min_stack_size > 100 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_tachometer", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "B tachometer task minimum free stack size: %lu.\n", (unsigned long)_task_stats.b_tachometer_task_min_stack_size);
        zh_syslog_send(ZH_KERN, _task_stats.b_tachometer_task_min_stack_size > 100 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_tachometer", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "Stats task minimum free stack size: %lu.\n", (unsigned long)uxTaskGetStackHighWaterMark(NULL));
        zh_syslog_send(ZH_KERN, (uint32_t)uxTaskGetStackHighWaterMark(NULL) > 100 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_syslog", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "Encoder task minimum free stack size: %lu.\n", (unsigned long)encoder_stats->min_stack_size);
        zh_syslog_send(ZH_KERN, encoder_stats->min_stack_size > 100 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_encoder", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "Expander task minimum free stack size: %lu.\n", (unsigned long)pcf8574_stats->min_stack_size);
        zh_syslog_send(ZH_KERN, pcf8574_stats->min_stack_size > 100 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_pcf8574", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "Number of event post error: %lu.\n", (unsigned long)encoder_stats->event_post_error);
        zh_syslog_send(ZH_KERN, encoder_stats->event_post_error == 0 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_encoder", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "Number of queue overflow error: %lu.\n", (unsigned long)encoder_stats->queue_overflow_error);
        zh_syslog_send(ZH_KERN, encoder_stats->queue_overflow_error == 0 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_encoder", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "Number of i2c driver error: %lu.\n", (unsigned long)pcf8574_stats->i2c_driver_error);
        zh_syslog_send(ZH_KERN, pcf8574_stats->i2c_driver_error == 0 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_pcf8574", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "Number of event post error: %lu.\n", (unsigned long)pcf8574_stats->event_post_error);
        zh_syslog_send(ZH_KERN, pcf8574_stats->event_post_error == 0 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_pcf8574", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "Number of vector error: %lu.\n", (unsigned long)pcf8574_stats->vector_error);
        zh_syslog_send(ZH_KERN, pcf8574_stats->vector_error == 0 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_pcf8574", buffer);
        vTaskDelay(500 / portTICK_PERIOD_MS);
        snprintf(buffer, sizeof(buffer), "Number of queue overflow error: %lu.\n", (unsigned long)pcf8574_stats->queue_overflow_error);
        zh_syslog_send(ZH_KERN, pcf8574_stats->queue_overflow_error == 0 ? ZH_INFO : ZH_WARNING, esp_app_desc->project_name, "zh_pcf8574", buffer);
        vTaskDelay(60000 / portTICK_PERIOD_MS);
    }
    vTaskDelete(NULL);
}