/*
 * پروژه ۵: فلش همیشه به بالا - ماتریس LED RGB ESP32-S3 (Waveshare)  
 * این طرح از QMI8658C IMU شیب را خوانده و یک نقطه را به صورت نرم و هماهنگ بر اساس جهت‌گیری برد روی ماتریس 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، ۳۴↔۱۵
 // نادرست -> پیکان به سمت همان طرفی که بالا است اشاره می‌کند
bool useOppositeMapping = false;

 // رنگ پیکان (۰-۲۵۵ هر کدام)
uint8_t dotRed   = 0;
uint8_t dotGreen = 150;
uint8_t dotBlue  = 0;

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

 // راهنمایی برای ترسیم فلش
enum ArrowDir {
  ARROW_CENTER,
  ARROW_UP,
  ARROW_DOWN,
  ARROW_LEFT,
  ARROW_RIGHT
};

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۸ یک پیکان ساده رسم کنید که در جهت داده شده اشاره کند.
void drawArrow(ArrowDir dir, uint16_t color) {
  matrix.fillScreen(0);

  switch (dir) {
    case ARROW_UP:
 // نکته
      matrix.drawPixel(3, 0, color);
      matrix.drawPixel(4, 0, color);
 // ردیف دوم
      matrix.drawPixel(2, 1, color);
      matrix.drawPixel(3, 1, color);
      matrix.drawPixel(4, 1, color);
      matrix.drawPixel(5, 1, color);
 // میله
      matrix.drawLine(3, 2, 3, 6, color);
      matrix.drawLine(4, 2, 4, 6, color);
      break;

    case ARROW_DOWN:
 // نکته
      matrix.drawPixel(3, 7, color);
      matrix.drawPixel(4, 7, color);
 // رشد بالاتر از نوک
      matrix.drawPixel(2, 6, color);
      matrix.drawPixel(3, 6, color);
      matrix.drawPixel(4, 6, color);
      matrix.drawPixel(5, 6, color);
 // میله
      matrix.drawLine(3, 1, 3, 5, color);
      matrix.drawLine(4, 1, 4, 5, color);
      break;

    case ARROW_LEFT:
 // نکته
      matrix.drawPixel(0, 3, color);
      matrix.drawPixel(0, 4, color);
 // ستون بعد از انعام
      matrix.drawPixel(1, 2, color);
      matrix.drawPixel(1, 3, color);
      matrix.drawPixel(1, 4, color);
      matrix.drawPixel(1, 5, color);
 // میله
      matrix.drawLine(2, 3, 6, 3, color);
      matrix.drawLine(2, 4, 6, 4, color);
      break;

    case ARROW_RIGHT:
 // نکته
      matrix.drawPixel(7, 3, color);
      matrix.drawPixel(7, 4, color);
 // ستون قبل از انعام
      matrix.drawPixel(6, 2, color);
      matrix.drawPixel(6, 3, color);
      matrix.drawPixel(6, 4, color);
      matrix.drawPixel(6, 5, color);
 // میله
      matrix.drawLine(1, 3, 5, 3, color);
      matrix.drawLine(1, 4, 5, 4, color);
      break;

    case ARROW_CENTER:
    default:
 // ساده بعلاوه در مرکز
      matrix.drawLine(3, 3, 4, 3, color);
      matrix.drawLine(3, 4, 4, 4, color);
      matrix.drawLine(3, 3, 3, 4, color);
      matrix.drawLine(4, 3, 4, 4, color);
      break;
  }

  matrix.show();
}

 // -------- IMU → تشخیص سمت --------
 // ما قبلاً کالیبره کردیم:
 // +X = USB، -X = OUSB
 // +ی = ۱۵، -ی = ۳۴ (بعد از تصحیح شما)

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_34; // +Y = ۳۴ بالا
    } else {
      return SIDE_15; // -Y = ۱۵ بالا
    }
  }

 // در غیر این صورت، از محور 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;
}

 // نقشه از طرف بالای UP به جایی که فلش باید اشاره کند
Side arrowSideFromUpSide(Side upSide) {
  if (!useOppositeMapping) {
 // فلش سمت بالا را نشان می‌دهد
    return upSide;
  }

 // فلش طرف مقابل را نشان می‌دهد
  switch (upSide) {
    case SIDE_USB:   return SIDE_OUSB;
    case SIDE_OUSB:  return SIDE_USB;
    case SIDE_15:    return SIDE_34;
    case SIDE_34:    return SIDE_15;
    case SIDE_CENTER:
    default:         return SIDE_CENTER;
  }
}

 // SIDE را به ArrowDir تبدیل کنید
ArrowDir arrowDirFromSide(Side s) {
  switch (s) {
    case SIDE_USB:    return ARROW_UP;
    case SIDE_OUSB:   return ARROW_DOWN;
    case SIDE_15:     return ARROW_LEFT;
    case SIDE_34:     return ARROW_RIGHT;
    case SIDE_CENTER:
    default:          return ARROW_CENTER;
  }
}

 // ---------------- راه‌اندازی و حلقه ----------------

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

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

 // IMU: SDA=11, SCL=12 روی 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 arrowSide  = arrowSideFromUpSide(upSide);
  ArrowDir dir    = arrowDirFromSide(arrowSide);

  uint16_t color = matrix.Color(dotRed, dotGreen, dotBlue);
  drawArrow(dir, color);

 // اشکال‌زدایی
  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(" | ARROW="); Serial.println(sideName(arrowSide));

  delay(80);
}