ESP32 Fluid simulation on 16x16 Led Matrix

hello can you try using this code. there have been some optimizations made

// colors: 0 = Red, 32 = Orange, 64 = Yellow, 96 = Green, 128 = Aqua, 160 = Blue, 192 = Purple, 224 = Pink

#include <FastLED.h>
#include <Wire.h>
#include <MPU6050.h>
#include // For std::sort

// Pin definitions
#define LED_PIN 5
#define SDA_PIN 21
#define SCL_PIN 22
#define BUTTON_PIN 4 // Button for color switching

#define NUM_LEDS 256
#define MATRIX_WIDTH 16
#define MATRIX_HEIGHT 16
#define FLUID_PARTICLES 64
#define BRIGHTNESS 30
#define NUM_COLORS 3 // Number of color options

// Structures
struct Vector2D {
float x;
float y;
};

struct Particle {
Vector2D position;
Vector2D velocity;
};

// Global variables
CRGB leds[NUM_LEDS];
MPU6050 mpu;
Particle particles[FLUID_PARTICLES];
Vector2D acceleration = {0, 0};

// Color switching variables
uint8_t currentColorIndex = 0;
unsigned long lastDebounceTime = 0;
const unsigned long debounceDelay = 200;

// Define the colors (you can change these hue values)
const uint8_t COLORS[NUM_COLORS] = {
160, // Blue
0, // Red
96 // Green
};

// Mutex for synchronization (ESP32 style)
portMUX_TYPE dataMux = portMUX_INITIALIZER_UNLOCKED;

// Constants for physics
const float GRAVITY = 0.08f;
const float DAMPING = 0.92f;
const float MAX_VELOCITY = 0.6f;

// Forward declarations
void initMPU6050();
void initLEDs();
void initParticles();
void updateParticles();
void drawParticles();
void MPUTask(void *parameter);
void LEDTask(void *parameter);
void checkButton();

// Function to convert x,y coordinates to LED index
inline int xy(int x, int y) {
x = constrain(x, 0, MATRIX_WIDTH - 1);
y = constrain(y, 0, MATRIX_HEIGHT - 1);
// Zigzag mapping
return (y & 1) ? (y * MATRIX_WIDTH + (MATRIX_WIDTH - 1 - x)) : (y * MATRIX_WIDTH + x);
}

void checkButton() {
static bool lastButtonState = HIGH;
bool buttonState = digitalRead(BUTTON_PIN);

if (buttonState == LOW && lastButtonState == HIGH) {
    if (millis() - lastDebounceTime > debounceDelay) {
        currentColorIndex = (currentColorIndex + 1) % NUM_COLORS;
        lastDebounceTime = millis();
    }
}
lastButtonState = buttonState;

}

void drawParticles() {
FastLED.clear();

// Keep track of which (x,y) cells are occupied
static bool occupied[MATRIX_WIDTH][MATRIX_HEIGHT];
memset(occupied, false, sizeof(occupied));

// Sort the particles by "y * width + x" so we draw them in ascending order
// (or any other criterion you want). This helps ensure consistent layering.
std::sort(particles, particles + FLUID_PARTICLES, [&](const Particle &a, const Particle &b){
    float posA = a.position.y * MATRIX_WIDTH + a.position.x;
    float posB = b.position.y * MATRIX_WIDTH + b.position.x;
    return posA < posB;
});

// Now draw the particles in that order
for (int i = 0; i < FLUID_PARTICLES; i++) {
    int x = round(particles[i].position.x);
    int y = round(particles[i].position.y);

    // Constrain coordinates
    x = constrain(x, 0, MATRIX_WIDTH - 1);
    y = constrain(y, 0, MATRIX_HEIGHT - 1);

    if (!occupied[x][y]) {
        int index = xy(x, y);
        if (index >= 0 && index < NUM_LEDS) {
            // Compute brightness based on velocity magnitude
            float vx = particles[i].velocity.x;
            float vy = particles[i].velocity.y;
            float speed = sqrtf(vx*vx + vy*vy);

            // Make it glow more if faster
            uint8_t hue = COLORS[currentColorIndex];
            uint8_t sat = 255;
            uint8_t val = constrain((int)(180 + speed * 50), 180, 255);
            leds[index] = CHSV(hue, sat, val);

            occupied[x][y] = true;
        }
    }
    else {
        // Try to place the particle near its position if (x,y) is already occupied
        bool placed = false;
        for (int r = 1; r < 3 && !placed; r++) {
            for (int dx = -r; dx <= r && !placed; dx++) {
                for (int dy = -r; dy <= r && !placed; dy++) {
                    if (abs(dx) + abs(dy) == r) {
                        int newX = x + dx;
                        int newY = y + dy;
                        if (newX >= 0 && newX < MATRIX_WIDTH &&
                            newY >= 0 && newY < MATRIX_HEIGHT &&
                            !occupied[newX][newY]) 
                        {
                            int index = xy(newX, newY);
                            if (index >= 0 && index < NUM_LEDS) {
                                leds[index] = CHSV(COLORS[currentColorIndex], 255, 180);
                                occupied[newX][newY] = true;
                                placed = true;
                            }
                        }
                    }
                }
            }
        }
    }
}

FastLED.show();

}

void updateParticles() {
// Safely read current accel from shared variable
Vector2D currentAccel;
portENTER_CRITICAL(&dataMux);
currentAccel = acceleration;
portEXIT_CRITICAL(&dataMux);

// Scale it down a bit so it feels "softer"
currentAccel.x *= 0.3f;
currentAccel.y *= 0.3f;

// Update positions and velocities
for (int i = 0; i < FLUID_PARTICLES; i++) {
    Particle &p = particles[i];

    p.velocity.x = p.velocity.x * 0.9f + (currentAccel.x * GRAVITY);
    p.velocity.y = p.velocity.y * 0.9f + (currentAccel.y * GRAVITY);

    p.velocity.x = constrain(p.velocity.x, -MAX_VELOCITY, MAX_VELOCITY);
    p.velocity.y = constrain(p.velocity.y, -MAX_VELOCITY, MAX_VELOCITY);

    float newX = p.position.x + p.velocity.x;
    float newY = p.position.y + p.velocity.y;

    // Bounce off walls with damping
    if (newX < 0.0f) {
        newX = 0.0f;
        p.velocity.x = fabsf(p.velocity.x) * DAMPING;
    }
    else if (newX >= (MATRIX_WIDTH - 1)) {
        newX = MATRIX_WIDTH - 1;
        p.velocity.x = -fabsf(p.velocity.x) * DAMPING;
    }

    if (newY < 0.0f) {
        newY = 0.0f;
        p.velocity.y = fabsf(p.velocity.y) * DAMPING;
    }
    else if (newY >= (MATRIX_HEIGHT - 1)) {
        newY = MATRIX_HEIGHT - 1;
        p.velocity.y = -fabsf(p.velocity.y) * DAMPING;
    }

    p.position.x = newX;
    p.position.y = newY;

    // Slow the particles slightly
    p.velocity.x *= 0.95f;
    p.velocity.y *= 0.95f;
}

// Very simple collision/repulsion: O(N^2) for 64 particles is still fine.
for (int i = 0; i < FLUID_PARTICLES; i++) {
    Particle &pi = particles[i];
    for (int j = i + 1; j < FLUID_PARTICLES; j++) {
        Particle &pj = particles[j];

        float dx = pj.position.x - pi.position.x;
        float dy = pj.position.y - pi.position.y;
        float distSq = dx*dx + dy*dy;

        // If they are within a small range, push them apart
        if (distSq < 1.0f && distSq > 0.0f) {
            float distance = sqrtf(distSq);
            // Normalize (dx, dy)
            dx /= distance;
            dy /= distance;

            // Repulsion
            float repulsion = 0.3f;  // tweak to taste
            pi.position.x -= dx * 0.5f * repulsion;
            pi.position.y -= dy * 0.5f * repulsion;
            pj.position.x += dx * 0.5f * repulsion;
            pj.position.y += dy * 0.5f * repulsion;

            // Average out velocity so they don't explode outward
            Vector2D avgVel = {
                (pi.velocity.x + pj.velocity.x) * 0.5f,
                (pi.velocity.y + pj.velocity.y) * 0.5f
            };
            pi.velocity = avgVel;
            pj.velocity = avgVel;
        }
    }
}

}

void initMPU6050() {
Serial.println(“Initializing MPU6050…”);
mpu.initialize();

if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed!");
    while (true) { delay(100); }
}

// ±2g range for accelerometer
mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
Serial.println("MPU6050 initialized");

}

void initLEDs() {
Serial.println(“Initializing LEDs…”);
FastLED.addLeds<WS2812B, LED_PIN, GRB>(leds, NUM_LEDS);
FastLED.setBrightness(BRIGHTNESS);
FastLED.clear(true);
Serial.println(“LEDs initialized”);
}

void initParticles() {
Serial.println(“Initializing particles…”);
int index = 0;

// Place the particles near the bottom rows
for (int y = MATRIX_HEIGHT - 4; y < MATRIX_HEIGHT; y++) {
    for (int x = 0; x < MATRIX_WIDTH && index < FLUID_PARTICLES; x++) {
        particles[index].position = { (float)x, (float)y };
        particles[index].velocity = { 0.0f, 0.0f };
        index++;
    }
}
Serial.printf("Total particles initialized: %d\n", index);

}

// Task that reads from the MPU and updates global acceleration
void MPUTask(void *parameter) {
while (true) {
int16_t ax, ay, az;
mpu.getAcceleration(&ax, &ay, &az);

    portENTER_CRITICAL(&dataMux);
    // Convert raw readings to ~[-2, 2] range and constrain to [-1, 1]
    acceleration.x = -constrain(ax / 16384.0f, -1.0f, 1.0f);
    acceleration.y =  constrain(ay / 16384.0f, -1.0f, 1.0f);
    portEXIT_CRITICAL(&dataMux);

    // 10ms delay is fine for reading the accelerometer
    vTaskDelay(pdMS_TO_TICKS(10));
}

}

// Task that updates and draws the particles
void LEDTask(void *parameter) {
TickType_t xLastWakeTime = xTaskGetTickCount();
const TickType_t xFrequency = pdMS_TO_TICKS(16); // ~60 FPS

while (true) {
    checkButton();
    updateParticles();
    drawParticles();

    // Wait until the next frame
    vTaskDelayUntil(&xLastWakeTime, xFrequency);
}

}

void setup() {
Serial.begin(115200);
delay(1000);
Serial.println(“Starting initialization…”);

pinMode(BUTTON_PIN, INPUT_PULLUP);

Wire.begin(SDA_PIN, SCL_PIN);
Wire.setClock(400000);

initMPU6050();
initLEDs();
initParticles();

// Create tasks on separate cores (ESP32)
xTaskCreatePinnedToCore(
    MPUTask,
    "MPUTask",
    4096,
    NULL,
    2,
    NULL,
    0
);

xTaskCreatePinnedToCore(
    LEDTask,
    "LEDTask",
    4096,
    NULL,
    1,
    NULL,
    1
);

Serial.println("Setup complete");

}

void loop() {
// Nothing here, tasks do all the work
vTaskDelete(NULL);
}