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

// ─── MAX7219 ──────────────────────────────────────────────────────────────────
LedControl lc = LedControl(11, 13, 10, 1);

// ─── MPU6050 ──────────────────────────────────────────────────────────────────
#define MPU_ADDR  0x68

// ─── Config ───────────────────────────────────────────────────────────────────
#define GRID_SIZE    8
#define NUM_GRAINS   10     // 1–64
#define TILT_THRESH  1500   // raise if too jittery, lower if sluggish
#define UPDATE_MS    50

// ─── State ────────────────────────────────────────────────────────────────────
struct Grain { int8_t x, y; };
Grain grains[NUM_GRAINS];
bool  grid[GRID_SIZE][GRID_SIZE];

int16_t ax, ay, az;
bool    mpuOK = false;

// ══════════════════════════════════════════════════════════════════════════════
void setup() {
  Serial.begin(9600);

  // --- MAX7219 init ---
  lc.shutdown(0, false);
  lc.setIntensity(0, 6);
  lc.clearDisplay(0);
  Serial.println("MAX7219 ready");

  // --- MPU6050 init with existence check ---
  Wire.begin();
  Wire.beginTransmission(MPU_ADDR);
  uint8_t err = Wire.endTransmission();

  if (err == 0) {
    // Wake up MPU6050
    Wire.beginTransmission(MPU_ADDR);
    Wire.write(0x6B);  // PWR_MGMT_1
    Wire.write(0x00);  // clear sleep bit
    Wire.endTransmission(true);
    mpuOK = true;
    Serial.println("MPU6050 ready");
  } else {
    Serial.print("MPU6050 NOT found! I2C error: ");
    Serial.println(err);
    Serial.println("Running with simulated gravity (tilting right)...");
  }

  // --- Place grains ---
  randomSeed(analogRead(A0));
  initGrains();

  // --- Force immediate render so you see grains right away ---
  renderMatrix();
  Serial.print(NUM_GRAINS);
  Serial.println(" grains placed, simulation started.");
}

// ══════════════════════════════════════════════════════════════════════════════
void loop() {
  static uint32_t lastUpdate = 0;

  if (millis() - lastUpdate >= UPDATE_MS) {
    lastUpdate = millis();

    if (mpuOK) {
      readMPU();
    } else {
      // Simulate a slight right tilt so sand still moves without MPU
      ax = 3000; ay = 0; az = 0;
    }

    updateSand();
    renderMatrix();
  }
}

// ─── MPU6050 Read ─────────────────────────────────────────────────────────────
void readMPU() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);  // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom((uint8_t)MPU_ADDR, (uint8_t)6, (uint8_t)true);

  if (Wire.available() >= 6) {
    ax = (Wire.read() << 8) | Wire.read();
    ay = (Wire.read() << 8) | Wire.read();
    az = (Wire.read() << 8) | Wire.read();
  }
}

// ─── Init Grains ──────────────────────────────────────────────────────────────
void initGrains() {
  memset(grid, 0, sizeof(grid));

  for (uint8_t i = 0; i < NUM_GRAINS; i++) {
    uint8_t attempts = 0;
    do {
      grains[i].x = random(0, GRID_SIZE);
      grains[i].y = random(0, GRID_SIZE);
      attempts++;
      if (attempts > 100) break; // safety escape
    } while (grid[grains[i].x][grains[i].y]);

    grid[grains[i].x][grains[i].y] = true;
  }
}

// ─── Sand Physics ─────────────────────────────────────────────────────────────
void updateSand() {
  int8_t dx = 0, dy = 0;

  // Map accelerometer to gravity direction
  if      (ax >  TILT_THRESH)  dx =  1;   // tilt right
  else if (ax < -TILT_THRESH)  dx = -1;   // tilt left
  if      (ay >  TILT_THRESH)  dy = -1;   // tilt forward
  else if (ay < -TILT_THRESH)  dy =  1;   // tilt back

  // Default: fall downward if flat
  if (dx == 0 && dy == 0) dy = 1;

  // Shuffle order to remove sweep bias
  for (uint8_t i = 0; i < NUM_GRAINS - 1; i++) {
    uint8_t j = i + random(0, NUM_GRAINS - i);
    Grain tmp  = grains[i];
    grains[i]  = grains[j];
    grains[j]  = tmp;
  }

  for (uint8_t i = 0; i < NUM_GRAINS; i++) {
    int8_t cx = grains[i].x;
    int8_t cy = grains[i].y;

    // --- Try primary direction ---
    int8_t nx = cx + dx;
    int8_t ny = cy + dy;

    if (inBounds(nx, ny) && !grid[nx][ny]) {
      grid[cx][cy] = false;
      grains[i].x  = nx;
      grains[i].y  = ny;
      grid[nx][ny] = true;
      continue;
    }

    // --- Try diagonals (slide around obstacles) ---
    // Perpendicular to gravity direction
    int8_t d1x = cx + dx - dy,  d1y = cy + dy + dx;
    int8_t d2x = cx + dx + dy,  d2y = cy + dy - dx;

    bool c1 = inBounds(d1x, d1y) && !grid[d1x][d1y];
    bool c2 = inBounds(d2x, d2y) && !grid[d2x][d2y];

    if (c1 && c2) {
      if (random(2)) { c1 = false; }  // randomly pick one
      else           { c2 = false; }
    }

    if (c1) {
      grid[cx][cy]   = false;
      grains[i].x    = d1x;
      grains[i].y    = d1y;
      grid[d1x][d1y] = true;
    } else if (c2) {
      grid[cx][cy]   = false;
      grains[i].x    = d2x;
      grains[i].y    = d2y;
      grid[d2x][d2y] = true;
    }
    // else: grain stays
  }
}

// ─── Render ───────────────────────────────────────────────────────────────────
void renderMatrix() {
  for (uint8_t row = 0; row < GRID_SIZE; row++) {
    byte b = 0;
    for (uint8_t col = 0; col < GRID_SIZE; col++) {
      if (grid[col][row]) b |= (1 << (7 - col));
    }
    lc.setRow(0, row, b);
  }
}

// ─── Helper ───────────────────────────────────────────────────────────────────
bool inBounds(int8_t x, int8_t y) {
  return (x >= 0 && x < GRID_SIZE && y >= 0 && y < GRID_SIZE);
}