make event queue go faster

1 files changed, 18 insertions, 2 deletions

diff --git a/src/main/main.cpp b/src/main/main.cpp
index e2c187b1..d283b01d 100644
--- a/src/main/main.cpp
+++ b/src/main/main.cpp
@@ -6,6 +6,9 @@
 
 #include "freertos/portmacro.h"
 
+#include "gpios.hpp"
+#include "i2c.hpp"
+#include "system_events.hpp"
 #include "tinyfsm.hpp"
 
 #include "audio_fsm.hpp"
@@ -14,11 +17,24 @@
 #include "ui_fsm.hpp"
 
 extern "C" void app_main(void) {
+  ESP_ERROR_CHECK(drivers::init_i2c());
+  drivers::Gpios* gpios = system_fsm::SystemState::early_init_gpios();
+
+  QueueSetHandle_t set = xQueueCreateSet(2);
+  auto* event_queue = events::queues::SystemAndAudio();
+  xQueueAddToSet(event_queue->has_events(), set);
+  xQueueAddToSet(gpios->IsReadPending(), set);
+
   tinyfsm::FsmList<system_fsm::SystemState, ui::UiState,
                    audio::AudioState>::start();
 
-  auto& queue = events::EventQueue::GetInstance();
   while (1) {
-    queue.ServiceSystemAndAudio(portMAX_DELAY);
+    QueueSetMemberHandle_t member = xQueueSelectFromSet(set, portMAX_DELAY);
+    if (member == event_queue->has_events()) {
+      event_queue->Service(0);
+    } else if (member == gpios->IsReadPending()) {
+      xSemaphoreTake(member, 0);
+      events::System().Dispatch(system_fsm::internal::GpioInterrupt{});
+    }
   }
 }