diff --git a/firmware/atom_s3_i2c_display/lib/atom_s3_mode_manager/atom_s3_mode_manager.cpp b/firmware/atom_s3_i2c_display/lib/atom_s3_mode_manager/atom_s3_mode_manager.cpp
new file mode 100644
index 00000000..3d6ddb2a
--- /dev/null
+++ b/firmware/atom_s3_i2c_display/lib/atom_s3_mode_manager/atom_s3_mode_manager.cpp
@@ -0,0 +1,80 @@
+#include <atom_s3_mode_manager.h>
+
+AtomS3ModeManager* AtomS3ModeManager::instance = nullptr;
+
+std::vector<Mode*> AtomS3ModeManager::allModes = {};
+int AtomS3ModeManager::current_mode_index = 0;
+
+AtomS3ModeManager::AtomS3ModeManager(AtomS3LCD &lcd, AtomS3Button &button, AtomS3I2C &i2c)
+  : atoms3lcd(lcd), atoms3button(button), atoms3i2c(i2c)
+{
+  instance = this;
+}
+
+void AtomS3ModeManager::task(void *parameter) {
+  if (instance == nullptr) {
+    return;
+  }
+
+  instance->atoms3lcd.printWaitMessage(instance->atoms3i2c.i2c_slave_addr);
+  while (true) {
+    // Check if Mode is forced to change
+    bool isModeForced = false;
+    int forced_mode_index;
+    for (int i = 0; i < allModes.size(); i++) {
+      if (allModes[i]->getModeName() == instance->atoms3i2c.forcedMode) {
+        isModeForced = true;
+        forced_mode_index = i;
+        break;
+      }
+    }
+    // Force mode change
+    if (isModeForced) {
+      instance->changeMode(current_mode_index, forced_mode_index);
+      current_mode_index = forced_mode_index;
+      instance->atoms3i2c.forcedMode = "";
+    }
+    // Change mode by long click
+    if (instance->atoms3button.wasLongPressed()) {
+      int next_mode_index = (current_mode_index + 1) % allModes.size();
+      instance->changeMode(current_mode_index, next_mode_index);
+      current_mode_index = next_mode_index;
+    }
+    vTaskDelay(pdMS_TO_TICKS(500));
+  }
+}
+
+void AtomS3ModeManager::createTask(uint8_t xCoreID) {
+  xTaskCreatePinnedToCore(task, "Mode Manager Task", 2048, this, 24, NULL, xCoreID);
+}
+
+void AtomS3ModeManager::initializeAllModes(uint8_t xCoreID) {
+  // Start user-defined mode
+  for (int i = 0; i < allModes.size(); i++) {
+    allModes[i]->createTask(xCoreID);
+    allModes[i]->suspendTask();
+  }
+}
+
+void AtomS3ModeManager::startCurrentMode() {
+  allModes[current_mode_index]->resumeTask();
+}
+
+void AtomS3ModeManager::changeMode(int suspend_mode_index, int resume_mode_index) {
+    // Suspend
+    allModes[suspend_mode_index]->suspendTask();
+    // Transition
+    allModes[suspend_mode_index]->waitForTaskSuspended();
+    instance->atoms3i2c.stopReceiveEvent();
+    instance->atoms3lcd.drawBlack();
+    instance->atoms3lcd.printMessage("Wait for mode switch ...");
+    vTaskDelay(pdMS_TO_TICKS(1000));
+    instance->atoms3lcd.resetLcdData();
+    // Resume
+    allModes[resume_mode_index]->resumeTask();
+    instance->atoms3i2c.startReceiveEvent();
+}
+
+void AtomS3ModeManager::addMode(Mode &mode) {
+  allModes.push_back(&mode);
+}
diff --git a/firmware/atom_s3_i2c_display/lib/atom_s3_mode_manager/atom_s3_mode_manager.h b/firmware/atom_s3_i2c_display/lib/atom_s3_mode_manager/atom_s3_mode_manager.h
new file mode 100644
index 00000000..f35605c2
--- /dev/null
+++ b/firmware/atom_s3_i2c_display/lib/atom_s3_mode_manager/atom_s3_mode_manager.h
@@ -0,0 +1,34 @@
+#ifndef ATOM_S3_MODE_MANAGER_H
+#define ATOM_S3_MODE_MANAGER_H
+
+#include <vector>
+
+#include <atom_s3_button.h>
+#include <atom_s3_lcd.h>
+#include <atom_s3_i2c.h>
+
+#include <mode.h>
+
+class AtomS3ModeManager {
+public:
+  AtomS3ModeManager(AtomS3LCD &lcd, AtomS3Button &button, AtomS3I2C &i2c);
+  void createTask(uint8_t xCoreID);
+  void initializeAllModes(uint8_t xCoreID);
+  void startCurrentMode();
+  void addMode(Mode &mode);
+
+private:
+  static AtomS3ModeManager* instance; /**< Singleton instance of AtomS3ModeManager. */
+  // Common tasks for AtomS3
+  AtomS3Button &atoms3button;
+  AtomS3LCD &atoms3lcd;
+  AtomS3I2C &atoms3i2c;
+
+  static std::vector<Mode*> allModes;
+  static int current_mode_index;
+
+  static void task(void *parameter);
+  void changeMode(int suspend_mode_index, int resume_mode_index);
+};
+
+#endif
diff --git a/firmware/atom_s3_i2c_display/lib/mode/mode.h b/firmware/atom_s3_i2c_display/lib/mode/mode.h
index 669c87cd..c11bdeda 100644
--- a/firmware/atom_s3_i2c_display/lib/mode/mode.h
+++ b/firmware/atom_s3_i2c_display/lib/mode/mode.h
@@ -36,7 +36,7 @@ class Mode {
 
   void waitForTaskSuspended() {
     while (!isTaskSuspended()) {
-      delay(100);
+      vTaskDelay(pdMS_TO_TICKS(100));
     }
   }
 
diff --git a/firmware/atom_s3_i2c_display/src/main.cpp b/firmware/atom_s3_i2c_display/src/main.cpp
index 53f185a0..7b3ee70a 100644
--- a/firmware/atom_s3_i2c_display/src/main.cpp
+++ b/firmware/atom_s3_i2c_display/src/main.cpp
@@ -1,72 +1,35 @@
 #include <atom_s3_lcd.h>
 #include <atom_s3_i2c.h>
 #include <atom_s3_button.h>
+
+#include <atom_s3_mode_manager.h>
 #include <display_information_mode.h>
 #include <display_qrcode_mode.h>
 #include <display_image_mode.h>
 
-// Common tasks for AtomS3
 AtomS3Button atoms3button;
 AtomS3LCD atoms3lcd;
 AtomS3I2C atoms3i2c(atoms3lcd, atoms3button);
+AtomS3ModeManager atoms3modemanager(atoms3lcd, atoms3button, atoms3i2c);
 
-// User-defined modes:
 DisplayInformationMode display_information_mode(atoms3lcd, atoms3i2c);
 DisplayQRcodeMode display_qrcode_mode(atoms3lcd, atoms3i2c);
 DisplayImageMode display_image_mode(atoms3lcd, atoms3i2c);
-Mode* modes[] = { &display_information_mode, &display_qrcode_mode, &display_image_mode };
-int current_mode_index = 0;
-int num_modes = sizeof(modes) / sizeof(modes[0]);
-
-void changeMode(int suspend_mode_index, int resume_mode_index) {
-    // Suspend
-    modes[suspend_mode_index]->suspendTask();
-    // Transition
-    modes[suspend_mode_index]->waitForTaskSuspended();
-    atoms3i2c.stopReceiveEvent();
-    atoms3lcd.drawBlack();
-    atoms3lcd.printMessage("Wait for mode switch ...");
-    delay(1000);
-    atoms3lcd.resetLcdData();
-    // Resume
-    modes[resume_mode_index]->resumeTask();
-    atoms3i2c.startReceiveEvent();
-}
 
 void setup() {
-  atoms3lcd.printWaitMessage(atoms3i2c.i2c_slave_addr);
+  atoms3modemanager.addMode(display_information_mode);
+  atoms3modemanager.addMode(display_qrcode_mode);
+  atoms3modemanager.addMode(display_image_mode);
+
+  // Start one of the user-defined modes
+  uint8_t eachModeCoreID = 1;
+  atoms3modemanager.initializeAllModes(eachModeCoreID);
+  atoms3modemanager.startCurrentMode();
 
-  // Start user-defined mode
-  for (int i = 0; i < num_modes; i++) {
-    uint8_t xCoreID = 1;
-    modes[i]->createTask(xCoreID);
-    modes[i]->suspendTask();
-    }
-  modes[current_mode_index]->resumeTask();
+  // Start mode manager
+  uint8_t modeManagerCoreID = 0;
+  atoms3modemanager.createTask(modeManagerCoreID);
 }
 
 void loop() {
-  // Check if Mode is forced to change
-  bool isModeForced = false;
-  int forced_mode_index;
-  for (int i = 0; i < num_modes; i++) {
-    if (modes[i]->getModeName() == atoms3i2c.forcedMode) {
-      isModeForced = true;
-      forced_mode_index = i;
-      break;
-    }
-  }
-  // Force mode change
-  if (isModeForced) {
-    changeMode(current_mode_index, forced_mode_index);
-    current_mode_index = forced_mode_index;
-    atoms3i2c.forcedMode = "";
-  }
-  // Change mode by long click
-  if (atoms3button.wasLongPressed()) {
-    int next_mode_index = (current_mode_index + 1) % num_modes;
-    changeMode(current_mode_index, next_mode_index);
-    current_mode_index = next_mode_index;
-  }
-  delay(500);
 }