/* 
  Project 6: Tilt Circle Game – ESP32-S3 RGB LED Matrix (Waveshare)

  This sketch reads tilt from the QMI8658C IMU and smoothly moves a dot 
  on the 8×8 RGB LED matrix based on board orientation.

  ▶️ Video Tutorial:
  https://youtu.be/JKLuYrRcLMI

  📚⬇️ Resources & Code Page:
  https://robojax.com/RJT829

  QMI8658_RGB_2
*/

#include <Arduino.h>
#include <math.h>

#include <Adafruit_GFX.h>
#include <Adafruit_NeoMatrix.h>
#include <Adafruit_NeoPixel.h>

#include <QMI8658.h>   // by Lahav Gahali

// -------- LED MATRIX SETUP --------
#define MATRIX_PIN    14
#define MATRIX_WIDTH  8
#define MATRIX_HEIGHT 8

// Buzzer pin – change this to your actual buzzer GPIO.
const int BUZZER_PIN = 6;   // TODO: set to your buzzer pin

// Matrix brightness (0–255)
const uint8_t MATRIX_BRIGHTNESS = 10;

Adafruit_NeoMatrix matrix = Adafruit_NeoMatrix(
  MATRIX_WIDTH, MATRIX_HEIGHT, MATRIX_PIN,
  NEO_MATRIX_TOP + NEO_MATRIX_LEFT +
  NEO_MATRIX_ROWS + NEO_MATRIX_PROGRESSIVE,
  NEO_GRB + NEO_KHZ800
);

// -------- IMU SETUP --------
QMI8658 imu;

// -------- GAME CONFIG --------

// How often the dot is allowed to move (ms).
// Bigger = slower movement.
const uint16_t MOVE_INTERVAL_MS = 150;   // try 120–250

// How much tilt (m/s^2) before the dot moves.
// Increase if it feels too sensitive.
const float ACC_TILT_THRESHOLD = 2.0f;   // about ~0.2 g

// Dot base color (RGB)
const uint8_t DOT_R = 255;
const uint8_t DOT_G = 255;
const uint8_t DOT_B = 255;


// Circle geometry (centered on 8x8)
const float CIRCLE_CENTER_X = (MATRIX_WIDTH - 1) / 2.0f;   // 3.5
const float CIRCLE_CENTER_Y = (MATRIX_HEIGHT - 1) / 2.0f;  // 3.5
const float CIRCLE_RADIUS   = 3.0f;
const float CIRCLE_THICKNESS = 0.8f; // +- thickness around radius

// -------- GAME STATE --------

// Dot position on the 8x8 grid (0..7)
int dotX = 3;
int dotY = 3;

// Colors (16-bit NeoMatrix colors)
uint16_t dotColor;
uint16_t circleColor;

// To detect “just touched circle” vs “still on circle”
bool wasOnCircle = false;

// Timer for rate-limiting movement
unsigned long lastMoveTime = 0;

// -------- HELPER FUNCTIONS --------

// Classic NeoPixel color wheel (0-255 -> rainbow)
uint16_t wheel(byte pos) {
  if (pos < 85) {
    return matrix.Color(pos * 3, 255 - pos * 3, 0);
  } else if (pos < 170) {
    pos -= 85;
    return matrix.Color(255 - pos * 3, 0, pos * 3);
  } else {
    pos -= 170;
    return matrix.Color(0, pos * 3, 255 - pos * 3);
  }
}

// Is a given pixel approximately on the circle?
bool isOnCircle(int x, int y) {
  float dx = x - CIRCLE_CENTER_X;
  float dy = y - CIRCLE_CENTER_Y;
  float d2 = dx * dx + dy * dy;

  float rMin = CIRCLE_RADIUS - CIRCLE_THICKNESS;
  float rMax = CIRCLE_RADIUS + CIRCLE_THICKNESS;

  return (d2 >= rMin * rMin) && (d2 <= rMax * rMax);
}

// Draw circle + dot
void drawScene() {
  matrix.fillScreen(0);

  // Draw circle
  for (int y = 0; y < MATRIX_HEIGHT; y++) {
    for (int x = 0; x < MATRIX_WIDTH; x++) {
      if (isOnCircle(x, y)) {
        matrix.drawPixel(x, y, circleColor);
      }
    }
  }

  // Draw dot (on top)
  matrix.drawPixel(dotX, dotY, dotColor);

  matrix.show();
}

// Simple blocking beep (short)
void buzzOnce() {
  digitalWrite(BUZZER_PIN, HIGH);
  delay(40);
  digitalWrite(BUZZER_PIN, LOW);
}

// Use accelerometer to decide dot movement
void updateDotFromTilt(float ax, float ay) {
  unsigned long now = millis();
  if (now - lastMoveTime < MOVE_INTERVAL_MS) {
    return; // too soon, wait
  }

  int dx = 0;
  int dy = 0;

  // On this board, Y tilt feels like "left/right" on the matrix,
  // and X tilt feels like "up/down" → so we swap.

  // --- Horizontal movement from AY (tilt left/right) ---
  if (ay > ACC_TILT_THRESHOLD) {
    dx = 1;   // tilt to the right → move dot to the right
  } else if (ay < -ACC_TILT_THRESHOLD) {
    dx = -1;  // tilt to the left → move dot to the left
  }

  // --- Vertical movement from AX (tilt forward/back) ---
  // This was inverted. We flip the signs:
  // ax >  threshold  = tilt "forward" (away)  → move dot UP  (dy = -1)
  // ax < -threshold  = tilt "back"    (toward)→ move dot DOWN(dy =  1)
  if (ax > ACC_TILT_THRESHOLD) {
    dy = -1;  // was +1 before
  } else if (ax < -ACC_TILT_THRESHOLD) {
    dy = 1;   // was -1 before
  }

  if (dx != 0 || dy != 0) {
    dotX = constrain(dotX + dx, 0, MATRIX_WIDTH  - 1);
    dotY = constrain(dotY + dy, 0, MATRIX_HEIGHT - 1);
    lastMoveTime = now;
  }
}


// -------- SETUP & LOOP --------

void setup() {
  Serial.begin(115200);
  delay(100);

  // Matrix init
  matrix.begin();
  matrix.setBrightness(MATRIX_BRIGHTNESS);
  matrix.fillScreen(0);
  matrix.show();

  // Buzzer init
  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, LOW);

  // IMU init
if (!imu.begin(11, 12)) {
    Serial.println("Failed to initialize QMI8658!");
    while (1) {
      delay(100);
    }
  }

  // Use m/s^2 and dps (optional, but nice)
  imu.setAccelUnit_mps2(true);
  imu.setGyroUnit_dps(true);
  imu.setDisplayPrecision(3);

  Serial.println("QMI8658 initialized.");

  // Game initial state
  randomSeed((uint32_t)micros());

  dotX = MATRIX_WIDTH  / 2;
  dotY = MATRIX_HEIGHT / 2;

  dotColor    = matrix.Color(DOT_R, DOT_G, DOT_B);
  circleColor = wheel(random(256));

  drawScene();
}

void loop() {
  // Read accelerometer
  float ax, ay, az;
  if (imu.readAccelMPS2(ax, ay, az)) {
    // Debug if needed:
    // Serial.print("AX: "); Serial.print(ax);
    // Serial.print("  AY: "); Serial.print(ay);
    // Serial.print("  AZ: "); Serial.println(az);

    updateDotFromTilt(ax, ay);
  }

  // Check collision with circle
  bool onCircle = isOnCircle(dotX, dotY);
  if (onCircle && !wasOnCircle) {
    // Just touched circle: change color + beep
    circleColor = wheel(random(256));
    buzzOnce();
  }
  wasOnCircle = onCircle;

  // Redraw
  drawScene();

  // Small delay so we don’t hammer I2C too hard
  delay(10);
}