/*
 * پروژه ۴: نقطه مایل - ماتریس LED RGB ESP32-S3 (Waveshare)
 * 
 * این طرح انحراف را از IMU QMI8658C خوانده و به آرامی یک نقطه را بر اساس جهت‌گیری برد بر روی ماتریس LED RGB ۸×۸ حرکت می‌دهد.
 * 
 * ▶️ ویدیو آموزشی:
 * https://youtu.be/JKLuYrRcLMI
 * 
 * 📚⬇️ منابع و صفحه شِفر (کود):
 * 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> // توسط لَهَاو گَهالی

 // -------- تنظیم ماتریس LED --------
#define MATRIX_PIN    14
#define MATRIX_WIDTH  8
#define MATRIX_HEIGHT 8

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

 // -------- تنظیم IMU QMI8658 --------
QMI8658 imu;
QMI8658_Data imuData;

 // -------- تنظیمات کاربر --------

 // حقیقت -> نقطه در طرف مقابل: USB↔OUSB، 34↔15
 // کاذب -> نقطه در همان سمت با بالا
bool useOppositeMapping = false;

 // رنگ نقطه (۰-۲۵۵ هر کدام)
uint8_t dotRed   = 0;
uint8_t dotGreen = 100;
uint8_t dotBlue  = 0;

 // کنارهای تخته
enum Side {
  SIDE_CENTER = 0,
  SIDE_USB,
  SIDE_OUSB,
  SIDE_15,
  SIDE_34
};

 // موقعیت نقطه نرم (در مختصات پیکسل، اما به صورت اعشاری برای تسهیل نگهداری شده است)
float dotPosX = 3.0f; // از مرکز شروع کنید
float dotPosY = 3.0f;

 // عامل صاف‌سازی: کوچکتر = حرکت کندتر (0.1 بسیار کند، 0.5 سریع‌تر)
const float dotSmooth = 0.25f;


bool isFlat = false;

const char* sideName(Side s) {
  switch (s) {
    case SIDE_CENTER: return "CENTER";
    case SIDE_USB:    return "USB";
    case SIDE_OUSB:   return "OUSB";
    case SIDE_15:     return "15";
    case SIDE_34:     return "34";
    default:          return "?";
  }
}

 // تشخیص دهید که کدام سمت بالا است با استفاده از محورهای کالیبره شده:
 // +X = یو اس بی، -X = او یو اس بی، +Y = ۳۴، -Y = ۱۵
Side detectSideUp(float ax_g, float ay_g, float az_g) {
 // تشخیص مسطح
  const float flatThreshXY = 0.15f;
  const float flatThreshZ  = 0.15f;

  if (fabs(ax_g) < flatThreshXY &&
      fabs(ay_g) < flatThreshXY &&
      fabs(az_g - 1.0f) < flatThreshZ) {
    isFlat = true;
    return SIDE_CENTER;
  }
  isFlat = false;

 // آستانه‌ها برای گفتن "این محور واقعاً کج است"
  const float tiltThreshY = 0.5f;
  const float tiltThreshX = 0.5f;

 // محور Y را برای ۱۵ / ۳۴ در صورتی که به وضوح کج باشد ترجیح دهید.
  if (fabs(ay_g) >= tiltThreshY) {
    if (ay_g > 0) {
      return SIDE_15; // +Y = ۳۴ بالا
    } else {
      return SIDE_34; // -Y = 15 بالا
    }
  }

 // ۲) در غیر این صورت، محور X را برای USB / OUSB بررسی کنید.
  if (fabs(ax_g) >= tiltThreshX) {
    if (ax_g > 0) {
      return SIDE_USB; // +X = USB بالا
    } else {
      return SIDE_OUSB; // -X = OUSB بالا
    }
  }

 // اگر هیچ چیزی به شدت کج نشده است، فقط آن را مرکز بنامید.
  return SIDE_CENTER;
}

 // نقشه‌ای از سمت بالا به جایی که نقطه باید برود
Side dotSideFromUpSide(Side upSide) {
  switch (upSide) {
    case SIDE_USB:
      return useOppositeMapping ? SIDE_OUSB : SIDE_USB;

    case SIDE_OUSB:
      return useOppositeMapping ? SIDE_USB : SIDE_OUSB;

    case SIDE_34:
      return useOppositeMapping ? SIDE_15 : SIDE_34;

    case SIDE_15:
      return useOppositeMapping ? SIDE_34 : SIDE_15;

    case SIDE_CENTER:
    default:
      return SIDE_CENTER;
  }
}

 // تبدیل مختصات نقطه به مختصات ماتریس
void getDotPixel(Side dotSide, int &px, int &py) {
 // ماتریس (0,0) = بالا-چپ
 // بالا وسط: (3,0) → USB
 // وسط پایین: (3,7) → او اس یو بی
 // چپ مرکز: (0,3) → 15
 // مرکز راست: (7,3) → 34
 // مرکز: (۳،۳)

  switch (dotSide) {
    case SIDE_USB:   px = 3; py = 0; break;
    case SIDE_OUSB:  px = 3; py = 7; break;
    case SIDE_15:    px = 0; py = 3; break;
    case SIDE_34:    px = 7; py = 3; break;
    case SIDE_CENTER:
    default:         px = 3; py = 3; break;
  }
}

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

  matrix.begin();
  matrix.setBrightness(20);
  matrix.fillScreen(0);
  matrix.show();

 // IMU: SDA=۱۱، SCL=۱۲ برای ESP32-S3-Matrix
  if (!imu.begin(11, 12)) {
    Serial.println("Failed to initialize QMI8658!");
    while (1) { delay(1000); }
  }

  imu.setAccelUnit_mg(true);
  imu.setGyroUnit_dps(true);
  imu.setDisplayPrecision(4);

  Serial.print("QMI8658 initialized. useOppositeMapping = ");
  Serial.println(useOppositeMapping ? "TRUE" : "FALSE");
}

void loop() {
  if (!imu.readSensorData(imuData)) {
    return;
  }

  float ax_g = imuData.accelX / 1000.0f;
  float ay_g = imuData.accelY / 1000.0f;
  float az_g = imuData.accelZ / 1000.0f;

  Side upSide  = detectSideUp(ax_g, ay_g, az_g);
  Side dotSide = dotSideFromUpSide(upSide);

  int targetX, targetY;
  getDotPixel(dotSide, targetX, targetY);

 // حرکت نرم به سمت هدف
  dotPosX += (targetX - dotPosX) * dotSmooth;
  dotPosY += (targetY - dotPosY) * dotSmooth;

 // تبدیل به مختصات پیکسلی صحیح
  int px = (int)round(dotPosX);
  int py = (int)round(dotPosY);

 // گیره فقط برای احتیاط
  if (px < 0) px = 0;
  if (px > 7) px = 7;
  if (py < 0) py = 0;
  if (py > 7) py = 7;

 // --- نقطه بکش ---
  matrix.fillScreen(0);
  uint16_t color = matrix.Color(dotRed, dotGreen, dotBlue);
  matrix.drawPixel(px, py, color);
  matrix.show();

 // اشکال‌زدایی
  Serial.print("AX="); Serial.print(ax_g, 3);
  Serial.print(" AY="); Serial.print(ay_g, 3);
  Serial.print(" AZ="); Serial.print(az_g, 3);
  Serial.print(" | UP=");  Serial.print(sideName(upSide));
  Serial.print(" | DOT="); Serial.print(sideName(dotSide));
  Serial.print(" | px=");  Serial.print(px);
  Serial.print(" py=");    Serial.println(py);

  delay(80);
}