diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
index 1f27c48..664bfe6 100644
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -1 +1 @@
-idf_component_register(SRCS "ate0005.c" INCLUDE_DIRS "" REQUIRES zh_160x_i2c zh_encoder zh_inclinometer zh_tachometer zh_espnow zh_ota_server zh_syslog esp_wifi nvs_flash esp_timer esp_app_format)
\ No newline at end of file
+idf_component_register(SRCS "ate0005.c" INCLUDE_DIRS "" REQUIRES zh_160x_i2c zh_encoder zh_inclinometer zh_tachometer zh_espnow zh_ota_server zh_syslog zh_menu esp_wifi nvs_flash esp_timer esp_app_format)
\ No newline at end of file
diff --git a/main/ate0005.c b/main/ate0005.c
index c5d2a5d..7a0aabb 100644
--- a/main/ate0005.c
+++ b/main/ate0005.c
@@ -1,4 +1,8 @@
 #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;
@@ -16,10 +20,18 @@ 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);
@@ -37,15 +49,49 @@ 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);
 
-volatile static task_stats_t _task_stats = {A_INCLINOMETER_TASK_STACK_SIZE, B_INCLINOMETER_TASK_STACK_SIZE, A_TACHOMETER_TASK_STACK_SIZE, B_TACHOMETER_TASK_STACK_SIZE};
-static const char *_main_menu[] = {MAIN_MENU_ANGLE_ANGLE_NAME, MAIN_MENU_TACHO_TACHO_NAME, MAIN_MENU_ANGLE_TACHO_NAME, MAIN_MENU_REMOTE_NAME};
-volatile static bool _is_menu_mode = false;
+/* 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 = 0;
+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();
@@ -59,7 +105,8 @@ void app_main(void)
     _zh_system_load();
     _zh_encoder_init();
     _zh_timer_init();
-    _zh_a_inclinometer_init();
+    _zh_menu_init();
+    _zh_a_inclinometer_init(); /* start with default mode */
     _zh_b_inclinometer_init();
     _zh_wifi_init();
     _zh_webserver_init();
@@ -74,13 +121,16 @@ void app_main(void)
     }
 }
 
+/* ────────────────────────────────  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_wifi_init(&wifi_init_config);
+    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,
@@ -92,18 +142,20 @@ static void _zh_wifi_init(void)
         .sta.ssid = WIFI_STA_SSID,
         .sta.password = WIFI_STA_PASS,
     };
-    esp_wifi_set_mode(WIFI_MODE_APSTA);
-    esp_wifi_set_config(WIFI_IF_AP, &wifi_ap_config);
-    esp_wifi_set_config(WIFI_IF_STA, &wifi_sta_config);
+    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_wifi_start();
+    ESP_ERROR_CHECK(esp_wifi_start());
 }
 
+/* ────────────────────────────────  Web server  ─────────────────────────────────── */
+
 static void _zh_webserver_init(void)
 {
     httpd_config_t config = HTTPD_DEFAULT_CONFIG();
-    httpd_start(&_http_server_handle, &config);
+    ESP_ERROR_CHECK(httpd_start(&_http_server_handle, &config));
     zh_ota_server_init(_http_server_handle, "/");
 }
 
@@ -112,6 +164,8 @@ 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);
@@ -124,56 +178,31 @@ static void _zh_encoder_init(void)
     config.s_gpio_number = MODE_ENCODER_S_GPIO;
     config.s_gpio_debounce_time = 1000;
     config.encoder_min_value = 0;
-    config.encoder_max_value = (sizeof(_main_menu) / sizeof(_main_menu[0])) - 1;
+    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);
 }
 
-static void _zh_a_inclinometer_init(void)
+/* ────────────────────────────────  Menu  ───────────────────────────────────────── */
+
+static void _zh_menu_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);
+    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));
 }
 
-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);
-}
+/* ────────────────────────────────  PCF8574 / LCD  ──────────────────────────────── */
 
 static void _zh_pcf8574_init(void)
 {
@@ -185,7 +214,7 @@ static void _zh_pcf8574_init(void)
         .glitch_ignore_cnt = 7,
         .flags.enable_internal_pullup = true,
     };
-    i2c_new_master_bus(&i2c_bus_config, &_i2c_bus_handle);
+    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;
@@ -194,6 +223,7 @@ static void _zh_pcf8574_init(void)
     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)
@@ -220,23 +250,54 @@ static void _zh_system_load(void)
     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[0]);
+    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_timer_create(&timer_args, &_inclinometer_reset_timer_handle);
+    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)
         {
@@ -255,110 +316,185 @@ static void _zh_encoder_event_handler(void *arg, esp_event_base_t event_base, in
                 esp_timer_stop(_inclinometer_reset_timer_handle);
                 _is_reset_mode = false;
             }
-            _is_menu_mode = !_is_menu_mode;
-            if (_is_menu_mode == true)
+
+            if (menu_status.is_active == false)
             {
-                switch (_current_menu_value)
-                {
-                case MODE_ANGLE_ANGLE:
-                    if (_a_inclinometer_task_handle != NULL)
-                    {
-                        vTaskDelete(_a_inclinometer_task_handle);
-                    }
-                    if (_b_inclinometer_task_handle != NULL)
-                    {
-                        vTaskDelete(_b_inclinometer_task_handle);
-                    }
-                    zh_inclinometer_deinit(&_a_inclinometer_handle);
-                    zh_inclinometer_deinit(&_b_inclinometer_handle);
-                    break;
-                case MODE_TACHO_TACHO:
-                    if (_a_tachometer_task_handle != NULL)
-                    {
-                        vTaskDelete(_a_tachometer_task_handle);
-                    }
-                    if (_b_tachometer_task_handle != NULL)
-                    {
-                        vTaskDelete(_b_tachometer_task_handle);
-                    }
-                    zh_tachometer_deinit(&_a_tachometer_handle);
-                    zh_tachometer_deinit(&_b_tachometer_handle);
-                    break;
-                case MODE_ANGLE_TACHO:
-                    if (_a_inclinometer_task_handle != NULL)
-                    {
-                        vTaskDelete(_a_inclinometer_task_handle);
-                    }
-                    if (_b_tachometer_task_handle != NULL)
-                    {
-                        vTaskDelete(_b_tachometer_task_handle);
-                    }
-                    zh_inclinometer_deinit(&_a_inclinometer_handle);
-                    zh_tachometer_deinit(&_b_tachometer_handle);
-                    break;
-                case MODE_REMOTE:
-                    // To Do.
-                    break;
-                default:
-                    break;
-                }
-                zh_160x_clear_row(&_lcd_160x_handle, 1);
-                zh_160x_set_cursor(&_lcd_160x_handle, 0, 0);
-                zh_160x_on_cursor(&_lcd_160x_handle, true);
+                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_160x_off_cursor(&_lcd_160x_handle);
-                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:
-                    zh_160x_set_cursor(&_lcd_160x_handle, 1, 0);
-                    zh_160x_print_char(&_lcd_160x_handle, "NOT CONNECTED");
-                    break;
-                default:
-                    break;
-                }
+                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 (_is_menu_mode == true)
+        if (menu_status.is_active == true)
         {
-            _current_menu_value = (uint8_t)encoder_event->encoder_position;
-            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:");
-            zh_160x_print_char(&_lcd_160x_handle, _main_menu[(uint8_t)encoder_event->encoder_position]);
-            zh_160x_set_cursor(&_lcd_160x_handle, 0, 0);
+            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;
     }
 }
 
-static void _zh_espnow_event_handler(void *arg, esp_event_base_t event_base, int32_t event_id, void *event_data)
+/* ────────────────────────────────  Menu callbacks  ────────────────────────────── */
+
+static void _menu_enter_cb(void *arg)
 {
-    // To Do.
+    (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;
@@ -374,6 +510,7 @@ static void _zh_wifi_event_handler(void *arg, esp_event_base_t event_base, int32
             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;
@@ -388,7 +525,7 @@ static void _zh_wifi_event_handler(void *arg, esp_event_base_t event_base, int32
         _wifi_reconnect_retry_num = 0;
         _is_wifi_connected = true;
         zh_syslog_init_config_t config = ZH_SYSLOG_INIT_CONFIG_DEFAULT();
-        strcpy(config.syslog_ip, SYSLOG_IP);
+        snprintf(config.syslog_ip, sizeof(config.syslog_ip), "%s", SYSLOG_IP);
         zh_syslog_init(&config);
         if (_zh_stats_task_handle == NULL)
         {
@@ -400,13 +537,18 @@ static void _zh_wifi_event_handler(void *arg, esp_event_base_t event_base, int32
     }
 }
 
+/* ────────────────────────────────  Reset timer callback  ──────────────────────── */
+
 static void _zh_inclinometer_reset_timer_on_alarm_cb(void *arg)
 {
     (void)arg;
     _is_reset_mode = false;
-    _is_reset_occurred = true;
-    if (_is_menu_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:
@@ -422,17 +564,69 @@ static void _zh_inclinometer_reset_timer_on_alarm_cb(void *arg)
     }
 }
 
+/* ────────────────────────────────  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);
-    for (;;)
+
+    while (!_stop_a_inclinometer)
     {
         zh_inclinometer_get(&_a_inclinometer_handle, &current_angle);
         if (prev_angle != current_angle)
@@ -456,12 +650,14 @@ 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);
-    for (;;)
+
+    while (!_stop_b_inclinometer)
     {
         zh_inclinometer_get(&_b_inclinometer_handle, &current_angle);
         if (prev_angle != current_angle)
@@ -485,12 +681,14 @@ 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);
-    for (;;)
+
+    while (!_stop_a_tachometer)
     {
         zh_tachometer_get(&_a_tachometer_handle, &current_rpm);
         if (prev_rpm != current_rpm)
@@ -514,12 +712,14 @@ 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);
-    for (;;)
+
+    while (!_stop_b_tachometer)
     {
         zh_tachometer_get(&_b_tachometer_handle, &current_rpm);
         if (prev_rpm != current_rpm)
@@ -547,43 +747,43 @@ static void _zh_stats_processing_task(void *pvParameter)
         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", _task_stats.a_inclinometer_task_min_stack_size);
+        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", _task_stats.b_inclinometer_task_min_stack_size);
+        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", _task_stats.a_tachometer_task_min_stack_size);
+        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", _task_stats.b_tachometer_task_min_stack_size);
+        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", (uint32_t)uxTaskGetStackHighWaterMark(NULL));
+        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", encoder_stats->min_stack_size);
+        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", pcf8574_stats->min_stack_size);
+        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", encoder_stats->event_post_error);
+        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", encoder_stats->queue_overflow_error);
+        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", pcf8574_stats->i2c_driver_error);
+        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", pcf8574_stats->event_post_error);
+        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", pcf8574_stats->vector_error);
+        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", pcf8574_stats->queue_overflow_error);
+        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);
     }
diff --git a/main/ate0005.h b/main/ate0005.h
index fdfaf39..d94f08a 100644
--- a/main/ate0005.h
+++ b/main/ate0005.h
@@ -27,21 +27,24 @@
 #include "zh_ota_server.h"
 #include "zh_espnow.h"
 #include "zh_syslog.h"
+#include "zh_menu.h"
 
-#define STATS_TASK_PRIORITY tskIDLE_PRIORITY                          /*!< Priority of the statistics logging task. */
-#define STATS_TASK_STACK_SIZE configMINIMAL_STACK_SIZE + 1280         /*!< Stack size for the statistics logging task. */
-#define A_INCLINOMETER_TASK_PRIORITY tskIDLE_PRIORITY + 5             /*!< Priority of the A inclinometer processing task. */
-#define A_INCLINOMETER_TASK_STACK_SIZE configMINIMAL_STACK_SIZE + 512 /*!< Stack size for the A inclinometer processing task. */
-#define B_INCLINOMETER_TASK_PRIORITY tskIDLE_PRIORITY + 5             /*!< Priority of the B inclinometer processing task. */
-#define B_INCLINOMETER_TASK_STACK_SIZE configMINIMAL_STACK_SIZE + 512 /*!< Stack size for the B inclinometer processing task. */
-#define A_TACHOMETER_TASK_PRIORITY tskIDLE_PRIORITY + 5               /*!< Priority of the A tachometer processing task. */
-#define A_TACHOMETER_TASK_STACK_SIZE configMINIMAL_STACK_SIZE + 512   /*!< Stack size for the A tachometer processing task. */
-#define B_TACHOMETER_TASK_PRIORITY tskIDLE_PRIORITY + 5               /*!< Priority of the B tachometer processing task. */
-#define B_TACHOMETER_TASK_STACK_SIZE configMINIMAL_STACK_SIZE + 512   /*!< Stack size for the B tachometer processing task. */
-#define ENCODER_TASK_PRIORITY tskIDLE_PRIORITY + 4                    /*!< Priority of the mode encoder task. */
-#define ENCODER_TASK_STACK_SIZE configMINIMAL_STACK_SIZE              /*!< Stack size for the mode encoder task. */
-#define EXPANDER_TASK_PRIORITY tskIDLE_PRIORITY + 4                   /*!< Priority of the PCF8574 expander task. */
-#define EXPANDER_TASK_STACK_SIZE configMINIMAL_STACK_SIZE             /*!< Stack size for the PCF8574 expander task. */
+#define STATS_TASK_PRIORITY (tskIDLE_PRIORITY)                          /*!< Priority of the statistics logging task. */
+#define STATS_TASK_STACK_SIZE (configMINIMAL_STACK_SIZE + 1280)         /*!< Stack size for the statistics logging task. */
+#define A_INCLINOMETER_TASK_PRIORITY (tskIDLE_PRIORITY + 5)             /*!< Priority of the A inclinometer processing task. */
+#define A_INCLINOMETER_TASK_STACK_SIZE (configMINIMAL_STACK_SIZE + 512) /*!< Stack size for the A inclinometer processing task. */
+#define B_INCLINOMETER_TASK_PRIORITY (tskIDLE_PRIORITY + 5)             /*!< Priority of the B inclinometer processing task. */
+#define B_INCLINOMETER_TASK_STACK_SIZE (configMINIMAL_STACK_SIZE + 512) /*!< Stack size for the B inclinometer processing task. */
+#define A_TACHOMETER_TASK_PRIORITY (tskIDLE_PRIORITY + 5)               /*!< Priority of the A tachometer processing task. */
+#define A_TACHOMETER_TASK_STACK_SIZE (configMINIMAL_STACK_SIZE + 512)   /*!< Stack size for the A tachometer processing task. */
+#define B_TACHOMETER_TASK_PRIORITY (tskIDLE_PRIORITY + 5)               /*!< Priority of the B tachometer processing task. */
+#define B_TACHOMETER_TASK_STACK_SIZE (configMINIMAL_STACK_SIZE + 512)   /*!< Stack size for the B tachometer processing task. */
+#define ENCODER_TASK_PRIORITY (tskIDLE_PRIORITY + 4)                    /*!< Priority of the mode encoder task. */
+#define ENCODER_TASK_STACK_SIZE (configMINIMAL_STACK_SIZE + 1280)       /*!< Stack size for the mode encoder task. */
+#define EXPANDER_TASK_PRIORITY (tskIDLE_PRIORITY + 4)                   /*!< Priority of the PCF8574 expander task. */
+#define EXPANDER_TASK_STACK_SIZE (configMINIMAL_STACK_SIZE)             /*!< Stack size for the PCF8574 expander task. */
+#define MENU_TASK_PRIORITY (tskIDLE_PRIORITY + 3)                       /*!< Priority of the menu task. */
+#define MENU_TASK_STACK_SIZE (configMINIMAL_STACK_SIZE + 512)           /*!< Stack size for the menu task. */
 
 // #define TRIAC_ON true   /*!< Triac enabled state. */
 // #define TRIAC_OFF false /*!< Triac disabled state. */
@@ -104,10 +107,10 @@ extern "C"
      */
     typedef struct
     {
-        uint32_t a_inclinometer_task_min_stack_size; /*!< Minimum free stack size of the A inclinometer task. */
-        uint32_t b_inclinometer_task_min_stack_size; /*!< Minimum free stack size of the B inclinometer task. */
-        uint32_t a_tachometer_task_min_stack_size;   /*!< Minimum free stack size of the A tachometer task. */
-        uint32_t b_tachometer_task_min_stack_size;   /*!< Minimum free stack size of the B tachometer task. */
+        volatile uint32_t a_inclinometer_task_min_stack_size; /*!< Minimum free stack size of the A inclinometer task. */
+        volatile uint32_t b_inclinometer_task_min_stack_size; /*!< Minimum free stack size of the B inclinometer task. */
+        volatile uint32_t a_tachometer_task_min_stack_size;   /*!< Minimum free stack size of the A tachometer task. */
+        volatile uint32_t b_tachometer_task_min_stack_size;   /*!< Minimum free stack size of the B tachometer task. */
     } task_stats_t;
 
 #ifdef __cplusplus
diff --git a/main/idf_component.yml b/main/idf_component.yml
index 245a70e..ddb4d90 100644
--- a/main/idf_component.yml
+++ b/main/idf_component.yml
@@ -19,4 +19,6 @@ dependencies:
   zh_tachometer:
     git: "https://github.com/aZholtikov/zh_tachometer"
   zh_espnow:
-    git: "https://github.com/aZholtikov/zh_espnow"
\ No newline at end of file
+    git: "https://github.com/aZholtikov/zh_espnow"
+  zh_menu:
+    git: "https://github.com/aZholtikov/zh_menu"
\ No newline at end of file