DIY Avionics Simulator with ESP32 - Artificial Horizon, Compass & Altimeter

// by miercemk May, 2026

#include <Arduino.h>
#include <Arduino_GFX_Library.h>
#include <Wire.h>

#define DISPLAY_WIDTH  480
#define DISPLAY_HEIGHT 480
#define CX 240
#define CY 240

#define BL_PIN     6
#define PANEL_CS   16
#define PANEL_SCK  2
#define PANEL_SDA  1
#define PCLK_NEG   1

#define ENCODER_CLK 4
#define ENCODER_DT  42

#define I2C_SDA 38
#define I2C_SCL 39
#define PCF8574_ADDR 0x21
#define MPU6050_ADDR 0x68
#define QMC5883L_ADDR 0x0D

Arduino_DataBus       *panelBus = nullptr;
Arduino_ESP32RGBPanel *rgbpanel = nullptr;
Arduino_RGB_Display   *gfx      = nullptr;
uint16_t              *fb       = nullptr;

float pitchDeg = 0;
float rollDeg  = 0;

float compassHeadingDeg = 0;
unsigned long lastCompassRead = 0;

float altitudeFt = 6300.0f;
unsigned long lastAltitudeUpdate = 0;

float lastDrawPitch = 999;
float lastDrawRoll  = 999;

volatile int lastEncoded = 0;
volatile long encoderValue = 0;
long lastEncoderValue = 0;

int brightnessLevel = 255;

unsigned long lastMPURead = 0;

extern const uint8_t st7701_type7_init_operations[];

uint16_t rgb565(uint8_t r, uint8_t g, uint8_t b) {
  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
}

const uint16_t COL_BLACK      = 0x0000;
const uint16_t COL_WHITE      = 0xFFFF;
const uint16_t COL_YELLOW     = 0xFFE0;
const uint16_t COL_ORANGE     = 0xFD20;
const uint16_t COL_SKY        = rgb565(20, 165, 225);
const uint16_t COL_GROUND     = rgb565(150, 95, 45);
const uint16_t COL_RING_OUTER = rgb565(170, 170, 170);
const uint16_t COL_RING_INNER = rgb565(45, 45, 45);
const uint16_t COL_RING_EDGE  = rgb565(15, 15, 15);

enum ScreenMode {
  SCREEN_ATTITUDE = 0,
  SCREEN_COMPASS,
  SCREEN_ALTIMETER,
  SCREEN_AIRPLANE,
  SCREEN_COUNT
};

ScreenMode currentScreen = SCREEN_ATTITUDE;

static inline void putpix(int x, int y, uint16_t c) {
  if ((unsigned)x < DISPLAY_WIDTH && (unsigned)y < DISPLAY_HEIGHT) {
    fb[y * DISPLAY_WIDTH + x] = c;
  }
}

void clearFB(uint16_t color = COL_BLACK) {
  for (int i = 0; i < DISPLAY_WIDTH * DISPLAY_HEIGHT; i++) fb[i] = color;
}

void drawLine(int x0, int y0, int x1, int y1, uint16_t col) {
  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) {
    putpix(x0, y0, col);
    if (x0 == x1 && y0 == y1) break;

    int e2 = 2 * err;
    if (e2 >= dy) { err += dy; x0 += sx; }
    if (e2 <= dx) { err += dx; y0 += sy; }
  }
}

void drawThickLine(int x0, int y0, int x1, int y1, uint16_t col, int t) {
  for (int i = -t / 2; i <= t / 2; i++) {
    drawLine(x0, y0 + i, x1, y1 + i, col);
  }
}

void drawCircle(int cx, int cy, int r, uint16_t col) {
  int x = r, y = 0, err = 0;

  while (x >= y) {
    putpix(cx + x, cy + y, col);
    putpix(cx + y, cy + x, col);
    putpix(cx - y, cy + x, col);
    putpix(cx - x, cy + y, col);
    putpix(cx - x, cy - y, col);
    putpix(cx - y, cy - x, col);
    putpix(cx + y, cy - x, col);
    putpix(cx + x, cy - y, col);

    y++;
    if (err <= 0) err += 2 * y + 1;
    if (err > 0) {
      x--;
      err -= 2 * x + 1;
    }
  }
}

void fillCircle(int cx, int cy, int r, uint16_t col) {

  int r2 = r * r;

  for (int y = -r; y <= r; y++) {
    for (int x = -r; x <= r; x++) {

      if (x * x + y * y <= r2) {
        putpix(cx + x, cy + y, col);
      }
    }
  }
}

void fillTriangle(int x1, int y1, int x2, int y2, int x3, int y3, uint16_t col) {
  int minX = min(x1, min(x2, x3));
  int maxX = max(x1, max(x2, x3));
  int minY = min(y1, min(y2, y3));
  int maxY = max(y1, max(y2, y3));

  int area = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);

  for (int y = minY; y <= maxY; y++) {
    for (int x = minX; x <= maxX; x++) {
      int w1 = (x2 - x1) * (y - y1) - (y2 - y1) * (x - x1);
      int w2 = (x3 - x2) * (y - y2) - (y3 - y2) * (x - x2);
      int w3 = (x1 - x3) * (y - y3) - (y1 - y3) * (x - x3);

      if ((area >= 0 && w1 >= 0 && w2 >= 0 && w3 >= 0) ||
          (area < 0 && w1 <= 0 && w2 <= 0 && w3 <= 0)) {
        putpix(x, y, col);
      }
    }
  }
}

void fillRing(int cx, int cy, int rInner, int rOuter, uint16_t col) {
  int ri2 = rInner * rInner;
  int ro2 = rOuter * rOuter;

  for (int y = -rOuter; y <= rOuter; y++) {
    for (int x = -rOuter; x <= rOuter; x++) {
      int d2 = x * x + y * y;
      if (d2 >= ri2 && d2 <= ro2) {
        putpix(cx + x, cy + y, col);
      }
    }
  }
}

void fillArtificialHorizonDisc() {
  const int R = 206;
  const int r2 = R * R;

  float rollRad = rollDeg * PI / 180.0f;

  float cr = cos(rollRad);
  float sr = sin(rollRad);

  float pitchOffset = pitchDeg * 4.0f;

  // sky / ground
  for (int y = -R; y <= R; y++) {
    for (int x = -R; x <= R; x++) {

      if (x * x + y * y <= r2) {

        float yr = x * sr + y * cr;

        if (yr + pitchOffset < 0)
          putpix(CX + x, CY + y, COL_SKY);
        else
          putpix(CX + x, CY + y, COL_GROUND);
      }
    }
  }

  // helper transform
  auto H = [&](float lx, float ly, int &sx, int &sy) {

    sx = CX + lx * cr + ly * sr;
    sy = CY - lx * sr + ly * cr;
  };

  // --------------------------------------------------
  // VIRTUAL RUNWAY
  // --------------------------------------------------

  {
    int x1, y1, x2, y2, x3, y3;

    H(0, 10 - pitchOffset, x1, y1);
    H(-95, 115 - pitchOffset, x2, y2);
    H(95, 115 - pitchOffset, x3, y3);

    fillTriangle(
      x1, y1,
      x2, y2,
      x3, y3,
      rgb565(80, 38, 25)
    );

    int xa, ya, xb, yb;

    H(0, 10 - pitchOffset, xa, ya);
    H(-32, 115 - pitchOffset, xb, yb);
    drawThickLine(xa, ya, xb, yb, COL_ORANGE, 2);

    H(0, 10 - pitchOffset, xa, ya);
    H(32, 115 - pitchOffset, xb, yb);
    drawThickLine(xa, ya, xb, yb, COL_ORANGE, 2);

    H(-32, 115 - pitchOffset, xa, ya);
    H(32, 115 - pitchOffset, xb, yb);
    drawThickLine(xa, ya, xb, yb, COL_ORANGE, 2);
  }

  // --------------------------------------------------
  // MAIN HORIZON LINE
  // --------------------------------------------------

  {
    int x1, y1, x2, y2;

    H(-R, -pitchOffset, x1, y1);
    H(R,  -pitchOffset, x2, y2);

    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
  }

  // --------------------------------------------------
  // PITCH LADDER
  // --------------------------------------------------

  int step = 32;

  for (int i = -4; i <= 4; i++) {

    if (i == 0) continue;

    int value = abs(i) * 5;

    int len =
      (value == 10 || value == 20)
      ? 120
      : 58;

    int localY = i * step - pitchOffset;

    int x1, y1, x2, y2;

    H(-len / 2, localY, x1, y1);
    H( len / 2, localY, x2, y2);

    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);

    if (value == 10 || value == 20) {

      int xl, yl;
      int xr, yr;

      H(-len / 2 - 50, localY - 12, xl, yl);
      H( len / 2 + 24, localY - 12, xr, yr);

      drawSmallNumber(value, xl, yl, COL_WHITE);
      drawSmallNumber(value, xr, yr, COL_WHITE);
    }
  }
}

void drawSegDigit(int d, int x, int y, int s, uint16_t col) {
  bool seg[7];

  switch (d) {
    case 0: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=1; seg[5]=1; seg[6]=0; break;
    case 1: seg[0]=0; seg[1]=1; seg[2]=1; seg[3]=0; seg[4]=0; seg[5]=0; seg[6]=0; break;
    case 2: seg[0]=1; seg[1]=1; seg[2]=0; seg[3]=1; seg[4]=1; seg[5]=0; seg[6]=1; break;
    case 3: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=0; seg[5]=0; seg[6]=1; break;
    case 4: seg[0]=0; seg[1]=1; seg[2]=1; seg[3]=0; seg[4]=0; seg[5]=1; seg[6]=1; break;
    case 5: seg[0]=1; seg[1]=0; seg[2]=1; seg[3]=1; seg[4]=0; seg[5]=1; seg[6]=1; break;
    case 6: seg[0]=1; seg[1]=0; seg[2]=1; seg[3]=1; seg[4]=1; seg[5]=1; seg[6]=1; break;
    case 7: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=0; seg[4]=0; seg[5]=0; seg[6]=0; break;
    case 8: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=1; seg[5]=1; seg[6]=1; break;
    case 9: seg[0]=1; seg[1]=1; seg[2]=1; seg[3]=1; seg[4]=0; seg[5]=1; seg[6]=1; break;
  }

  int w = 5 * s;
  int h = 9 * s;
  int t = s;

  if (seg[0]) drawThickLine(x, y, x + w, y, col, t);
  if (seg[1]) drawThickLine(x + w, y, x + w, y + h / 2, col, t);
  if (seg[2]) drawThickLine(x + w, y + h / 2, x + w, y + h, col, t);
  if (seg[3]) drawThickLine(x, y + h, x + w, y + h, col, t);
  if (seg[4]) drawThickLine(x, y + h / 2, x, y + h, col, t);
  if (seg[5]) drawThickLine(x, y, x, y + h / 2, col, t);
  if (seg[6]) drawThickLine(x, y + h / 2, x + w, y + h / 2, col, t);
}

void drawSmallNumber(int value, int x, int y, uint16_t col) {
  if (value == 10) {
    drawSegDigit(1, x, y, 2, col);
    drawSegDigit(0, x + 14, y, 2, col);
  }

  if (value == 20) {
    drawSegDigit(2, x, y, 2, col);
    drawSegDigit(0, x + 14, y, 2, col);
  }
}

void drawPitchScaleLine(int y, int value, uint16_t col) {
  int longLen  = 120;
  int shortLen = 58;

  bool major = (value == 10 || value == 20);
  int len = major ? longLen : shortLen;

  drawThickLine(CX - len / 2, y, CX + len / 2, y, col, 3);

  if (value == 10 || value == 20) {
    drawSmallNumber(value, CX - len / 2 - 50, y - 12, col);
    drawSmallNumber(value, CX + len / 2 + 24, y - 12, col);
  }
}

void drawPitchLadder() {
  int step = 32;

  float rollRad = rollDeg * PI / 180.0f;
  float cr = cos(rollRad);
  float sr = sin(rollRad);

  float pitchOffset = pitchDeg * 4.0f;

  for (int i = -4; i <= 4; i++) {
    if (i == 0) continue;

    int value = abs(i) * 5;
    int len = (value == 10 || value == 20) ? 120 : 58;

    float localY = i * step - pitchOffset;

    float x1r = -len / 2;
    float y1r = localY;
    float x2r =  len / 2;
    float y2r = localY;

    int x1 = CX + x1r * cr - y1r * sr;
    int y1 = CY + x1r * sr + y1r * cr;
    int x2 = CX + x2r * cr - y2r * sr;
    int y2 = CY + x2r * sr + y2r * cr;

    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
  }
}

void drawRotatedLine(float x1, float y1, float x2, float y2, float angleDeg, uint16_t col, int thick) {
  float a = angleDeg * PI / 180.0f;
  float cr = cos(a);
  float sr = sin(a);

  int sx1 = CX + x1 * cr - y1 * sr;
  int sy1 = CY + x1 * sr + y1 * cr;
  int sx2 = CX + x2 * cr - y2 * sr;
  int sy2 = CY + x2 * sr + y2 * cr;

  drawThickLine(sx1, sy1, sx2, sy2, col, thick);
}

void drawScreen_AirplaneDemo() {
  clearFB(rgb565(10, 18, 28));

// light gray frame like other instruments
fillRing(CX, CY, 229, 239, COL_RING_OUTER);
fillRing(CX, CY, 218, 228, COL_RING_INNER);

drawCircle(CX, CY, 217, COL_RING_EDGE);
drawCircle(CX, CY, 228, COL_RING_EDGE);
drawCircle(CX, CY, 239, COL_WHITE);

// inner dark display area
fillCircle(CX, CY, 217, rgb565(10, 18, 28));

  // background reference grid
  drawCircle(CX, CY, 210, rgb565(80, 80, 80));
  drawCircle(CX, CY, 140, rgb565(50, 50, 50));
  drawThickLine(CX - 210, CY, CX + 210, CY, rgb565(60, 60, 60), 1);
  drawThickLine(CX, CY - 210, CX, CY + 210, rgb565(60, 60, 60), 1);

  // pitch moves airplane slightly up/down
  int oldCY = CY;
  int pitchMove = constrain((int)(pitchDeg * 3.0f), -90, 90);

  // local center offset
  int baseY = CY + pitchMove;

  float roll = rollDeg;

  auto L = [&](float x1, float y1, float x2, float y2, uint16_t col, int t) {
    float a = roll * PI / 180.0f;
    float cr = cos(a);
    float sr = sin(a);

    int sx1 = CX + x1 * cr - (y1 + pitchMove) * sr;
    int sy1 = CY + x1 * sr + (y1 + pitchMove) * cr;
    int sx2 = CX + x2 * cr - (y2 + pitchMove) * sr;
    int sy2 = CY + x2 * sr + (y2 + pitchMove) * cr;

    drawThickLine(sx1, sy1, sx2, sy2, col, t);
  };

  uint16_t bodyCol = rgb565(255, 180, 40);
  uint16_t wingCol = rgb565(40, 220, 255);
  uint16_t tailCol = rgb565(255, 80, 80);

  // airplane body
  L(0, -120, 0, 110, bodyCol, 8);

  // nose
  L(0, -120, -22, -75, bodyCol, 5);
  L(0, -120,  22, -75, bodyCol, 5);

  // main wings
  L(-20, -25, -145, 35, wingCol, 8);
  L( 20, -25,  145, 35, wingCol, 8);

  // wing tips
  L(-145, 35, -120, 55, wingCol, 5);
  L( 145, 35,  120, 55, wingCol, 5);

  // tail wings
  L(-15, 80, -75, 125, tailCol, 6);
  L( 15, 80,  75, 125, tailCol, 6);

  // center dot
  fillCircle(CX, CY + pitchMove, 8, COL_WHITE);

  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);
}

void drawRollScaleOnRing() {
  for (int a = -50; a <= 50; a += 10) {
    float rad = (a - 90) * PI / 180.0;

    int r1 = 211;
    int r2 = 224;

    if (a % 30 == 0) r1 = 207;

    int x1 = CX + cos(rad) * r1;
    int y1 = CY + sin(rad) * r1;
    int x2 = CX + cos(rad) * r2;
    int y2 = CY + sin(rad) * r2;

    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
  }

  // smaller static orange triangle
  fillTriangle(CX, CY - 210, CX - 11, CY - 229, CX + 11, CY - 229, COL_ORANGE);

  // smaller moving white triangle
float rollRad = rollDeg * PI / 180.0f;
float cr = cos(rollRad);
float sr = sin(rollRad);

auto ROLL = [&](float lx, float ly, int &sx, int &sy) {
  sx = CX + lx * cr + ly * sr;
  sy = CY - lx * sr + ly * cr;
};

int x1, y1, x2, y2, x3, y3;

ROLL(0, -200, x1, y1);
ROLL(-12, -181, x2, y2);
ROLL(12, -181, x3, y3);

fillTriangle(x1, y1, x2, y2, x3, y3, COL_WHITE);
}

void drawVirtualRunway() {
  // dark runway body
  fillTriangle(
    CX, CY + 10,
    CX - 95, CY + 115,
    CX + 95, CY + 115,
    rgb565(80, 38, 25)
  );

  // orange runway perspective lines
  drawThickLine(CX, CY + 10, CX - 32, CY + 115, COL_ORANGE, 2);
  drawThickLine(CX, CY + 10, CX + 32, CY + 115, COL_ORANGE, 2);
  drawThickLine(CX - 32, CY + 115, CX + 32, CY + 115, COL_ORANGE, 2);
}

void drawFrameLetter(char c, int x, int y, int s, uint16_t col) {
  switch (c) {
    case 'N':
      drawThickLine(x, y + 28*s, x, y, col, s);
      drawThickLine(x, y, x + 18*s, y + 28*s, col, s);
      drawThickLine(x + 18*s, y + 28*s, x + 18*s, y, col, s);
      break;

    case 'E':
      drawThickLine(x, y, x, y + 28*s, col, s);
      drawThickLine(x, y, x + 18*s, y, col, s);
      drawThickLine(x, y + 14*s, x + 15*s, y + 14*s, col, s);
      drawThickLine(x, y + 28*s, x + 18*s, y + 28*s, col, s);
      break;

    case 'S':
      drawThickLine(x + 18*s, y, x, y, col, s);
      drawThickLine(x, y, x, y + 14*s, col, s);
      drawThickLine(x, y + 14*s, x + 18*s, y + 14*s, col, s);
      drawThickLine(x + 18*s, y + 14*s, x + 18*s, y + 28*s, col, s);
      drawThickLine(x + 18*s, y + 28*s, x, y + 28*s, col, s);
      break;

    case 'W':
      drawThickLine(x, y, x + 4*s, y + 28*s, col, s);
      drawThickLine(x + 4*s, y + 28*s, x + 9*s, y + 12*s, col, s);
      drawThickLine(x + 9*s, y + 12*s, x + 14*s, y + 28*s, col, s);
      drawThickLine(x + 14*s, y + 28*s, x + 18*s, y, col, s);
      break;
  }
}

void drawFrameNumberText(const char *txt, int x, int y, int s, uint16_t col) {
  for (int i = 0; txt[i]; i++) {
    int dx = i * 14 * s;
    drawSegDigit(txt[i] - '0', x + dx, y, s, col);
  }
}

void drawCompassText(const char *txt, int cx, int cy, int s, uint16_t col) {
  int len = strlen(txt);
  int w;

  if (txt[0] >= '0' && txt[0] <= '9') {
    w = len * 14 * s;
    drawFrameNumberText(txt, cx - w / 2, cy - 9 * s, s, col);
  } else {
    w = 20 * s;
    drawFrameLetter(txt[0], cx - w / 2, cy - 14 * s, s, col);
  }
}

void drawAircraftSymbol() {
  // longer and thicker horizontal yellow wings
  drawThickLine(CX - 140, CY, CX - 35, CY, COL_YELLOW, 8);
  drawThickLine(CX + 35, CY, CX + 140, CY, COL_YELLOW, 8);

  // thicker central inverted V
  drawThickLine(CX - 35, CY, CX, CY + 28, COL_YELLOW, 8);
  drawThickLine(CX, CY + 28, CX + 35, CY, COL_YELLOW, 8);

  // small center point / hub
  fillTriangle(CX, CY - 4, CX - 7, CY + 6, CX + 7, CY + 6, COL_YELLOW);
}

void drawConcentricBezel() {
  // dark ring is thin, and horizon disc touches it directly
  fillRing(CX, CY, 207, 224, COL_RING_INNER);

  // bright outer ring
  fillRing(CX, CY, 225, 239, COL_RING_OUTER);

  drawCircle(CX, CY, 206, COL_RING_EDGE);
  drawCircle(CX, CY, 224, COL_RING_EDGE);
  drawCircle(CX, CY, 239, COL_WHITE);
}

void drawCompassDot(int x, int y, uint16_t col) {
  fillCircle(x, y, 2, col);
}

void drawCompassTicks(float headingDeg = 0) {
  const int R_OUT  = 216;  // речиси до сивиот обрач
  const int R_IN   = 194;  // подолги црти
  const int R_DOT  = 194;  // точки на средина од цртите

  // 36 црти = на секои 10 степени
  // помеѓу N и E има точно 9 црти: 10,20,30,40,50,60,70,80,90
  for (int deg = 0; deg < 360; deg += 10) {
    float a = (deg - headingDeg - 90) * PI / 180.0;

    int x1 = CX + cos(a) * R_IN;
    int y1 = CY + sin(a) * R_IN;
    int x2 = CX + cos(a) * R_OUT;
    int y2 = CY + sin(a) * R_OUT;

    drawThickLine(x1, y1, x2, y2, COL_WHITE, 3);
  }

  // дискретни точки точно на средина помеѓу секои две црти
  // значи на 5,15,25...
  for (int deg = 5; deg < 360; deg += 10) {
    float a = (deg - headingDeg - 90) * PI / 180.0;

    int x = CX + cos(a) * R_DOT;
    int y = CY + sin(a) * R_DOT;

    fillCircle(x, y, 2, COL_WHITE);
  }
}
void drawCompassLetters(float headingDeg = 0) {
  struct Mark {
    int deg;
    const char* txt;
    uint16_t col;
    int scale;
    int radius;
  };

  Mark marks[] = {
    // smaller N/E/S/W
    {0,   "N",  COL_YELLOW, 1, 158},
    {90,  "E",  COL_YELLOW, 1, 158},
    {180, "S",  COL_YELLOW, 1, 158},
    {270, "W",  COL_YELLOW, 1, 158},

    // degree labels
    {30,  "3",  COL_WHITE, 2, 160},
    {60,  "6",  COL_WHITE, 2, 160},
    {120, "12", COL_WHITE, 2, 160},
    {150, "15", COL_WHITE, 2, 165},
    {210, "21", COL_WHITE, 2, 155},
    {240, "24", COL_WHITE, 2, 150},
    {300, "30", COL_WHITE, 2, 150},
    {330, "33", COL_WHITE, 2, 150}
  };

  for (int i = 0; i < 12; i++) {
    float a = (marks[i].deg - headingDeg - 90) * PI / 180.0;

    int x = CX + cos(a) * marks[i].radius;
    int y = CY + sin(a) * marks[i].radius;

    drawCompassText(marks[i].txt, x, y, marks[i].scale, marks[i].col);
  }
}

void drawCompassAirplane() {
  // static yellow airplane symbol
  uint16_t c = COL_ORANGE;

  // nose / fuselage
 drawThickLine(CX, CY - 194, CX, CY + 65, c, 5);

  // nose sides
  drawThickLine(CX, CY - 130, CX - 28, CY - 40, c, 4);
  drawThickLine(CX, CY - 130, CX + 28, CY - 40, c, 4);

  // wings
  drawThickLine(CX - 28, CY - 40, CX - 88, CY + 10, c, 4);
  drawThickLine(CX + 28, CY - 40, CX + 88, CY + 10, c, 4);
  drawThickLine(CX - 88, CY + 10, CX - 88, CY + 35, c, 4);
  drawThickLine(CX + 88, CY + 10, CX + 88, CY + 35, c, 4);
  drawThickLine(CX - 88, CY + 35, CX - 20, CY + 10, c, 4);
  drawThickLine(CX + 88, CY + 35, CX + 20, CY + 10, c, 4);

  // body lower part
  drawThickLine(CX - 20, CY + 10, CX - 20, CY + 85, c, 4);
  drawThickLine(CX + 20, CY + 10, CX + 20, CY + 85, c, 4);

  // tail
  drawThickLine(CX - 20, CY + 85, CX - 55, CY + 110, c, 4);
  drawThickLine(CX + 20, CY + 85, CX + 55, CY + 110, c, 4);
  drawThickLine(CX - 55, CY + 110, CX - 55, CY + 130, c, 4);
  drawThickLine(CX + 55, CY + 110, CX + 55, CY + 130, c, 4);
  drawThickLine(CX - 55, CY + 130, CX, CY + 108, c, 4);
  drawThickLine(CX + 55, CY + 130, CX, CY + 108, c, 4);
}

void drawScreen_Compass() {
  clearFB(COL_BLACK);

  // thinner gray frame - половина од претходната дебелина
  fillRing(CX, CY, 229, 239, COL_RING_OUTER);
  fillRing(CX, CY, 218, 228, COL_RING_INNER);

  drawCircle(CX, CY, 217, COL_RING_EDGE);
  drawCircle(CX, CY, 228, COL_RING_EDGE);
  drawCircle(CX, CY, 239, COL_WHITE);

  fillCircle(CX, CY, 217, rgb565(42, 50, 52));

  float heading = compassHeadingDeg;

  drawCompassTicks(heading);
  drawCompassLetters(heading);
  drawCompassAirplane();

  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);
}

void drawScreen_AttitudeIndicator() {
  
  clearFB(COL_BLACK);

  drawConcentricBezel();

  fillArtificialHorizonDisc();

  drawAircraftSymbol();
  drawRollScaleOnRing();

  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);
}

void drawAltimeterNumbers() {
  const int R_NUM = 155;

  for (int n = 0; n <= 9; n++) {
    float deg = n * 36.0;
    float a = (deg - 90) * PI / 180.0;

    int x = CX + cos(a) * R_NUM;
    int y = CY + sin(a) * R_NUM;

    drawSegDigit(n, x - 8, y - 14, 3, COL_WHITE);
  }
}

void drawAltimeterTicks() {
  const int R_OUT = 205;
  const int R_IN_MAJOR = 178;
  const int R_IN_MINOR = 192;

  for (int i = 0; i < 100; i++) {
    float deg = i * 3.6;
    float a = (deg - 90) * PI / 180.0;

    bool major = (i % 10 == 0);
    bool medium = (i % 5 == 0);

    int r1 = major ? R_IN_MAJOR : (medium ? 185 : R_IN_MINOR);
    int r2 = R_OUT;

    int x1 = CX + cos(a) * r1;
    int y1 = CY + sin(a) * r1;
    int x2 = CX + cos(a) * r2;
    int y2 = CY + sin(a) * r2;

    drawThickLine(x1, y1, x2, y2, COL_WHITE, major ? 4 : 2);
  }
}

void drawAltimeterText() {
  // simple static text with lines, framebuffer-safe
  // ALT
// ALT
drawFrameLetter('A', CX - 48, CY - 78, 1, COL_WHITE);

// L
drawThickLine(CX - 10, CY - 78, CX - 10, CY - 50, COL_WHITE, 1);
drawThickLine(CX - 10, CY - 50, CX + 8, CY - 50, COL_WHITE, 1);

// T
drawFrameLetter('T', CX + 28, CY - 78, 1, COL_WHITE);

  // small “x1000 ft” imitation
  drawThickLine(CX - 28, CY - 38, CX + 28, CY - 38, COL_WHITE, 1);
  drawThickLine(CX - 18, CY - 30, CX + 18, CY - 30, COL_WHITE, 1);
}

void drawAltimeterHand(float angleDeg, int length, int width, uint16_t col) {
  float a = (angleDeg - 90) * PI / 180.0;

  int tipX = CX + cos(a) * length;
  int tipY = CY + sin(a) * length;

  float px = -sin(a);
  float py = cos(a);

  int leftX  = CX + px * width;
  int leftY  = CY + py * width;
  int rightX = CX - px * width;
  int rightY = CY - py * width;

  fillTriangle(leftX, leftY, rightX, rightY, tipX, tipY, col);
}

void drawAltimeterHands(int altitudeFt) {

  // LONG thin hand
  // one full rotation = 1000 ft
  float longAngle =
      ((altitudeFt % 1000) / 1000.0f) * 360.0f;

  // SHORT thick hand
  // one full rotation = 10000 ft
  float shortAngle =
      ((altitudeFt % 10000) / 10000.0f) * 360.0f;

  // short thick hand
  drawAltimeterHand(shortAngle, 95, 11, COL_WHITE);

  // long thin hand
  drawAltimeterHand(longAngle, 175, 4, COL_WHITE);

  fillCircle(CX, CY, 14, rgb565(120,120,120));
  fillCircle(CX, CY, 7, COL_WHITE);
}

void drawAltimeterSmallWindow() {
  // small striped reference window at bottom, like aircraft altimeters
  int x0 = CX - 42;
  int y0 = CY + 72;

 fillCircle(CX, CY + 88, 38, rgb565(25, 25, 25));

  for (int i = 0; i < 5; i++) {
    drawThickLine(x0 + i * 16, y0 + 35, x0 + i * 16 + 35, y0, COL_WHITE, 5);
  }
}

void drawScreen_Altimeter() {
  clearFB(COL_BLACK);

  // concentric frame, same style as compass
  fillRing(CX, CY, 229, 239, COL_RING_OUTER);
  fillRing(CX, CY, 218, 228, COL_RING_INNER);

  drawCircle(CX, CY, 217, COL_RING_EDGE);
  drawCircle(CX, CY, 228, COL_RING_EDGE);
  drawCircle(CX, CY, 239, COL_WHITE);

  // dial background
  fillCircle(CX, CY, 217, rgb565(25, 28, 30));
  fillCircle(CX, CY, 105, rgb565(36, 40, 42));

  drawAltimeterTicks();
  drawAltimeterNumbers();
  drawAltimeterText();
  drawAltimeterSmallWindow();

  
  int altitudeDisplay = (int)altitudeFt;

  drawAltimeterHands(altitudeDisplay);

  gfx->draw16bitRGBBitmap(0, 0, fb, DISPLAY_WIDTH, DISPLAY_HEIGHT);

  
}

void drawCurrentScreen() {
  switch (currentScreen) {
    case SCREEN_ATTITUDE:
      drawScreen_AttitudeIndicator();
      break;

    case SCREEN_COMPASS:
      drawScreen_Compass();
      break;

    case SCREEN_ALTIMETER:
      drawScreen_Altimeter();
      break;

   case SCREEN_AIRPLANE:
      drawScreen_AirplaneDemo();
      break;     

    default:
      drawScreen_AttitudeIndicator();
      break;
  }
}  

void init_display() {
 pinMode(BL_PIN, OUTPUT);
ledcAttach(BL_PIN, 5000, 8);
ledcWrite(BL_PIN, brightnessLevel);

  panelBus = new Arduino_SWSPI(
    GFX_NOT_DEFINED, PANEL_CS, PANEL_SCK, PANEL_SDA, GFX_NOT_DEFINED
  );

  rgbpanel = new Arduino_ESP32RGBPanel(
      40,7,15,41,
      46,3,8,18,17,
      14,13,12,11,10,9,
      5,45,48,47,21,
      1,50,10,50,
      1,30,10,30,
      PCLK_NEG,6000000UL
  );

  gfx = new Arduino_RGB_Display(
    DISPLAY_WIDTH, DISPLAY_HEIGHT, rgbpanel, 0, true,
    panelBus, GFX_NOT_DEFINED,
    st7701_type7_init_operations, sizeof(st7701_type7_init_operations)
  );

  gfx->begin(8000000);

  fb = (uint16_t*)ps_malloc(DISPLAY_WIDTH * DISPLAY_HEIGHT * 2);
  if (!fb) {
    Serial.println("Framebuffer allocation failed!");
    while (1);
  }
}

void pcf8574_init() {
  Wire.begin(I2C_SDA, I2C_SCL);

  Wire.beginTransmission(PCF8574_ADDR);
  Wire.write(0xFF);        // all pins high = inputs with pullups
  Wire.endTransmission();
}

uint8_t pcf8574_read() {
  Wire.requestFrom(PCF8574_ADDR, (uint8_t)1);

  if (Wire.available()) {
    return Wire.read();
  }

  return 0xFF;
}

bool isEncoderButtonPressed() {
  uint8_t state = pcf8574_read();

  // P5 low = button pressed
  return !(state & (1 << 5));
}

bool mpu6050_init() {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B);      // PWR_MGMT_1
  Wire.write(0x00);      // wake up
  if (Wire.endTransmission() != 0) return false;

  delay(100);

  // accelerometer ±2g
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x1C);
  Wire.write(0x00);
  Wire.endTransmission();

  return true;
}

bool mpu6050_readAccel(float &ax, float &ay, float &az) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x3B); // ACCEL_XOUT_H
  if (Wire.endTransmission(false) != 0) return false;

  Wire.requestFrom(MPU6050_ADDR, (uint8_t)6);
  if (Wire.available() < 6) return false;

  int16_t rawX = (Wire.read() << 8) | Wire.read();
  int16_t rawY = (Wire.read() << 8) | Wire.read();
  int16_t rawZ = (Wire.read() << 8) | Wire.read();

  ax = rawX / 16384.0f;
  ay = rawY / 16384.0f;
  az = rawZ / 16384.0f;

  return true;
}

void updateMPU6050() {
  float ax, ay, az;

  if (!mpu6050_readAccel(ax, ay, az)) return;

  float newRoll  = atan2(ay, az) * 180.0f / PI;
  float newPitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0f / PI;

  // smoothing
  rollDeg  = rollDeg  * 0.85f + newRoll  * 0.15f;
  pitchDeg = pitchDeg * 0.85f + newPitch * 0.15f;
}

bool qmc5883l_init() {
  Wire.beginTransmission(QMC5883L_ADDR);
  Wire.write(0x0B);      // SET/RESET period register
  Wire.write(0x01);
  Wire.endTransmission();

  Wire.beginTransmission(QMC5883L_ADDR);
  Wire.write(0x09);      // control register
  Wire.write(0x1D);      // continuous, 200Hz, 2G, 512 OSR
  if (Wire.endTransmission() != 0) return false;

  return true;
}

bool qmc5883l_readHeading(float &headingDeg) {
  Wire.beginTransmission(QMC5883L_ADDR);
  Wire.write(0x00);
  if (Wire.endTransmission(false) != 0) return false;

  Wire.requestFrom(QMC5883L_ADDR, (uint8_t)6);
  if (Wire.available() < 6) return false;

  int16_t x = Wire.read() | (Wire.read() << 8);
  int16_t y = Wire.read() | (Wire.read() << 8);
  int16_t z = Wire.read() | (Wire.read() << 8);

  float heading = atan2((float)y, (float)x) * 180.0f / PI;

  if (heading < 0) heading += 360.0f;
  if (heading >= 360) heading -= 360.0f;

  headingDeg = heading;

  Serial.print("QMC X=");
  Serial.print(x);
  Serial.print(" Y=");
  Serial.print(y);
  Serial.print(" Z=");
  Serial.print(z);
  Serial.print(" Heading=");
  Serial.println(headingDeg);

  return true;
}

void updateQMC5883L() {
  float h;

  if (!qmc5883l_readHeading(h)) {
    Serial.println("QMC5883L read failed");
    return;
  }

  float diff = h - compassHeadingDeg;

  if (diff > 180.0f)  diff -= 360.0f;
  if (diff < -180.0f) diff += 360.0f;

  compassHeadingDeg += diff * 0.15f;

  if (compassHeadingDeg < 0) compassHeadingDeg += 360.0f;
  if (compassHeadingDeg >= 360.0f) compassHeadingDeg -= 360.0f;
}

void updateSimulatedAltitude() {
  unsigned long now = millis();

  if (lastAltitudeUpdate == 0) {
    lastAltitudeUpdate = now;
    return;
  }

  float dt = (now - lastAltitudeUpdate) / 1000.0f;
  lastAltitudeUpdate = now;

  // dead zone за да не “плива” кога е речиси рамно
  float p = pitchDeg;

  if (abs(p) < 2.0f) {
    p = 0;
  }

  // брзина на промена на висина
  // 1 степен pitch ≈ 12 ft/sec
  float climbRateFtPerSec = -p * 12.0f;

  altitudeFt += climbRateFtPerSec * dt;

  // ограничување
  if (altitudeFt < 0) altitudeFt = 0;
  if (altitudeFt > 9999) altitudeFt = 9999;
}

void IRAM_ATTR updateEncoder() {
  int MSB = digitalRead(ENCODER_CLK);
  int LSB = digitalRead(ENCODER_DT);

  int encoded = (MSB << 1) | LSB;
  int sum = (lastEncoded << 2) | encoded;

  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011)
    encoderValue++;

  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000)
    encoderValue--;

  lastEncoded = encoded;
}

void handleBrightnessEncoder() {
  noInterrupts();
  long currentValue = encoderValue;
  interrupts();

  if (currentValue != lastEncoderValue) {
    int diff = currentValue - lastEncoderValue;

    brightnessLevel += diff * 4;

    if (brightnessLevel < 20) brightnessLevel = 20;
    if (brightnessLevel > 255) brightnessLevel = 255;

    ledcWrite(BL_PIN, brightnessLevel);

    lastEncoderValue = currentValue;

    Serial.print("Brightness: ");
    Serial.println(brightnessLevel);
  }
}

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

  pinMode(ENCODER_CLK, INPUT_PULLUP);
  pinMode(ENCODER_DT, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), updateEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_DT), updateEncoder, CHANGE);

  init_display();

  pcf8574_init();

//  scanI2C();
  
  if (mpu6050_init()) {
  Serial.println("MPU6050 OK");
} else {
  Serial.println("MPU6050 NOT FOUND");
}

if (qmc5883l_init()) {
  Serial.println("QMC5883L OK");
} else {
  Serial.println("QMC5883L NOT FOUND");
}

  drawCurrentScreen();
}

void loop() {

  handleBrightnessEncoder();
  
  static bool lastBtn = false;

  bool btn = isEncoderButtonPressed();

  if (btn && !lastBtn) {
    currentScreen = (ScreenMode)((currentScreen + 1) % SCREEN_COUNT);
    drawCurrentScreen();
    delay(250);
  }

  lastBtn = btn;

  if (currentScreen == SCREEN_ATTITUDE && millis() - lastMPURead > 120) {
    lastMPURead = millis();
    updateMPU6050();
    drawCurrentScreen();
  }
 if (currentScreen == SCREEN_COMPASS && millis() - lastCompassRead > 120) {
  lastCompassRead = millis();
  updateQMC5883L();
  drawCurrentScreen();
}


if (currentScreen == SCREEN_ALTIMETER && millis() - lastMPURead > 120) {
  lastMPURead = millis();

  updateMPU6050();
  updateSimulatedAltitude();

  drawCurrentScreen();
}

if (currentScreen == SCREEN_AIRPLANE && millis() - lastMPURead > 120) {
  lastMPURead = millis();
  updateMPU6050();
  drawCurrentScreen();
}

}