From 22cefac580f1dc420ed381abc6d14d332b2c88a7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Franti=C5=A1ek=20Boh=C3=A1=C4=8Dek?=
 <fandabohacek@gmail.com>
Date: Fri, 25 Jun 2021 16:49:51 +0200
Subject: [PATCH] feat: add timeout for rgb leds

---
 lib-pheripherals/include/mzapo_rgb_led.h | 28 ++++++++++++++++++++-
 lib-pheripherals/src/mzapo_rgb_led.c     | 32 ++++++++++++++++++++++++
 2 files changed, 59 insertions(+), 1 deletion(-)

diff --git a/lib-pheripherals/include/mzapo_rgb_led.h b/lib-pheripherals/include/mzapo_rgb_led.h
index d5c78c2045f8a64b81470207708b1dbc68440d7f..9e24640c5770ef108b54b2d2db36d9b0089a480d 100644
--- a/lib-pheripherals/include/mzapo_rgb_led.h
+++ b/lib-pheripherals/include/mzapo_rgb_led.h
@@ -2,8 +2,9 @@
 #define __MZAPO_RGB_LED_H__
 
 #include "display_utils.h"
+#include <time.h>
 
-typedef enum { LED_LEFT, LED_RIGHT } mzapo_rgb_leds_t;
+typedef enum { LED_LEFT, LED_RIGHT, RGB_LEDS_COUNT } mzapo_rgb_leds_t;
 
 #ifdef __cplusplus
 extern "C" {
@@ -14,8 +15,14 @@ typedef struct {
   uint8_t red;
 } __attribute__((__packed__)) rgb_led_pixel_t;
 
+typedef struct {
+  uint32_t timeout_ms;
+  struct timespec set_time;
+} rgb_led_state_t;
+
 typedef struct {
   volatile rgb_led_pixel_t *mem_base;
+  rgb_led_state_t states[RGB_LEDS_COUNT];
 } mzapo_rgb_led_t;
 
 /**
@@ -38,6 +45,25 @@ mzapo_rgb_led_t rgb_led_create(unsigned char *mem_base);
 void rgb_led_set(mzapo_rgb_led_t *rgb_led, mzapo_rgb_leds_t id, uint8_t r,
                  uint8_t g, uint8_t b);
 
+/**
+ * @brief Set given rgb led to rgb, then turn it off after timeout
+ *
+ * @param rgb_led
+ * @param id what led to change
+ * @param r red (min 0, max 255)
+ * @param g green (min 0, max 255)
+ * @param b blue (min 0, max 255)
+ * @param timeout_ms milliseconds till led is turned off
+ */
+void rgb_led_set_timeout(mzapo_rgb_led_t *rgb_led, mzapo_rgb_leds_t id, uint8_t r,
+                         uint8_t g, uint8_t b, uint32_t timeout_ms);
+/**
+ * @brief Check if timeout was met so led should be turned off
+ *
+ * @param rgb_led
+ */
+void rgb_led_update(mzapo_rgb_led_t *rgb_led);
+
 /**
  * @brief Set given rgb led to full red
  *
diff --git a/lib-pheripherals/src/mzapo_rgb_led.c b/lib-pheripherals/src/mzapo_rgb_led.c
index d4264011426ac115418ccba871d9bdcaedb74ecc..ecd24d1ca1a900a524eb3d08e9975053f15180a2 100644
--- a/lib-pheripherals/src/mzapo_rgb_led.c
+++ b/lib-pheripherals/src/mzapo_rgb_led.c
@@ -1,11 +1,17 @@
 #include "mzapo_rgb_led.h"
+#include <bits/time.h>
 #include <stdlib.h>
+#include <time.h>
 
 mzapo_rgb_led_t rgb_led_create(unsigned char *mem_base) {
   mzapo_rgb_led_t rgb_led = {
     .mem_base = (volatile rgb_led_pixel_t*)mem_base,
   };
 
+  for (int i = 0; i < RGB_LEDS_COUNT; i++) {
+    rgb_led.states[i].timeout_ms = 0;
+  }
+
   return rgb_led;
 }
 
@@ -36,3 +42,29 @@ rgb_led_pixel_t rgb_led_get(mzapo_rgb_led_t *rgb_led, mzapo_rgb_leds_t id) {
 void rgb_led_clear(mzapo_rgb_led_t *rgb_led, mzapo_rgb_leds_t id) {
   rgb_led_set(rgb_led, id, 0, 0, 0);
 }
+
+void rgb_led_set_timeout(mzapo_rgb_led_t *rgb_led, mzapo_rgb_leds_t id,
+                         uint8_t r, uint8_t g, uint8_t b, uint32_t timeout_ms) {
+  rgb_led_set(rgb_led, id, r, g, b);
+  rgb_led->states[id].timeout_ms = timeout_ms;
+  clock_gettime(CLOCK_MONOTONIC, &rgb_led->states[id].set_time);
+}
+
+void rgb_led_update(mzapo_rgb_led_t *rgb_led) {
+  struct timespec now;
+  clock_gettime(CLOCK_MONOTONIC, &now);
+
+  for (int i = 0; i < RGB_LEDS_COUNT; i++) {
+    if (rgb_led->states[i].timeout_ms != 0) {
+      struct timespec set_time = rgb_led->states[i].set_time;
+      uint32_t diff = (((now.tv_sec - set_time.tv_sec) * 1000000) +
+                       (now.tv_nsec - set_time.tv_nsec)) /
+                      1000;
+
+      if (diff >= rgb_led->states[i].timeout_ms) {
+        rgb_led->states[i].timeout_ms = 0;
+        rgb_led_clear(rgb_led, i);
+      }
+    }
+  }
+}