#include "ButtonHandler.h"
#include "Config.h"
#include <stdlib.h>
#include "pico/stdlib.h"
#include "Adafruit_NeoPixel.hpp"
#include <cstring>
#include <memory>
#include <algorithm>
#include <cmath>
#define PIN_WS2812B 28
#define NUM_PIXELS 130
Adafruit_NeoPixel ws2812b(NUM_PIXELS, PIN_WS2812B, NEO_GRB + NEO_KHZ800);
uint8_t lerp(uint8_t a, uint8_t b, BytePercent t)
{
// Note: BytePercent multiplication by a negative value will round towards
// negative infinity instead of zero (which is not strictly correct), but
// should be pretty much imperceptible. More correct, but slower would be
// return a <= b ? a + t * uint8_t(b - a) : b - t * uint8_t(a - b);
return a + t * int16_t(b - a);
}
RGBPixel lerp(RGBPixel a, RGBPixel b, BytePercent t)
{
return {
lerp(a.red(), b.red(), t),
lerp(a.green(), b.green(), t),
lerp(a.blue(), b.blue(), t)
};
}
void fillColorTable(std::vector<RGBPixel>& lut, const PaletteEntry palette[])
{
int p = 0;
// TODO: Could be optimized by stepping through each palette entry and fill each section of the lut
BytePercent ptInterval(palette[1].t - palette[0].t);
for (uint16_t i=0; i<lut.size(); ++i)
{
BytePercent t(i * 255 / (lut.size() - 1));
while (t > palette[p+1].t)
{
++p;
ptInterval = palette[p+1].t - palette[p].t;
}
lut[i] = lerp(palette[p].c, palette[p+1].c, (t-palette[p].t) / ptInterval);
}
}
void setPixelColor(Adafruit_NeoPixel& ws2812b, const uint16_t n, const RGBPixel rgb)
{
ws2812b.setPixelColor(n, rgb.red(), rgb.green(), rgb.blue());
}
static bool changeAnims = false;
bool shouldChangeAnims()
{
if (changeAnims)
{
changeAnims = false;
return true;
}
return false;
}
void requestChangeAnims()
{
changeAnims = true;
}
void changeBrightness(bool first)
{
const int STEP_SIZE = 5;
static int dir;
if (first)
dir = -STEP_SIZE;
if (ws2812b.getBrightness() < STEP_SIZE)
dir = +STEP_SIZE;
else if (ws2812b.getBrightness() > 255-STEP_SIZE)
dir = -STEP_SIZE;
ws2812b.setBrightness(ws2812b.getBrightness() + dir);
}
uint32_t frameIntervalUS()
{
return 14000;
}
uint32_t speedFrameInterval = 0;
uint32_t speedAdjustedFrameIntervalUS()
{
return speedFrameInterval;
}
void changeSpeed(bool first)
{
const int STEP_SIZE = 3;
static int dir = STEP_SIZE;
if (Config.getSpeed() < STEP_SIZE+1)
dir = +STEP_SIZE;
else if (Config.getSpeed() > 255-STEP_SIZE)
dir = -STEP_SIZE;
Config.setSpeed(Config.getSpeed() + dir);
speedFrameInterval = frameIntervalUS() * Config.getSpeed() / 100;
}
ButtonHandler buttonHandler(requestChangeAnims, changeBrightness, changeSpeed, 100);
void buttonCallback(bool pressed, uint16_t timestampMS)
{
buttonHandler.handleButtonEvent(pressed, timestampMS);
}
// Naive debounced button implementation
bool buttonPressedState = false;
bool enableButtonCallback(repeating_timer_t *rt);
void buttonCallbackRaw(uint gpio, uint32_t events)
{
buttonCallback(buttonPressedState = (events & GPIO_IRQ_EDGE_FALL), to_ms_since_boot(get_absolute_time()));
// Debounce by ignoring any further interrupts for 100ms
// Would be better to continue to monitor interrupts and wait until 100ms after the
// last one to be sure we've settled.
gpio_set_irq_enabled(gpio, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, false);
static repeating_timer_t debounceTimer;
add_repeating_timer_ms(100, enableButtonCallback, NULL, &debounceTimer);
}
bool enableButtonCallback(repeating_timer_t *rt = nullptr)
{
bool pressed = gpio_get(1) == 0;
if (buttonPressedState != pressed)
buttonCallback(buttonPressedState = pressed, to_ms_since_boot(get_absolute_time()));
gpio_set_irq_enabled_with_callback(1, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &buttonCallbackRaw);
return false;
}
void linearBrightnessAdjustmentAndConvertToGamma(std::vector<RGBPixel>& buf, uint8_t* dest, uint8_t brightness, float g)
{
uint8_t* bytes = (uint8_t*)&buf[0];
uint16_t len = buf.size() * 3;
if (brightness != 255 || g != 1.0f)
{
static uint8_t lut[255];
static uint8_t lastBrightness;
static float lastGamma = 0;
if (lastBrightness != brightness || lastGamma != g)
{
const float brightInc = float(brightness) / 255.0f / 255.0f;
const float invG = 1.0f / g;
lut[0] = 0;
float v = brightInc;
for (uint8_t i=1; i!=0; ++i, v += brightInc)
lut[i] = 255.0f * pow(v, invG) + 0.5f;
// uint16_t scale = brightness + 1;
// for (uint16_t i=0, ii=0; i<256; ++i, ii += scale)
// lut[i] = gamma(ii >> 8u, g);
lastBrightness = brightness;
lastGamma = g;
}
for (int i = 0; i < len; ++i)
dest[i] = lut[bytes[i]];
}
else
{
memcpy(dest, bytes, len);
}
}
void calcFPS()
{
static uint32_t _lastFrameOutputTimeUS = 0;
static uint32_t _averageFrameTimeUS = 0;
uint32_t t = time_us_32();
uint32_t dt = t - _lastFrameOutputTimeUS;
if (_averageFrameTimeUS == 0) {
if (_lastFrameOutputTimeUS != 0)
_averageFrameTimeUS = dt;
}
else {
_averageFrameTimeUS = _averageFrameTimeUS * 15 / 16 + dt / 16;
static int i = 0;
if (++i % 256 == 0)
printf("FPS = %.1fHz\n", 1000000.f / _averageFrameTimeUS);
}
_lastFrameOutputTimeUS = t;
}
// All Drawer implementations loop their animations.
class Drawer
{
public:
virtual ~Drawer() = default;
// Move the animation forward. Speed is controlled by calling advanceTime() with
// an adjusted deltaTime with respect to the actual time passing by.
virtual void advanceTime(uint16_t deltaTimeUS) = 0;
// Set the animation time to a specific point in the loop.
virtual void setPosition(BytePercent percent) = 0;
// After advanceTime() has been called, this check will report if the implementation
// has reached the end of its animation loop and is about to loop to the beginning.
// Note that if draw() is called, this flag will be reset, the animation will start
// again from the beginning, adn the drawn pixels will reflect a state near the
// beginning of the loop.
virtual bool hasReachedEnd() const = 0;
// Set the current time to the very last position in the animation loop. The next
// call to draw() will draw the last possible point in the animation loop.
virtual void setToLastPosition() = 0;
// Set the current time to the very beginning of the animation loop. The next
// call to draw() will draw the first possible point in the animation loop. This
// is the same state as when the drawer is initially instantiated.
virtual void reset() = 0;
// Perform the actual drawing of the animation into the pixel buffer. This reflects
// the current time of the animation as determined by prior calls to advanceTime(),
// setPosition(), setToLastPosition(), and reset(). See documentation of
// hasReachedEnd() to understand exactly what is drawn when time advancing past the
// the threshold of the end of the animation loop.
virtual void draw(std::vector<RGBPixel>& buffer) = 0;
};
class SnakeDrawer : public Drawer
{
public:
SnakeDrawer(const PaletteEntry* palette, uint8_t length, uint16_t numPixels)
: _pixels(length), _numPixels(numPixels)
{
reset();
fillColorTable(_pixels, palette);
}
void reset()
{
_timeUS = 0;
}
void advanceTime(uint16_t deltaTimeUS)
{
_timeUS += deltaTimeUS;
}
void setPosition(BytePercent percent)
{
_timeUS = percent * uint16_t(_numPixels * 2) * US_PER_PIXEL;
}
bool hasReachedEnd() const
{
return _timeUS >= _numPixels * 2 * US_PER_PIXEL;
}
virtual void setToLastPosition()
{
_timeUS = _numPixels * 2 * US_PER_PIXEL;
}
void draw(std::vector<RGBPixel>& buffer)
{
// keep timeUS in the interval 0..2*numPixels*(scaling factor)
if (hasReachedEnd())
_timeUS -= _numPixels * 2 * US_PER_PIXEL;
const uint16_t i = _timeUS / US_PER_PIXEL; // i is in the range 0..2*numPixels
const uint16_t length = _pixels.size();
const int dir = i < _numPixels ? +1 : -1;
uint16_t head, mid, tail;
if (dir > 0)
{
head = i;
const uint16_t frontBodyLen = std::min(i, length);
mid = head - frontBodyLen;
tail = mid + length - frontBodyLen;
}
else
{
head = 2 * _numPixels - i - 1;
const uint16_t frontBodyLen = std::min(uint16_t(i - _numPixels), length);
mid = head + frontBodyLen;
tail = mid - length + frontBodyLen;
}
uint16_t si = 0;
for (uint16_t j=tail; j!=mid; j -= dir)
buffer[j] = _pixels[si++];
for (uint16_t j=mid; j!=head; j += dir)
buffer[j] = _pixels[si++];
}
private:
static constexpr uint16_t US_PER_PIXEL = 8 * 1024; // usecs it takes for the snake to advance 1 pixel
std::vector<RGBPixel> _pixels;
uint32_t _timeUS;
const uint16_t _numPixels;
};
class CommetDrawer : public Drawer
{
public:
CommetDrawer(const PaletteEntry* palette, uint8_t length, uint16_t numPixels)
: _pixels(length), _numPixels(numPixels)
{
reset();
fillColorTable(_pixels, palette);
}
void reset()
{
_timeUS = _pixels.size() * US_PER_PIXEL / 2;
}
void advanceTime(uint16_t deltaTimeUS)
{
_timeUS += deltaTimeUS;
}
void setPosition(BytePercent percent)
{
_timeUS = percent * uint16_t(_numPixels + _pixels.size()) * US_PER_PIXEL;
}
bool hasReachedEnd() const
{
return _timeUS >= (_numPixels + _pixels.size()) * US_PER_PIXEL;
}
virtual void setToLastPosition()
{
_timeUS = (_numPixels + _pixels.size()) * US_PER_PIXEL - 1;
}
void draw(std::vector<RGBPixel>& buffer)
{
const uint16_t length = _pixels.size();
// keep timeUS in the interval 0..(numPixels+length)*(scaling factor)
if (hasReachedEnd())
_timeUS -= (_numPixels + length) * US_PER_PIXEL;
const uint16_t i = _timeUS / US_PER_PIXEL; // i is in the range 0..(numPixels+length)
int16_t start = i - length;
uint16_t end = std::min(_numPixels, i);
uint8_t ci = 0;
if (start < 0)
{
ci = -start;
start = 0;
}
for (uint16_t j = start; j < end; ++j)
buffer[j] = _pixels[ci++];
}
private:
static constexpr uint16_t US_PER_PIXEL = 8 * 1024; // usecs it takes for the snake to advance 1 pixel
std::vector<RGBPixel> _pixels;
uint32_t _timeUS;
const uint16_t _numPixels;
};
class ContinuousCommetDrawer : public Drawer
{
public:
ContinuousCommetDrawer(const PaletteEntry* palette, uint8_t length, uint16_t numPixels)
: _pixels(length), _numPixels(numPixels)
{
reset();
fillColorTable(_pixels, palette);
}
void reset()
{
_timeUS = 0;
}
void advanceTime(uint16_t deltaTimeUS)
{
_timeUS += deltaTimeUS;
}
void setPosition(BytePercent percent)
{
_timeUS = percent * _numPixels * US_PER_PIXEL;
}
bool hasReachedEnd() const
{
return _timeUS >= _numPixels * US_PER_PIXEL;
}
virtual void setToLastPosition()
{
_timeUS = _numPixels * US_PER_PIXEL - 1;
}
void draw(std::vector<RGBPixel>& buffer)
{
// keep timeUS in the interval 0..numPixels*(scaling factor)
if (hasReachedEnd())
_timeUS -= _numPixels * US_PER_PIXEL;
const uint16_t i = _timeUS / US_PER_PIXEL; // i is in the range 0..numPixels
const uint16_t length = _pixels.size();
uint16_t ci = 0;
if (i >= length)
{
for (uint16_t j = i - length; j < i; ++j)
buffer[j] = _pixels[ci++];
}
else
{
for (uint16_t j = _numPixels - length + i; j < _numPixels; ++j)
buffer[j] = _pixels[ci++];
for (uint16_t j = 0; j < i; ++j)
buffer[j] = _pixels[ci++];
}
}
private:
static constexpr uint16_t US_PER_PIXEL = 8 * 1024; // usecs it takes for the snake to advance 1 pixel
std::vector<RGBPixel> _pixels;
uint32_t _timeUS;
const uint16_t _numPixels;
};
class WipeDrawer : public Drawer
{
public:
WipeDrawer(const RGBPixel color, uint16_t numPixels)
: _color(color), _numPixels(numPixels)
{
reset();
}
void reset()
{
_timeUS = 0;
}
void advanceTime(uint16_t deltaTimeUS)
{
_timeUS += deltaTimeUS;
}
void setPosition(BytePercent percent)
{
_timeUS = percent * _numPixels * US_PER_PIXEL;
}
bool hasReachedEnd() const
{
return _timeUS >= _numPixels * US_PER_PIXEL;
}
virtual void setToLastPosition()
{
_timeUS = _numPixels * US_PER_PIXEL - 1;
}
void draw(std::vector<RGBPixel>& buffer)
{
// keep timeUS in the interval 0..numPixels*(scaling factor)
if (hasReachedEnd())
_timeUS -= _numPixels * US_PER_PIXEL;
const uint16_t i = _timeUS / US_PER_PIXEL; // i is in the range 0..numPixels
for (uint16_t j=0; j<i; ++j)
buffer[j] = _color;
}
private:
static constexpr uint16_t US_PER_PIXEL = 8 * 1024; // usecs it takes for the snake to advance 1 pixel
uint32_t _timeUS;
RGBPixel _color;
const uint16_t _numPixels;
};
class WaveDrawer : public Drawer
{
public:
WaveDrawer(const PaletteEntry* palette, uint8_t length, uint16_t numPixels)
: _pixels(length), _length(length), _numPixels(numPixels)
{
reset();
// TODO: Support sine wave color table
fillColorTable(_pixels, palette);
}
void reset()
{
_timeUS = 0;
}
void advanceTime(uint16_t deltaTimeUS)
{
_timeUS += deltaTimeUS;
}
void setPosition(BytePercent percent)
{
_timeUS = percent * uint16_t((_pixels.size() - 1) * 2) * US_PER_PIXEL;
}
bool hasReachedEnd() const
{
return _timeUS >= (_pixels.size() - 1) * 2 * US_PER_PIXEL;
}
virtual void setToLastPosition()
{
_timeUS = (_pixels.size() - 1) * 2 * US_PER_PIXEL - 1;
}
void draw(std::vector<RGBPixel>& buffer)
{
const int16_t length = _pixels.size() - 1;
// keep timeUS in the interval 0..2*length*(scaling factor)
if (hasReachedEnd())
_timeUS -= length * 2 * US_PER_PIXEL;
const int16_t i = _timeUS / US_PER_PIXEL; // i is in the range 0.2*length
// TODO: Could be simpler and more optimal
int16_t start = i - 2 * length;
int16_t mid = std::min(int16_t(start + length), _numPixels);
int16_t end = std::min(int16_t(mid + length), _numPixels);
int16_t firstStart = std::max(start, int16_t(0));
int16_t firstMid = std::max(mid, int16_t(0));
uint16_t j=firstStart;
uint8_t wi = length - (firstMid - firstStart);
for (; j<firstMid; ++j)
buffer[j] = _pixels[wi++];
wi = end - firstMid;
for (; j<end; ++j)
buffer[j] = _pixels[wi--];
while (j<_numPixels)
{
end = std::min(int16_t(end + length), _numPixels);
wi = 0;
for (; j<end; ++j)
buffer[j] = _pixels[wi++];
end = std::min(int16_t(end + length), _numPixels);
wi = length;
for (; j<end; ++j)
buffer[j] = _pixels[wi--];
}
}
private:
static constexpr uint16_t US_PER_PIXEL = 8 * 1024; // usecs it takes for the snake to advance 1 pixel
std::vector<RGBPixel> _pixels;
uint32_t _timeUS;
const int16_t _numPixels;
uint8_t _length;
};
class FadeDrawer : public Drawer
{
public:
enum FadeType : uint8_t {FADE_IN, FADE_OUT};
FadeDrawer(const RGBPixel color, FadeType fadeType, uint16_t numPixels)
: _color(color), _fadeType(fadeType), _numPixels(numPixels)
{}
void reset()
{
_timeUS = US_PER_STEP;
}
void advanceTime(uint16_t deltaTimeUS)
{
_timeUS += deltaTimeUS;
}
void setPosition(BytePercent percent)
{
_timeUS = percent * US_PER_STEP * 256;
}
bool hasReachedEnd() const
{
return _timeUS >= US_PER_STEP * 256;
}
virtual void setToLastPosition()
{
_timeUS = US_PER_STEP * 256 - 1;
}
void draw(std::vector<RGBPixel>& buffer)
{
// keep timeUS in the interval 0..(numPixels+length)*(scaling factor)
if (hasReachedEnd())
_timeUS -= US_PER_STEP * 256;
uint8_t i = _timeUS / US_PER_STEP; // i is in the range 0..255 where 0 is special in that it indicates completion
if (_fadeType == FADE_OUT)
i = i == 0 ? 1 : 257-i;
RGBPixel c = i == 0 ? _color : RGBPixel((_color.red()*i) >> 8u, (_color.green()*i) >> 8u, (_color.blue()*i) >> 8u);
for (uint16_t j = 0; j < _numPixels; ++j)
buffer[j] = c;
}
private:
static constexpr uint16_t US_PER_STEP = 4 * 1024; // usecs it takes for the fade from zero to 100%
uint32_t _timeUS;
RGBPixel _color;
FadeType _fadeType;
const uint16_t _numPixels;
};
void snake(const PaletteEntry* palette, uint8_t length, bool (*shouldBreak)())
{
std::vector<RGBPixel> buf(ws2812b.numPixels());
SnakeDrawer drawer(palette, length, buf.size());
while (!shouldBreak())
{
memset(&buf[0], 0, buf.size() * sizeof(RGBPixel));
drawer.advanceTime(speedAdjustedFrameIntervalUS());
drawer.draw(buf);
linearBrightnessAdjustmentAndConvertToGamma(buf, ws2812b.getPixels(), ws2812b.getBrightness(), 2.6f);
ws2812b.show();
calcFPS();
sleep_ms(10);
}
}
void continousCommet(int count, const PaletteEntry* palette, uint8_t length, bool (*shouldBreak)())
{
std::vector<RGBPixel> buf(ws2812b.numPixels());
std::vector<ContinuousCommetDrawer> drawers;
for (int i=0; i<count; ++i)
{
drawers.emplace_back(ContinuousCommetDrawer(palette, length, buf.size()));
drawers.back().setPosition(255 * i / count);
}
while (!shouldBreak())
{
memset(&buf[0], 0, buf.size() * sizeof(RGBPixel));
for (int i=0; i<drawers.size(); ++i)
{
drawers[i].advanceTime(speedAdjustedFrameIntervalUS());
drawers[i].draw(buf);
}
linearBrightnessAdjustmentAndConvertToGamma(buf, ws2812b.getPixels(), ws2812b.getBrightness(), 2.6f);
ws2812b.show();
calcFPS();
sleep_ms(10);
}
}
void commet(const PaletteEntry* palette, uint8_t length, bool (*shouldBreak)())
{
std::vector<RGBPixel> buf(ws2812b.numPixels());
CommetDrawer drawer(palette, length, buf.size());
while (!shouldBreak())
{
memset(&buf[0], 0, buf.size() * sizeof(RGBPixel));
drawer.advanceTime(speedAdjustedFrameIntervalUS());
drawer.draw(buf);
linearBrightnessAdjustmentAndConvertToGamma(buf, ws2812b.getPixels(), ws2812b.getBrightness(), 2.6f);
ws2812b.show();
calcFPS();
sleep_ms(10);
}
}
void wave(const PaletteEntry* palette, uint8_t length, bool (*shouldBreak)())
{
std::vector<RGBPixel> buf(ws2812b.numPixels());
WaveDrawer drawer(palette, length, buf.size());
while (!shouldBreak())
{
drawer.advanceTime(speedAdjustedFrameIntervalUS());
drawer.draw(buf);
linearBrightnessAdjustmentAndConvertToGamma(buf, ws2812b.getPixels(), ws2812b.getBrightness(), 2.6f);
ws2812b.show();
calcFPS();
sleep_ms(10);
}
}
void fromBodyCore(RGBPixel color, bool (*shouldBreak)())
{
std::vector<RGBPixel> buf(ws2812b.numPixels());
std::vector<std::unique_ptr<Drawer>> drawers;
drawers.push_back(std::make_unique<FadeDrawer>(color, FadeDrawer::FADE_IN, buf.size()));
drawers.push_back(std::make_unique<FadeDrawer>(color, FadeDrawer::FADE_OUT, buf.size()));
drawers.push_back(std::make_unique<WipeDrawer>(color, buf.size()));
drawers.push_back(std::make_unique<FadeDrawer>(color, FadeDrawer::FADE_OUT, buf.size()));
int i = 0;
while (!shouldBreak())
{
drawers[i]->advanceTime(speedAdjustedFrameIntervalUS());
const bool isEnding = drawers[i]->hasReachedEnd();
if (isEnding)
drawers[i]->setToLastPosition();
drawers[i]->draw(buf);
if (isEnding)
{
drawers[i]->reset();
i = (i + 1) % drawers.size();
}
linearBrightnessAdjustmentAndConvertToGamma(buf, ws2812b.getPixels(), ws2812b.getBrightness(), 2.6f);
ws2812b.show();
calcFPS();
sleep_ms(10);
}
}
int main() {
stdio_init_all();
// Button initialization
gpio_init(1);
gpio_set_dir(1, GPIO_IN);
gpio_pull_up(1);
enableButtonCallback();
gpio_init(PIN_WS2812B);
gpio_set_dir(PIN_WS2812B, GPIO_OUT);
ws2812b.begin();
for (int mode=Config.getMode(); true; mode = (mode + 1) % Config.getModeCount())
{
Config.setMode(mode);
speedFrameInterval = frameIntervalUS() * Config.getSpeed() / 100;
switch (mode) {
case 0: snake(Config.getPalette(), Config.getCustomModeData(0), shouldChangeAnims); break;
case 1: continousCommet(3, Config.getPalette(), Config.getCustomModeData(0), shouldChangeAnims); break;
case 2: wave(Config.getPalette(), ws2812b.numPixels() / 2, shouldChangeAnims); break;
case 3: continousCommet(4, Config.getPalette(), Config.getCustomModeData(0), shouldChangeAnims); break;
case 4: fromBodyCore(Config.getPaletteColor(0), shouldChangeAnims); break;
}
}
}