From 9b41663096d666d546a754d478cce66608c928ec Mon Sep 17 00:00:00 2001
From: Peter Poetzi <peter.poetzi@gmail.com>
Date: Fri, 1 Oct 2021 18:01:32 +0200
Subject: [PATCH] add speed sync for led animation

---
 config_peter.cmake |  3 ++-
 main/ledstrip.h    | 43 ++++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 44 insertions(+), 2 deletions(-)

diff --git a/config_peter.cmake b/config_peter.cmake
index 4e15f8f..cb82cb3 100644
--- a/config_peter.cmake
+++ b/config_peter.cmake
@@ -92,5 +92,6 @@ set(BOBBYCAR_BUILDFLAGS
     -DPINS_LEDSTRIP=33
     -DLEDSTRIP_LENGTH=288
     #  -DLEDSTRIP_WRONG_DIRECTION
-    -DLEDSTRIP_ANIMATION_DEFAULT=1
+    -DLEDSTRIP_ANIMATION_DEFAULT=2
+    -DLEDS_PER_METER=144
 )
diff --git a/main/ledstrip.h b/main/ledstrip.h
index 4e8aca1..1c9454e 100644
--- a/main/ledstrip.h
+++ b/main/ledstrip.h
@@ -1,4 +1,5 @@
 #pragma once
+
 #ifdef FEATURE_LEDSTRIP
 // 3rdparty lib includes
 #include <FastLED.h>
@@ -109,10 +110,50 @@ void showBetterRainbow() {
         std::fill(std::begin(leds), std::end(leds), CRGB{0, 0, 0});
 }
 
+void fill_rainbow_invert_at( struct CRGB * pFirstLED, int numToFill, int invertAtLed,
+                  uint8_t initialhue,
+                  float deltahue )
+{
+    float huecalc = initialhue;
+    CHSV hsv;
+    hsv.hue = initialhue;
+    hsv.val = 255;
+    hsv.sat = 240;
+    for( int i = 0; i < numToFill; i++) {
+        hsv.hue = huecalc;
+        pFirstLED[i] = hsv;
+        if(i>invertAtLed){
+            huecalc -= deltahue;
+        }else{
+            huecalc += deltahue;
+        }
+    }
+}
+
 void showSpeedSyncAnimation() {
     if (settings.ledstrip.enableLedAnimation)
     {
-        // Code that shows static animation relative to the ground
+#ifdef LEDS_PER_METER
+        const float leds_per_meter = LEDS_PER_METER;
+#else
+        const float leds_per_meter = 144;
+#endif
+
+        static auto last_interval = espchrono::millis_clock::now();
+        auto difference_ms = espchrono::ago(last_interval).count();
+
+        static float hue_result = 0;
+
+        const float hue_per_led = .1;
+        const float meter_per_second = avgSpeedKmh / 3.6;
+        const float leds_per_second = meter_per_second * leds_per_meter;
+        const float hue_per_second = leds_per_second * hue_per_led;
+
+        hue_result += hue_per_second * difference_ms / 1000.f;
+
+        fill_rainbow_invert_at(&*std::begin(leds), leds.size(),leds.size()/2, hue_result,-hue_per_led);
+
+        last_interval = espchrono::millis_clock::now();
     }
     else
         std::fill(std::begin(leds), std::end(leds), CRGB{0, 0, 0});