// face_id.cc - cube diagnostic: face colors + big face number + world-axis arrows
// Build:
//   g++ -std=c++17 -O2 -Wall -Wextra -I../rpi-rgb-led-matrix/include \
//     face_id.cc -L../rpi-rgb-led-matrix/lib -lrgbmatrix -lrt -lm -lpthread -o face_id
//
// Run example:
//   sudo ./face_id --led-rows=64 --led-cols=64 --led-chain=6 \
//     --led-gpio-mapping=adafruit-hat --led-slowdown-gpio=6

#include <csignal>
#include <cstdint>
#include <cstring>
#include <chrono>
#include <thread>
#include <algorithm>
#include <iostream>
#include <cmath>

#include "led-matrix.h"
#include "cube_orientation.h"

using rgb_matrix::RGBMatrix;
using rgb_matrix::FrameCanvas;
using rgb_matrix::Canvas;

static volatile sig_atomic_t g_running = 1;
static void SigHandler(int) { g_running = 0; }

// Simple 5x7 digit font (only '0'..'5' used here)
static const uint8_t kDigit5x7[10][7] = {
  // each row is 5 bits, MSB on left (bit 4)
  {0b01110,0b10001,0b10011,0b10101,0b11001,0b10001,0b01110}, //0
  {0b00100,0b01100,0b00100,0b00100,0b00100,0b00100,0b01110}, //1
  {0b01110,0b10001,0b00001,0b00010,0b00100,0b01000,0b11111}, //2
  {0b11110,0b00001,0b00001,0b01110,0b00001,0b00001,0b11110}, //3
  {0b00010,0b00110,0b01010,0b10010,0b11111,0b00010,0b00010}, //4
  {0b11111,0b10000,0b10000,0b11110,0b00001,0b00001,0b11110}, //5
  {0b00110,0b01000,0b10000,0b11110,0b10001,0b10001,0b01110}, //6
  {0b11111,0b00001,0b00010,0b00100,0b01000,0b01000,0b01000}, //7
  {0b01110,0b10001,0b10001,0b01110,0b10001,0b10001,0b01110}, //8
  {0b01110,0b10001,0b10001,0b01111,0b00001,0b00010,0b01100}  //9
};

static void DrawDigit(Canvas* c, int x0, int y0, int scale, int digit,
                      uint8_t r, uint8_t g, uint8_t b) {
  digit = std::clamp(digit, 0, 9);
  for (int row = 0; row < 7; ++row) {
    uint8_t bits = kDigit5x7[digit][row];
    for (int col = 0; col < 5; ++col) {
      bool on = (bits >> (4 - col)) & 1;
      if (!on) continue;
      int px = x0 + col * scale;
      int py = y0 + row * scale;
      for (int dy = 0; dy < scale; ++dy)
        for (int dx = 0; dx < scale; ++dx)
          c->SetPixel(px + dx, py + dy, r, g, b);
    }
  }
}

static inline void SetPixelSafe(Canvas *c, int x, int y, uint8_t r, uint8_t g, uint8_t b) {
  if (x < 0 || y < 0 || x >= c->width() || y >= c->height()) return;
  c->SetPixel(x, y, r, g, b);
}

// Bresenham line
static void DrawLine(Canvas *c, int x0, int y0, int x1, int y1, uint8_t r, uint8_t g, uint8_t b) {
  int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
  int dy = -abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
  int err = dx + dy;
  while (true) {
    SetPixelSafe(c, x0, y0, r, g, b);
    if (x0 == x1 && y0 == y1) break;
    int e2 = 2 * err;
    if (e2 >= dy) { err += dy; x0 += sx; }
    if (e2 <= dx) { err += dx; y0 += sy; }
  }
}

static void DrawArrow(Canvas *c, int x0, int y0, int x1, int y1, uint8_t r, uint8_t g, uint8_t b) {
  DrawLine(c, x0, y0, x1, y1, r, g, b);

  // Arrowhead
  int dx = x1 - x0;
  int dy = y1 - y0;
  float len = sqrtf(float(dx*dx + dy*dy));
  if (len < 1.0f) return;
  float ux = dx / len;
  float uy = dy / len;

  // Perp
  float px = -uy;
  float py = ux;

  int ah = 6;   // arrowhead length
  int aw = 3;   // half-width

  int bx = int(x1 - ux * ah);
  int by = int(y1 - uy * ah);

  int lx = int(bx + px * aw);
  int ly = int(by + py * aw);
  int rx = int(bx - px * aw);
  int ry = int(by - py * aw);

  DrawLine(c, x1, y1, lx, ly, r, g, b);
  DrawLine(c, x1, y1, rx, ry, r, g, b);
}

static void DrawRotatingNeedle(Canvas *c, int cx, int cy, float angle_rad) {
  // Needle length
  const int L = 24;

  // Tip point
  int x1 = cx + int(std::cos(angle_rad) * L);
  int y1 = cy + int(std::sin(angle_rad) * L);

  // Bright white needle
  DrawArrow(c, cx, cy, x1, y1, 240, 240, 240);

  // Small center ring
  for (int dy = -2; dy <= 2; dy++) {
    for (int dx = -2; dx <= 2; dx++) {
      if (dx*dx + dy*dy <= 3) SetPixelSafe(c, cx + dx, cy + dy, 240, 240, 240);
    }
  }
}





int main(int argc, char** argv) {
  std::signal(SIGINT, SigHandler);
  std::signal(SIGTERM, SigHandler);

  RGBMatrix::Options options;
  rgb_matrix::RuntimeOptions rt;
  if (!rgb_matrix::ParseOptionsFromFlags(&argc, &argv, &options, &rt)) return 1;

  RGBMatrix* matrix = RGBMatrix::CreateFromOptions(options, rt);
  if (!matrix) {
    std::cerr << "Failed to create matrix\n";
    return 1;
  }

  matrix->SetBrightness(options.brightness ? options.brightness : 60);
  FrameCanvas* canvas = matrix->CreateFrameCanvas();

  

// Apply shared cube physical orientation correction.
cube::CubeCanvas cube_canvas(canvas, 64, 64);
rgb_matrix::Canvas* draw = &cube_canvas;

// Face base colors (distinct but not blinding)
  const uint8_t face_rgb[6][3] = {
    {10, 10, 60},   // 0 TOP
    {60, 10, 10},   // 1 LEFT
    {10, 60, 10},   // 2 FRONT
    {60, 60, 10},   // 3 RIGHT
    {10, 60, 60},   // 4 BACK
    {60, 10, 60}    // 5 BOTTOM
  };

  while (g_running) {
    draw->Clear();

    for (int face = 0; face < 6; ++face) {
      int ox = face * 64;
      int oy = 0;

      // Fill face background
      for (int y = 0; y < 64; ++y) {
        for (int x = 0; x < 64; ++x) {
          draw->SetPixel(ox + x, oy + y,
                           face_rgb[face][0], face_rgb[face][1], face_rgb[face][2]);
        }
      }

      // Draw a big digit in the center
      int scale = 6;                 // 5*6=30 wide, 7*6=42 tall
      int digit_w = 5 * scale;
      int digit_h = 7 * scale;
      int x0 = ox + (64 - digit_w) / 2;
      int y0 = oy + (64 - digit_h) / 2;

// Diagnostic axes (world directions projected onto each face's 2D plane)
int cx = ox + 32;
int cy = oy + 32;

// Colors: X=red, Y=green, Z=blue
// Draw two in-plane axes for each face
if (face == 0) { // TOP (+Y normal): +X right, +Z down
  DrawArrow(draw, cx, cy, cx + 22, cy, 255, 60, 60);
  DrawArrow(draw, cx, cy, cx, cy + 22, 60, 60, 255);
} else if (face == 5) { // BOTTOM (-Y normal): +X right, +Z up
  DrawArrow(draw, cx, cy, cx + 22, cy, 255, 60, 60);
  DrawArrow(draw, cx, cy, cx, cy - 22, 60, 60, 255);
} else if (face == 4) { // FRONT (+Z normal): +X right, +Y up
  DrawArrow(draw, cx, cy, cx + 22, cy, 255, 60, 60);
  DrawArrow(draw, cx, cy, cx, cy - 22, 60, 255, 60);
} else if (face == 2) { // REAR (-Z normal): +X left, +Y up
  DrawArrow(draw, cx, cy, cx - 22, cy, 255, 60, 60);
  DrawArrow(draw, cx, cy, cx, cy - 22, 60, 255, 60);
} else if (face == 1) { // RIGHT (+X normal): +Z left, +Y up
  DrawArrow(draw, cx, cy, cx - 22, cy, 60, 60, 255);
  DrawArrow(draw, cx, cy, cx, cy - 22, 60, 255, 60);
} else if (face == 3) { // LEFT (-X normal): +Z right, +Y up
  DrawArrow(draw, cx, cy, cx + 22, cy, 60, 60, 255);
  DrawArrow(draw, cx, cy, cx, cy - 22, 60, 255, 60);
}

// Center dot
for (int dy = -1; dy <= 1; dy++)
  for (int dx = -1; dx <= 1; dx++)
    SetPixelSafe(draw, cx + dx, cy + dy, 240, 240, 240);


// Slow rotation: one full turn about every ~30 seconds
const auto now = std::chrono::steady_clock::now();
const float t = std::chrono::duration<float>(now.time_since_epoch()).count();
const float angle = t * (2.0f * 3.1415926f / 30.0f);

DrawRotatingNeedle(draw, cx, cy, angle);

      // Digit color: white-ish
      DrawDigit(draw, x0, y0, scale, face, 200, 200, 200);
    }

    canvas = matrix->SwapOnVSync(canvas);
            cube_canvas.SetBase(canvas);
cube_canvas.SetBase(canvas);
cube_canvas.SetBase(canvas);
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
  }

  draw->Clear();
  canvas = matrix->SwapOnVSync(canvas);
            cube_canvas.SetBase(canvas);
cube_canvas.SetBase(canvas);
cube_canvas.SetBase(canvas);
  delete matrix;
  return 0;
}
