1 files changed, 66 insertions, 0 deletions
diff --git a/tracker-trackhat/led.cpp b/tracker-trackhat/led.cpp
new file mode 100644
index 00000000..db5ab6c2
--- /dev/null
+++ b/tracker-trackhat/led.cpp
@@ -0,0 +1,66 @@
+#include "trackhat.hpp"
+
+namespace trackhat_impl {
+
+void led_updater::update_(trackHat_Device_t* device, trackHat_SetLeds_t leds)
+{
+    if (leds_.ledRedState == leds.ledRedState &&
+        leds_.ledGreenState == leds.ledGreenState &&
+        leds_.ledBlueState == leds.ledBlueState)
+        return;
+    (void)trackHat_SetLeds(device, &leds);
+    leds_ = leds;
+}
+
+trackHat_SetLeds_t led_updater::next_state(led_mode mode, led_state new_state)
+{
+    switch (mode)
+    {
+    case led_mode::off:
+        state_ = led_state::stopped;
+        timer_ = std::nullopt;
+        return LED_off;
+    default:
+    case led_mode::constant:
+        state_ = led_state::stopped;
+        timer_ = std::nullopt;
+        return LED_idle;
+    case led_mode::dynamic:
+        break;
+    }
+
+    if (new_state <= led_state::stopped)
+    {
+        state_ = new_state;
+        timer_ = std::nullopt;
+        return LED_idle;
+    }
+    else if (new_state == state_)
+    {
+        timer_ = std::nullopt;
+        return leds_;
+    }
+    else if (!timer_)
+    {
+        timer_ = Timer{};
+        return leds_;
+    }
+    else if (timer_->elapsed_ms() > SWITCH_TIME_MS)
+    {
+        state_ = new_state;
+        timer_ = std::nullopt;
+        return new_state == led_state::not_tracking
+               ? LED_not_tracking
+               : LED_tracking;
+    }
+    else
+        return leds_;
+}
+
+void led_updater::update(trackHat_Device_t* device, led_mode mode, led_state new_state)
+{
+    auto leds = next_state(mode, new_state);
+    update_(device, leds);
+}
+
+} // namespace trackhat_impl