#include <Wire.h>
#include <MPU6050.h>
#include <LedControl.h>

// MPU6050 instance
MPU6050 mpu;

// LedControl instance for 8x8 dot matrix
LedControl lc = LedControl(11, 13, 7, 1); // (DIN, CLK, LOAD, number of displays)

// Variables to hold accelerometer data
int16_t ax, ay, az;
int16_t gx, gy, gz;

// Smoothed accelerometer data
float smoothedAx = 0, smoothedAy = 0;

// Number of particles
const int numParticles = 32;

// Array to hold positions of particles
int particlesX[numParticles];
int particlesY[numParticles];

// 8x8 grid to track particle positions for collision detection
bool grid[8][8];

// Initialize particles at random positions
void initializeParticles() {
  for (int i = 0; i < numParticles; i++) {
    particlesX[i] = random(0, 8);
    particlesY[i] = random(0, 8);
  }
}

// Smooth accelerometer data using exponential moving average
void smoothAccelerometerData(int16_t rawAx, int16_t rawAy) {
  const float alpha = 0.1; // Smoothing factor
  smoothedAx = alpha * rawAx + (1 - alpha) * smoothedAx;
  smoothedAy = alpha * rawAy + (1 - alpha) * smoothedAy;
}

// Check for collision and adjust position if necessary
void resolveCollision(int index) {
  for (int i = 0; i < numParticles; i++) {
    if (i != index && particlesX[i] == particlesX[index] && particlesY[i] == particlesY[index]) {
      // Move the particle to a nearby free position
      particlesX[index] = (particlesX[index] + 1) % 8;
      particlesY[index] = (particlesY[index] + 1) % 8;
      // Check again for collisions at the new position
      resolveCollision(index);
    }
  }
}

// Handle particle interactions
void handleParticleInteractions() {
  // Clear the grid
  memset(grid, false, sizeof(grid));
  
  for (int i = 0; i < numParticles; i++) {
    // Mark the grid with particle positions
    grid[particlesX[i]][particlesY[i]] = true;
  }

  for (int i = 0; i < numParticles; i++) {
    // If collision detected, find new position
    if (grid[particlesX[i]][particlesY[i]]) {
      resolveCollision(i);
      grid[particlesX[i]][particlesY[i]] = true; // Update grid with new position
    }
  }
}

void setup() {
  // Initialize serial communication
  Serial.begin(9600);

  // Initialize MPU6050
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }

  // Initialize dot matrix
  lc.shutdown(0, false); // Wake up displays
  lc.setIntensity(0, 8); // Set intensity levels
  lc.clearDisplay(0);    // Clear display register

  // Initialize particles
  initializeParticles();
}

void loop() {
  // Read accelerometer data
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Smooth accelerometer data
  smoothAccelerometerData(ax, ay);

  // Clear previous positions
  lc.clearDisplay(0);

  // Update particle positions based on smoothed accelerometer data
  for (int i = 0; i < numParticles; i++) {
    if (smoothedAx > 5000 && particlesX[i] < 7) particlesX[i]++;
    if (smoothedAx < -5000 && particlesX[i] > 0) particlesX[i]--;
    if (smoothedAy > 5000 && particlesY[i] < 7) particlesY[i]++;
    if (smoothedAy < -5000 && particlesY[i] > 0) particlesY[i]--;

    // Ensure particles stay within the grid
    particlesX[i] = constrain(particlesX[i], 0, 7);
    particlesY[i] = constrain(particlesY[i], 0, 7);
  }

  // Handle particle interactions and resolve collisions
  handleParticleInteractions();

  // Set new positions
  for (int i = 0; i < numParticles; i++) {
    lc.setLed(0, particlesX[i], particlesY[i], true);
  }

  // Delay for a short while to avoid too fast movement
  delay(100);
}