Start on C++ fixes

wpilibsuite · Oct 23, 2024 · 54a86d6 · 54a86d6
1 parent a08aa27
commit 54a86d6
Show file tree

Hide file tree

Showing 2 changed files with 232 additions and 4 deletions.
diff --git a/wpilibc/src/main/native/cpp/LEDPattern.cpp b/wpilibc/src/main/native/cpp/LEDPattern.cpp
@@ -7,12 +7,14 @@
 #include <algorithm>
 #include <cmath>
 #include <numbers>
+#include <ranges>
 #include <unordered_map>
 #include <utility>
 #include <vector>
 
 #include <wpi/MathExtras.h>
 #include <wpi/timestamp.h>
+#include <wpi/rotated_span.h>
 
 #include "frc/MathUtil.h"
 
@@ -31,15 +33,15 @@ void LEDPattern::ApplyTo(std::span<AddressableLED::LEDData> data) const {
 
 LEDPattern LEDPattern::Reversed() {
   return LEDPattern{[self = *this](auto data, auto writer) {
-    self.ApplyTo(data, [&](int i, Color color) {
+    self.ApplyTo(std::ranges::reverse_view{data}, [&](int i, Color color) {
       writer((data.size() - 1) - i, color);
     });
   }};
 }
 
 LEDPattern LEDPattern::OffsetBy(int offset) {
   return LEDPattern{[=, self = *this](auto data, auto writer) {
-    self.ApplyTo(data, [&data, &writer, offset](int i, Color color) {
+    self.ApplyTo(wpi::rotated_span{data, offset}, [&data, &writer, offset](int i, Color color) {
       int shiftedIndex =
           frc::FloorMod(i + offset, static_cast<int>(data.size()));
       writer(shiftedIndex, color);
@@ -62,7 +64,7 @@ LEDPattern LEDPattern::ScrollAtRelativeSpeed(units::hertz_t velocity) {
         (now % static_cast<int64_t>(std::floor(periodMicros))) / periodMicros;
     int offset = static_cast<int>(std::floor(t * bufLen));
 
-    self.ApplyTo(data, [=](int i, Color color) {
+    self.ApplyTo(wpi::rotated_span{data, offset}, [=](int i, Color color) {
       // floorMod so if the offset is negative, we still get positive outputs
       int shiftedIndex = frc::FloorMod(i + offset, static_cast<int>(bufLen));
       writer(shiftedIndex, color);
@@ -87,7 +89,7 @@ LEDPattern LEDPattern::ScrollAtAbsoluteSpeed(
     // offset values for negative velocities
     auto offset = static_cast<int64_t>(now) / microsPerLed;
 
-    self.ApplyTo(data, [=, &writer](int i, Color color) {
+    self.ApplyTo(wpi::rotated_span{data, offset}, [=, &writer](int i, Color color) {
       // FloorMod so if the offset is negative, we still get positive outputs
       int shiftedIndex = frc::FloorMod(i + offset, static_cast<int>(bufLen));
 

diff --git a/wpilibc/src/test/native/cpp/LEDPatternTest.cpp b/wpilibc/src/test/native/cpp/LEDPatternTest.cpp
@@ -813,6 +813,232 @@ TEST(LEDPatternTest, ClippingBrightness) {
   AssertIndexColor(buffer, 0, Color::kWhite);
 }
 
+TEST(LEDPatternTest, ReverseMask) {
+  std::array<AddressableLED::LEDData, 8> buffer;
+
+  std::array<std::pair<double, Color>, 4> colorSteps{
+    std::pair{0.0, Color::kRed},
+    std::pair{0.25, Color::kBlue},
+    std::pair{0.5, Color::kYellow},
+    std::pair{0.75, Color::kGreen}
+  };
+  std::array<std::pair<double, Color>, 2> maskSteps{
+    std::pair{0, Color::kWhite},
+    std::pair{0.5, Color::kBlack}
+  };
+
+  auto pattern = LEDPattern::Steps(colorSteps).Mask(LEDPattern::Steps(maskSteps)).Reversed();
+
+  pattern.ApplyTo(buffer);
+
+  AssertIndexColor(buffer, 7, Color::kRed);
+  AssertIndexColor(buffer, 6, Color::kRed);
+  AssertIndexColor(buffer, 5, Color::kBlue);
+  AssertIndexColor(buffer, 4, Color::kBlue);
+  AssertIndexColor(buffer, 3, Color::kBlack);
+  AssertIndexColor(buffer, 2, Color::kBlack);
+  AssertIndexColor(buffer, 1, Color::kBlack);
+  AssertIndexColor(buffer, 0, Color::kBlack);
+}
+
+TEST(LEDPatternTest, OffsetMask) {
+  std::array<AddressableLED::LEDData, 8> buffer;
+
+  std::array<std::pair<double, Color>, 4> colorSteps{
+    std::pair{0.0, Color::kRed},
+    std::pair{0.25, Color::kBlue},
+    std::pair{0.5, Color::kYellow},
+    std::pair{0.75, Color::kGreen}
+  };
+  std::array<std::pair<double, Color>, 2> maskSteps{
+    std::pair{0, Color::kWhite},
+    std::pair{0.5, Color::kBlack}
+  };
+
+  auto pattern = LEDPattern::Steps(colorSteps).Mask(LEDPattern::Steps(maskSteps)).Reversed().OffsetBy(4);
+
+  pattern.ApplyTo(buffer);
+
+  AssertIndexColor(buffer, 0, Color::kBlack);
+  AssertIndexColor(buffer, 1, Color::kBlack);
+  AssertIndexColor(buffer, 2, Color::kBlack);
+  AssertIndexColor(buffer, 3, Color::kBlack);
+  AssertIndexColor(buffer, 4, Color::kRed);
+  AssertIndexColor(buffer, 5, Color::kRed);
+  AssertIndexColor(buffer, 6, Color::kBlue);
+  AssertIndexColor(buffer, 7, Color::kBlue);
+}
+
+TEST(LEDPatternTest, RelativeScrollingMask) {
+  std::array<AddressableLED::LEDData, 8> buffer;
+
+  std::array<std::pair<double, Color>, 4> colorSteps{
+    std::pair{0.0, Color::kRed},
+    std::pair{0.25, Color::kBlue},
+    std::pair{0.5, Color::kYellow},
+    std::pair{0.75, Color::kGreen}
+  };
+  std::array<std::pair<double, Color>, 2> maskSteps{
+    std::pair{0, Color::kWhite},
+    std::pair{0.5, Color::kBlack}
+  };
+
+  auto pattern = LEDPattern::Steps(colorSteps).Mask(LEDPattern::Steps(maskSteps)).Reversed().ScrollAtRelativeSpeed(units::hertz_t{ 12.5 * 1e3 });
+
+  pattern.ApplyTo(buffer);
+
+  static uint64_t now = 0ull;
+  WPI_SetNowImpl([] { return now; });
+
+  {
+    now = 0ull;  // start
+    SCOPED_TRACE(fmt::format("Time {}", now));
+
+    pattern.ApplyTo(buffer);
+
+    AssertIndexColor(buffer, 0, Color::kRed);
+    AssertIndexColor(buffer, 1, Color::kRed);
+    AssertIndexColor(buffer, 2, Color::kBlue);
+    AssertIndexColor(buffer, 3, Color::kBlue);
+    AssertIndexColor(buffer, 4, Color::kBlack);
+    AssertIndexColor(buffer, 5, Color::kBlack);
+    AssertIndexColor(buffer, 6, Color::kBlack);
+    AssertIndexColor(buffer, 7, Color::kBlack);
+  }
+  {
+    now = 1ull;
+    SCOPED_TRACE(fmt::format("Time {}", now));
+
+    pattern.ApplyTo(buffer);
+
+    AssertIndexColor(buffer, 0, Color::kBlack);
+    AssertIndexColor(buffer, 1, Color::kRed);
+    AssertIndexColor(buffer, 2, Color::kRed);
+    AssertIndexColor(buffer, 3, Color::kBlue);
+    AssertIndexColor(buffer, 4, Color::kBlue);
+    AssertIndexColor(buffer, 5, Color::kBlack);
+    AssertIndexColor(buffer, 6, Color::kBlack);
+    AssertIndexColor(buffer, 7, Color::kBlack);
+  }
+  {
+    now = 2ull;
+    SCOPED_TRACE(fmt::format("Time {}", now));
+
+    pattern.ApplyTo(buffer);
+
+    AssertIndexColor(buffer, 0, Color::kBlack);
+    AssertIndexColor(buffer, 1, Color::kBlack);
+    AssertIndexColor(buffer, 2, Color::kRed);
+    AssertIndexColor(buffer, 3, Color::kRed);
+    AssertIndexColor(buffer, 4, Color::kBlue);
+    AssertIndexColor(buffer, 5, Color::kBlue);
+    AssertIndexColor(buffer, 6, Color::kBlack);
+    AssertIndexColor(buffer, 7, Color::kBlack);
+  }
+  {
+    now = 3ull;
+    SCOPED_TRACE(fmt::format("Time {}", now));
+
+    pattern.ApplyTo(buffer);
+
+    AssertIndexColor(buffer, 0, Color::kBlack);
+    AssertIndexColor(buffer, 1, Color::kBlack);
+    AssertIndexColor(buffer, 2, Color::kBlack);
+    AssertIndexColor(buffer, 3, Color::kRed);
+    AssertIndexColor(buffer, 4, Color::kRed);
+    AssertIndexColor(buffer, 5, Color::kBlue);
+    AssertIndexColor(buffer, 6, Color::kBlue);
+    AssertIndexColor(buffer, 7, Color::kBlack);
+  }
+
+  WPI_SetNowImpl(nullptr);  // cleanup
+}
+
+TEST(LEDPatternTest, AbsoluteScrollingMask) {
+  std::array<AddressableLED::LEDData, 8> buffer;
+
+  std::array<std::pair<double, Color>, 4> colorSteps{
+    std::pair{0.0, Color::kRed},
+    std::pair{0.25, Color::kBlue},
+    std::pair{0.5, Color::kYellow},
+    std::pair{0.75, Color::kGreen}
+  };
+  std::array<std::pair<double, Color>, 2> maskSteps{
+    std::pair{0, Color::kWhite},
+    std::pair{0.5, Color::kBlack}
+  };
+
+  auto pattern = LEDPattern::Steps(colorSteps).Mask(LEDPattern::Steps(maskSteps)).Reversed().ScrollAtAbsoluteSpeed(1_mps, 1_m);
+
+  pattern.ApplyTo(buffer);
+
+  static uint64_t now = 0ull;
+  WPI_SetNowImpl([] { return now; });
+
+  {
+    now = 0ull;  // start
+    SCOPED_TRACE(fmt::format("Time {}", now));
+
+    pattern.ApplyTo(buffer);
+
+    AssertIndexColor(buffer, 0, Color::kRed);
+    AssertIndexColor(buffer, 1, Color::kRed);
+    AssertIndexColor(buffer, 2, Color::kBlue);
+    AssertIndexColor(buffer, 3, Color::kBlue);
+    AssertIndexColor(buffer, 4, Color::kBlack);
+    AssertIndexColor(buffer, 5, Color::kBlack);
+    AssertIndexColor(buffer, 6, Color::kBlack);
+    AssertIndexColor(buffer, 7, Color::kBlack);
+  }
+  {
+    now = 1000000ull;
+    SCOPED_TRACE(fmt::format("Time {}", now));
+
+    pattern.ApplyTo(buffer);
+
+    AssertIndexColor(buffer, 0, Color::kBlack);
+    AssertIndexColor(buffer, 1, Color::kRed);
+    AssertIndexColor(buffer, 2, Color::kRed);
+    AssertIndexColor(buffer, 3, Color::kBlue);
+    AssertIndexColor(buffer, 4, Color::kBlue);
+    AssertIndexColor(buffer, 5, Color::kBlack);
+    AssertIndexColor(buffer, 6, Color::kBlack);
+    AssertIndexColor(buffer, 7, Color::kBlack);
+  }
+  {
+    now = 2000000ull;
+    SCOPED_TRACE(fmt::format("Time {}", now));
+
+    pattern.ApplyTo(buffer);
+
+    AssertIndexColor(buffer, 0, Color::kBlack);
+    AssertIndexColor(buffer, 1, Color::kBlack);
+    AssertIndexColor(buffer, 2, Color::kRed);
+    AssertIndexColor(buffer, 3, Color::kRed);
+    AssertIndexColor(buffer, 4, Color::kBlue);
+    AssertIndexColor(buffer, 5, Color::kBlue);
+    AssertIndexColor(buffer, 6, Color::kBlack);
+    AssertIndexColor(buffer, 7, Color::kBlack);
+  }
+  {
+    now = 3000000ull;
+    SCOPED_TRACE(fmt::format("Time {}", now));
+
+    pattern.ApplyTo(buffer);
+
+    AssertIndexColor(buffer, 0, Color::kBlack);
+    AssertIndexColor(buffer, 1, Color::kBlack);
+    AssertIndexColor(buffer, 2, Color::kBlack);
+    AssertIndexColor(buffer, 3, Color::kRed);
+    AssertIndexColor(buffer, 4, Color::kRed);
+    AssertIndexColor(buffer, 5, Color::kBlue);
+    AssertIndexColor(buffer, 6, Color::kBlue);
+    AssertIndexColor(buffer, 7, Color::kBlack);
+  }
+
+  WPI_SetNowImpl(nullptr);  // cleanup
+}
+
 void AssertIndexColor(std::span<AddressableLED::LEDData> data, int index,
                       Color color) {
   frc::Color8Bit color8bit{color};