#include <Arduino.h>
#include <TFT_eSPI.h>
#include <SPIFFS.h>

// =========================
// CONFIG
// =========================
#define SCREEN_COLS 80
#define SCREEN_ROWS 25

#define IMG_PATH       "/DOS330.img"
#define SECTOR_SIZE    512
#define FLOPPY_HEADS   2
#define FLOPPY_SPT     18

// =========================
// GLOBALS
// =========================
TFT_eSPI tft = TFT_eSPI();
File dosImg;

// 1 MB emulated RAM
static uint8_t mem[1024 * 1024];

// Text mode buffer (80x25)
static uint8_t textChars[SCREEN_ROWS][SCREEN_COLS];
static uint8_t textAttr[SCREEN_ROWS][SCREEN_COLS];

// Cursor state
static uint8_t cursorRow = 0;
static uint8_t cursorCol = 0;
static uint8_t cursorStart = 0;
static uint8_t cursorEnd = 15;
static uint8_t activePage = 0;

// CPU state
struct CPU {
  uint16_t ax, bx, cx, dx;
  uint16_t si, di, bp, sp;
  uint16_t cs, ds, es, ss;
  uint16_t ip;
  uint16_t flags;
} cpu;

// Flags
static const uint16_t FLAG_CF = 0x0001;
static const uint16_t FLAG_ZF = 0x0040;
static const uint16_t FLAG_SF = 0x0080;

// Segment override
static uint16_t *segOverride = nullptr;

// =========================
// SIMPLE 8x8 FONT (subset)
// =========================
static const uint8_t font8x8[96][8] PROGMEM = {
  // 32 ' '
  {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
  // 33 '!'
  {0x18,0x3C,0x3C,0x18,0x18,0x00,0x18,0x00},
  // 34 '"'
  {0x36,0x36,0x24,0x00,0x00,0x00,0x00,0x00},
  // 35 '#'
  {0x36,0x36,0x7F,0x36,0x7F,0x36,0x36,0x00},
  // 36 '$'
  {0x0C,0x3E,0x03,0x1E,0x30,0x1F,0x0C,0x00},
  // 37 '%'
  {0x00,0x63,0x33,0x18,0x0C,0x66,0x63,0x00},
  // 38 '&'
  {0x1C,0x36,0x1C,0x6E,0x3B,0x33,0x6E,0x00},
  // 39 '''
  {0x0C,0x0C,0x18,0x00,0x00,0x00,0x00,0x00},
  // 40 '('
  {0x18,0x0C,0x06,0x06,0x06,0x0C,0x18,0x00},
  // 41 ')'
  {0x06,0x0C,0x18,0x18,0x18,0x0C,0x06,0x00},
  // 42 '*'
  {0x00,0x66,0x3C,0xFF,0x3C,0x66,0x00,0x00},
  // 43 '+'
  {0x00,0x0C,0x0C,0x3F,0x0C,0x0C,0x00,0x00},
  // 44 ','
  {0x00,0x00,0x00,0x00,0x0C,0x0C,0x18,0x00},
  // 45 '-'
  {0x00,0x00,0x00,0x3F,0x00,0x00,0x00,0x00},
  // 46 '.'
  {0x00,0x00,0x00,0x00,0x0C,0x0C,0x00,0x00},
  // 47 '/'
  {0x60,0x30,0x18,0x0C,0x06,0x03,0x01,0x00},
  // 48 '0'
  {0x3E,0x63,0x73,0x7B,0x6F,0x67,0x3E,0x00},
  // 49 '1'
  {0x0C,0x0E,0x0F,0x0C,0x0C,0x0C,0x3F,0x00},
  // 50 '2'
  {0x1E,0x33,0x30,0x1C,0x06,0x33,0x3F,0x00},
  // 51 '3'
  {0x1E,0x33,0x30,0x1C,0x30,0x33,0x1E,0x00},
  // 52 '4'
  {0x38,0x3C,0x36,0x33,0x7F,0x30,0x78,0x00},
  // 53 '5'
  {0x3F,0x03,0x1F,0x30,0x30,0x33,0x1E,0x00},
  // 54 '6'
  {0x1C,0x06,0x03,0x1F,0x33,0x33,0x1E,0x00},
  // 55 '7'
  {0x3F,0x33,0x30,0x18,0x0C,0x0C,0x0C,0x00},
  // 56 '8'
  {0x1E,0x33,0x33,0x1E,0x33,0x33,0x1E,0x00},
  // 57 '9'
  {0x1E,0x33,0x33,0x3E,0x30,0x18,0x0E,0x00},
  // 58 ':'
  {0x00,0x0C,0x0C,0x00,0x0C,0x0C,0x00,0x00},
  // 59 ';'
  {0x00,0x0C,0x0C,0x00,0x0C,0x0C,0x18,0x00},
  // 60 '<'
  {0x18,0x0C,0x06,0x03,0x06,0x0C,0x18,0x00},
  // 61 '='
  {0x00,0x00,0x3F,0x00,0x3F,0x00,0x00,0x00},
  // 62 '>'
  {0x06,0x0C,0x18,0x30,0x18,0x0C,0x06,0x00},
  // 63 '?'
  {0x1E,0x33,0x30,0x18,0x0C,0x00,0x0C,0x00},
  // 64 '@'
  {0x3E,0x63,0x7B,0x7B,0x7B,0x03,0x1E,0x00},
  // 65 'A'
  {0x0C,0x1E,0x33,0x33,0x3F,0x33,0x33,0x00},
  // 66 'B'
  {0x3F,0x66,0x66,0x3E,0x66,0x66,0x3F,0x00},
  // 67 'C'
  {0x3C,0x66,0x03,0x03,0x03,0x66,0x3C,0x00},
  // 68 'D'
  {0x1F,0x36,0x66,0x66,0x66,0x36,0x1F,0x00},
  // 69 'E'
  {0x7F,0x46,0x16,0x1E,0x16,0x46,0x7F,0x00},
  // 70 'F'
  {0x7F,0x46,0x16,0x1E,0x16,0x06,0x0F,0x00},
  // 71 'G'
  {0x3C,0x66,0x03,0x03,0x73,0x66,0x7C,0x00},
  // 72 'H'
  {0x33,0x33,0x33,0x3F,0x33,0x33,0x33,0x00},
  // 73 'I'
  {0x1E,0x0C,0x0C,0x0C,0x0C,0x0C,0x1E,0x00},
  // 74 'J'
  {0x78,0x30,0x30,0x30,0x33,0x33,0x1E,0x00},
  // 75 'K'
  {0x67,0x66,0x36,0x1E,0x36,0x66,0x67,0x00},
  // 76 'L'
  {0x0F,0x06,0x06,0x06,0x46,0x66,0x7F,0x00},
  // 77 'M'
  {0x63,0x77,0x7F,0x7F,0x6B,0x63,0x63,0x00},
  // 78 'N'
  {0x63,0x67,0x6F,0x7B,0x73,0x63,0x63,0x00},
  // 79 'O'
  {0x1C,0x36,0x63,0x63,0x63,0x36,0x1C,0x00},
  // 80 'P'
  {0x3F,0x66,0x66,0x3E,0x06,0x06,0x0F,0x00},
  // 81 'Q'
  {0x1E,0x33,0x33,0x33,0x3B,0x1E,0x38,0x00},
  // 82 'R'
  {0x3F,0x66,0x66,0x3E,0x36,0x66,0x67,0x00},
  // 83 'S'
  {0x1E,0x33,0x07,0x0E,0x38,0x33,0x1E,0x00},
  // 84 'T'
  {0x3F,0x2D,0x0C,0x0C,0x0C,0x0C,0x1E,0x00},
  // 85 'U'
  {0x33,0x33,0x33,0x33,0x33,0x33,0x3F,0x00},
  // 86 'V'
  {0x33,0x33,0x33,0x33,0x33,0x1E,0x0C,0x00},
  // 87 'W'
  {0x63,0x63,0x63,0x6B,0x7F,0x77,0x63,0x00},
  // 88 'X'
  {0x63,0x63,0x36,0x1C,0x1C,0x36,0x63,0x00},
  // 89 'Y'
  {0x33,0x33,0x33,0x1E,0x0C,0x0C,0x1E,0x00},
  // 90 'Z'
  {0x7F,0x63,0x31,0x18,0x4C,0x66,0x7F,0x00},
  // 91 '['
  {0x1E,0x06,0x06,0x06,0x06,0x06,0x1E,0x00},
  // 92 '\'
  {0x03,0x06,0x0C,0x18,0x30,0x60,0x40,0x00},
  // 93 ']'
  {0x1E,0x18,0x18,0x18,0x18,0x18,0x1E,0x00},
  // 94 '^'
  {0x08,0x1C,0x36,0x63,0x00,0x00,0x00,0x00},
  // 95 '_'
  {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF},
};

// =========================
// FORWARD DECLS
// =========================
void handleInterrupt(uint8_t intno);
void int10_handler();
void int13_handler();
void int16_handler();
void int19_handler();
void micro86_test_module();   // <--- added forward decl

// =========================
// TEXT RENDERING
// =========================
void drawChar8x8(int col, int row, char c, uint16_t fg, uint16_t bg) {
  if (c < 32 || c > 127) c = ' ';
  const uint8_t *glyph = font8x8[c - 32];

  int x0 = col * 8;
  int y0 = row * 8;

  for (int y = 0; y < 8; y++) {
    uint8_t line = pgm_read_byte(&glyph[y]);
    for (int x = 0; x < 8; x++) {
      uint16_t color = (line & (0x80 >> x)) ? fg : bg;
      tft.drawPixel(x0 + x, y0 + y, color);
    }
  }
}

uint16_t attrToColorFG(uint8_t attr) {
  return tft.color565(255, 255, 255);
}

uint16_t attrToColorBG(uint8_t attr) {
  return tft.color565(0, 0, 0);
}

void renderTextScreen() {
  for (int r = 0; r < SCREEN_ROWS; r++) {
    for (int c = 0; c < SCREEN_COLS; c++) {
      char ch = textChars[r][c];
      uint8_t at = textAttr[r][c];
      uint16_t fg = attrToColorFG(at);
      uint16_t bg = attrToColorBG(at);
      drawChar8x8(c, r, ch, fg, bg);
    }
  }
}

// =========================
// CURSOR & SCROLL
// =========================
void clampCursor() {
  if (cursorRow >= SCREEN_ROWS) cursorRow = SCREEN_ROWS - 1;
  if (cursorCol >= SCREEN_COLS) cursorCol = SCREEN_COLS - 1;
}

void scrollUpOneLine() {
  for (int r = 1; r < SCREEN_ROWS; r++) {
    for (int c = 0; c < SCREEN_COLS; c++) {
      textChars[r - 1][c] = textChars[r][c];
      textAttr[r - 1][c]  = textAttr[r][c];
    }
  }
  for (int c = 0; c < SCREEN_COLS; c++) {
    textChars[SCREEN_ROWS - 1][c] = ' ';
    textAttr[SCREEN_ROWS - 1][c]  = 0x07;
  }
}

void clearScreenAttr(uint8_t attr) {
  for (int r = 0; r < SCREEN_ROWS; r++) {
    for (int c = 0; c < SCREEN_COLS; c++) {
      textChars[r][c] = ' ';
      textAttr[r][c]  = attr;
    }
  }
  cursorRow = 0;
  cursorCol = 0;
}

// =========================
// DISK ACCESS
// =========================
bool readSector(uint32_t lba, uint8_t *buf) {
  if (!dosImg) return false;
  uint64_t offset = (uint64_t)lba * SECTOR_SIZE;
  if (!dosImg.seek(offset, SeekSet)) return false;
  int n = dosImg.read(buf, SECTOR_SIZE);
  return (n == SECTOR_SIZE);
}

uint32_t chsToLba(uint16_t cylinder, uint8_t head, uint8_t sector) {
  if (sector == 0) sector = 1;
  uint32_t lba = (uint32_t)cylinder * FLOPPY_HEADS * FLOPPY_SPT;
  lba += (uint32_t)head * FLOPPY_SPT;
  lba += (uint32_t)(sector - 1);
  return lba;
}

// =========================
// BIOS TEXT OUTPUT
// =========================
void bios_int10_text_putc(char ch) {
  if (ch == '\r') {
    cursorCol = 0;
    return;
  }
  if (ch == '\n') {
    cursorRow++;
    if (cursorRow >= SCREEN_ROWS) {
      scrollUpOneLine();
      cursorRow = SCREEN_ROWS - 1;
    }
    return;
  }

  textChars[cursorRow][cursorCol] = ch;
  textAttr[cursorRow][cursorCol] = 0x07;
  cursorCol++;
  if (cursorCol >= SCREEN_COLS) {
    cursorCol = 0;
    cursorRow++;
    if (cursorRow >= SCREEN_ROWS) {
      scrollUpOneLine();
      cursorRow = SCREEN_ROWS - 1;
    }
  }
}

void bios_print(const char *s) {
  while (*s) bios_int10_text_putc(*s++);
}

// =========================
// MEMORY ACCESS
// =========================
uint8_t memRead8(uint32_t addr) {
  return mem[addr & 0xFFFFF];
}

void memWrite8(uint32_t addr, uint8_t v) {
  mem[addr & 0xFFFFF] = v;
}

uint16_t memRead16(uint32_t addr) {
  uint16_t lo = memRead8(addr);
  uint16_t hi = memRead8(addr + 1);
  return lo | (hi << 8);
}

void memWrite16(uint32_t addr, uint16_t v) {
  memWrite8(addr, v & 0xFF);
  memWrite8(addr + 1, v >> 8);
}

uint32_t segOff(uint16_t seg, uint16_t off) {
  return (((uint32_t)seg) << 4) + off;
}

// =========================
// CPU HELPERS
// =========================
inline uint16_t &R16(uint8_t i) {
  switch (i & 7) {
    case 0: return cpu.ax;
    case 1: return cpu.cx;
    case 2: return cpu.dx;
    case 3: return cpu.bx;
    case 4: return cpu.sp;
    case 5: return cpu.bp;
    case 6: return cpu.si;
    case 7: return cpu.di;
  }
  return cpu.ax;
}

uint16_t &segReg(uint8_t i) {
  switch (i & 3) {
    case 0: return cpu.es;
    case 1: return cpu.cs;
    case 2: return cpu.ss;
    case 3: return cpu.ds;
  }
  return cpu.ds;
}

void setZF_SF_CF(uint32_t res, uint32_t op1, uint32_t op2, bool isSub) {
  cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
  if ((res & 0xFFFF) == 0) cpu.flags |= FLAG_ZF;
  if (res & 0x8000) cpu.flags |= FLAG_SF;
  if (isSub) {
    if ((res & 0x10000) != 0) cpu.flags |= FLAG_CF;
  } else {
    if (res & 0x10000) cpu.flags |= FLAG_CF;
  }
}

// =========================
// CPU RESET
// =========================
void cpuReset() {
  memset(&cpu, 0, sizeof(cpu));
  cpu.cs = 0x0000;
  cpu.ip = 0x7C00;
  cpu.sp = 0xFFFE;
  cpu.ss = 0x0000;
  cpu.ds = 0x0000;
  cpu.es = 0x0000;
  cpu.flags = 0x0200;
  segOverride = nullptr;
}

// =========================
// BIOS-LIKE INTERRUPTS
// =========================
void int10_handler() {
  uint8_t ah = cpu.ax >> 8;
  uint8_t al = cpu.ax & 0xFF;
  uint8_t bh = cpu.bx >> 8;
  uint8_t bl = cpu.bx & 0xFF;
  uint8_t ch = cpu.cx >> 8;
  uint8_t cl = cpu.cx & 0xFF;
  uint8_t dh = cpu.dx >> 8;
  uint8_t dl = cpu.dx & 0xFF;

  switch (ah) {
    case 0x00:
      clearScreenAttr(0x07);
      break;
    case 0x02:
      cursorRow = dh;
      cursorCol = dl;
      clampCursor();
      break;
    case 0x06: {
      uint8_t lines = al;
      uint8_t attr  = bh;
      uint8_t top   = ch;
      uint8_t left  = cl;
      uint8_t bottom= dh;
      uint8_t right = dl;
      if (lines == 0) lines = bottom - top + 1;
      for (int i = 0; i < lines; i++) {
        for (int r = top; r < bottom; r++) {
          for (int c = left; c <= right; c++) {
            textChars[r][c] = textChars[r + 1][c];
            textAttr[r][c]  = textAttr[r + 1][c];
          }
        }
        for (int c = left; c <= right; c++) {
          textChars[bottom][c] = ' ';
          textAttr[bottom][c]  = attr;
        }
      }
      break;
    }
    case 0x07: {
      uint8_t lines = al;
      uint8_t attr  = bh;
      uint8_t top   = ch;
      uint8_t left  = cl;
      uint8_t bottom= dh;
      uint8_t right = dl;
      if (lines == 0) lines = bottom - top + 1;
      for (int i = 0; i < lines; i++) {
        for (int r = bottom; r > top; r--) {
          for (int c = left; c <= right; c++) {
            textChars[r][c] = textChars[r - 1][c];
            textAttr[r][c]  = textAttr[r - 1][c];
          }
        }
        for (int c = left; c <= right; c++) {
          textChars[top][c] = ' ';
          textAttr[top][c]  = attr;
        }
      }
      break;
    }
    case 0x0E:
      bios_int10_text_putc((char)al);
      break;
    default:
      break;
  }
}

void int13_handler() {
  uint8_t ah = cpu.ax >> 8;
  uint8_t al = cpu.ax & 0xFF;
  uint8_t cl = cpu.cx & 0xFF;
  uint8_t ch = cpu.cx >> 8;
  uint8_t dh = cpu.dx >> 8;
  uint8_t dl = cpu.dx & 0xFF;
  (void)dl;

  switch (ah) {
    case 0x00:
      cpu.ax &= 0x00FF;
      cpu.flags &= ~FLAG_CF;
      break;
    case 0x02: {
      uint8_t sector = cl & 0x3F;
      uint16_t cylinder = ((cl & 0xC0) << 2) | ch;
      uint16_t es = cpu.es;
      uint16_t bx = cpu.bx;
      uint32_t lba = chsToLba(cylinder, dh, sector);
      uint8_t buf[SECTOR_SIZE];
      bool ok = true;
      for (int i = 0; i < al; i++) {
        if (!readSector(lba + i, buf)) {
          ok = false;
          break;
        }
        uint32_t dest = segOff(es, bx + i * SECTOR_SIZE);
        for (int j = 0; j < SECTOR_SIZE; j++) {
          memWrite8(dest + j, buf[j]);
        }
      }
      if (ok) {
        cpu.ax = (cpu.ax & 0xFF00) | al;
        cpu.flags &= ~FLAG_CF;
      } else {
        cpu.ax = (cpu.ax & 0xFF00) | 0x01;
        cpu.flags |= FLAG_CF;
      }
      break;
    }
    default:
      cpu.flags |= FLAG_CF;
      break;
  }
}

void int16_handler() {
  uint8_t ah = cpu.ax >> 8;
  switch (ah) {
    case 0x00:
      cpu.ax = 0x1C0D;
      break;
    case 0x01:
      cpu.flags |= FLAG_ZF;
      break;
    default:
      break;
  }
}

void int19_handler() {
  uint8_t buf[SECTOR_SIZE];
  if (!readSector(0, buf)) return;
  uint32_t base = segOff(0x0000, 0x7C00);
  for (int i = 0; i < SECTOR_SIZE; i++) {
    memWrite8(base + i, buf[i]);
  }
  cpu.cs = 0x0000;
  cpu.ip = 0x7C00;
}

// =========================
// INT DISPATCH
// =========================
void handleInterrupt(uint8_t intno) {
  switch (intno) {
    case 0x10: int10_handler(); break;
    case 0x13: int13_handler(); break;
    case 0x16: int16_handler(); break;
    case 0x19: int19_handler(); break;
    default: break;
  }
}

// =========================
// IN / OUT STUBS
// =========================
uint8_t ioRead8(uint16_t port) {
  (void)port;
  return 0xFF;
}

void ioWrite8(uint16_t port, uint8_t value) {
  (void)port;
  (void)value;
}

// =========================
// STRING OPS HELPERS
// =========================
uint16_t getSegForData() {
  if (segOverride) return *segOverride;
  return cpu.ds;
}

uint16_t getSegForDest() {
  if (segOverride) return *segOverride;
  return cpu.es;
}

// =========================
// CPU STEP
// =========================
void cpuStep() {
  uint32_t addr = segOff(cpu.cs, cpu.ip);
  uint8_t op = memRead8(addr);
  cpu.ip++;
  segOverride = nullptr;

  switch (op) {
    // Segment overrides
    case 0x26: segOverride = &cpu.es; break;
    case 0x2E: segOverride = &cpu.cs; break;
    case 0x36: segOverride = &cpu.ss; break;
    case 0x3E: segOverride = &cpu.ds; break;

    // NOP
    case 0x90: break;

    // MOV r16, imm16
    case 0xB8: case 0xB9: case 0xBA: case 0xBB:
    case 0xBC: case 0xBD: case 0xBE: case 0xBF: {
      uint8_t reg = op - 0xB8;
      uint16_t imm = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      R16(reg) = imm;
      break;
    }

    // MOV r/m16, r16 ; MOV r16, r/m16 (partial, reg-reg only)
    case 0x89:
    case 0x8B: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (mod == 3) {
        if (op == 0x89) {
          R16(rm) = R16(reg);
        } else {
          R16(reg) = R16(rm);
        }
      }
      break;
    }

    // MOV seg, r/m16 ; MOV r/m16, seg (partial, seg-reg only)
    case 0x8E:
    case 0x8C: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 3;
      uint8_t rm  = modrm & 7;
      if (mod == 3) {
        if (op == 0x8E) {
          segReg(reg) = R16(rm);
        } else {
          R16(rm) = segReg(reg);
        }
      }
      break;
    }

    // XOR r16, r16 (31 C0 etc, reg-reg)
    case 0x31: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (mod == 3) {
        uint16_t r = R16(rm) ^ R16(reg);
        R16(rm) = r;
        cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
        if (r == 0) cpu.flags |= FLAG_ZF;
        if (r & 0x8000) cpu.flags |= FLAG_SF;
      }
      break;
    }

    // OR AX, imm16
    case 0x0D: {
      uint16_t imm = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint32_t res = (uint32_t)cpu.ax | imm;
      cpu.ax = (uint16_t)res;
      setZF_SF_CF(res, cpu.ax, imm, false);
      break;
    }

    // AND AX, imm16
    case 0x25: {
      uint16_t imm = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint32_t res = (uint32_t)cpu.ax & imm;
      cpu.ax = (uint16_t)res;
      setZF_SF_CF(res, cpu.ax, imm, false);
      break;
    }

    // ADD AX, imm16
    case 0x05: {
      uint16_t imm = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint32_t res = (uint32_t)cpu.ax + imm;
      cpu.ax = (uint16_t)res;
      setZF_SF_CF(res, cpu.ax, imm, false);
      break;
    }

    // SUB AX, imm16
    case 0x2D: {
      uint16_t imm = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint32_t res = (uint32_t)cpu.ax - imm;
      cpu.ax = (uint16_t)res;
      setZF_SF_CF(res, cpu.ax, imm, true);
      break;
    }

    // CMP AX, imm16
    case 0x3D: {
      uint16_t imm = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint32_t res = (uint32_t)cpu.ax - imm;
      setZF_SF_CF(res, cpu.ax, imm, true);
      break;
    }

    // TEST AX, imm16
    case 0xA9: {
      uint16_t imm = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint16_t r = cpu.ax & imm;
      cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
      if (r == 0) cpu.flags |= FLAG_ZF;
      if (r & 0x8000) cpu.flags |= FLAG_SF;
      break;
    }

    // PUSH reg
    case 0x50: case 0x51: case 0x52: case 0x53:
    case 0x54: case 0x55: case 0x56: case 0x57: {
      uint8_t reg = op - 0x50;
      cpu.sp -= 2;
      memWrite16(segOff(cpu.ss, cpu.sp), R16(reg));
      break;
    }

    // POP reg
    case 0x58: case 0x59: case 0x5A: case 0x5B:
    case 0x5C: case 0x5D: case 0x5E: case 0x5F: {
      uint8_t reg = op - 0x58;
      R16(reg) = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      break;
    }

    // PUSHF
    case 0x9C:
      cpu.sp -= 2;
      memWrite16(segOff(cpu.ss, cpu.sp), cpu.flags);
      break;

    // POPF
    case 0x9D:
      cpu.flags = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      break;

    // CALL rel16
    case 0xE8: {
      int16_t rel = (int16_t)memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint16_t ret = cpu.ip;
      cpu.sp -= 2;
      memWrite16(segOff(cpu.ss, cpu.sp), ret);
      cpu.ip += rel;
      break;
    }

    // CALL ptr16:16
    case 0x9A: {
      uint16_t off = memRead16(segOff(cpu.cs, cpu.ip));
      uint16_t seg = memRead16(segOff(cpu.cs, cpu.ip + 2));
      cpu.ip += 4;
      cpu.sp -= 2;
      memWrite16(segOff(cpu.ss, cpu.sp), cpu.cs);
      cpu.sp -= 2;
      memWrite16(segOff(cpu.ss, cpu.sp), cpu.ip);
      cpu.cs = seg;
      cpu.ip = off;
      break;
    }

    // RET
    case 0xC3: {
      uint16_t ret = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      cpu.ip = ret;
      break;
    }

    // RETF
    case 0xCB: {
      uint16_t ip = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      uint16_t cs = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      cpu.ip = ip;
      cpu.cs = cs;
      break;
    }

    // IRET
    case 0xCF: {
      uint16_t ip = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      uint16_t cs = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      uint16_t fl = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      cpu.ip = ip;
      cpu.cs = cs;
      cpu.flags = fl;
      break;
    }

    // JMP short
    case 0xEB: {
      int8_t rel = (int8_t)memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      cpu.ip += rel;
      break;
    }

    // JMP near
    case 0xE9: {
      int16_t rel = (int16_t)memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      cpu.ip += rel;
      break;
    }

    // JMP far
    case 0xEA: {
      uint16_t off = memRead16(segOff(cpu.cs, cpu.ip));
      uint16_t seg = memRead16(segOff(cpu.cs, cpu.ip + 2));
      cpu.ip += 4;
      cpu.cs = seg;
      cpu.ip = off;
      break;
    }

    // JZ/JNZ/JC/JNC (short)
    case 0x74: { // JZ
      int8_t rel = (int8_t)memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      if (cpu.flags & FLAG_ZF) cpu.ip += rel;
      break;
    }
    case 0x75: { // JNZ
      int8_t rel = (int8_t)memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      if (!(cpu.flags & FLAG_ZF)) cpu.ip += rel;
      break;
    }
    case 0x72: { // JC
      int8_t rel = (int8_t)memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      if (cpu.flags & FLAG_CF) cpu.ip += rel;
      break;
    }
    case 0x73: { // JNC
      int8_t rel = (int8_t)memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      if (!(cpu.flags & FLAG_CF)) cpu.ip += rel;
      break;
    }

    // INT imm8 (fake stackless, but with IRET support if code uses it)
    case 0xCD: {
      uint8_t intno = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      handleInterrupt(intno);
      break;
    }

    // CLI / STI
    case 0xFA: break;
    case 0xFB: break;

    // IN AL, imm8
    case 0xE4: {
      uint8_t port = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      cpu.ax = (cpu.ax & 0xFF00) | ioRead8(port);
      break;
    }

    // OUT imm8, AL
    case 0xE6: {
      uint8_t port = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      ioWrite8(port, (uint8_t)(cpu.ax & 0xFF));
      break;
    }

    // MOVSB
    case 0xA4: {
      uint16_t srcSeg = getSegForData();
      uint8_t v = memRead8(segOff(srcSeg, cpu.si));
      memWrite8(segOff(getSegForDest(), cpu.di), v);
      cpu.si++;
      cpu.di++;
      break;
    }

    // MOVSW
    case 0xA5: {
      uint16_t srcSeg = getSegForData();
      uint16_t v = memRead16(segOff(srcSeg, cpu.si));
      memWrite16(segOff(getSegForDest(), cpu.di), v);
      cpu.si += 2;
      cpu.di += 2;
      break;
    }

    // STOSB
    case 0xAA: {
      memWrite8(segOff(getSegForDest(), cpu.di), (uint8_t)(cpu.ax & 0xFF));
      cpu.di++;
      break;
    }

    // STOSW
    case 0xAB: {
      memWrite16(segOff(getSegForDest(), cpu.di), cpu.ax);
      cpu.di += 2;
      break;
    }

    // REP prefix (only for MOVS/STOS)
    case 0xF3: {
      uint8_t next = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      switch (next) {
        case 0xA4: // REP MOVSB
          while (cpu.cx) {
            uint16_t srcSeg = getSegForData();
            uint8_t v = memRead8(segOff(srcSeg, cpu.si));
            memWrite8(segOff(getSegForDest(), cpu.di), v);
            cpu.si++;
            cpu.di++;
            cpu.cx--;
          }
          break;
        case 0xA5: // REP MOVSW
          while (cpu.cx) {
            uint16_t srcSeg = getSegForData();
            uint16_t v = memRead16(segOff(srcSeg, cpu.si));
            memWrite16(segOff(getSegForDest(), cpu.di), v);
            cpu.si += 2;
            cpu.di += 2;
            cpu.cx--;
          }
          break;
        case 0xAA: // REP STOSB
          while (cpu.cx) {
            memWrite8(segOff(getSegForDest(), cpu.di), (uint8_t)(cpu.ax & 0xFF));
            cpu.di++;
            cpu.cx--;
          }
          break;
        case 0xAB: // REP STOSW
          while (cpu.cx) {
            memWrite16(segOff(getSegForDest(), cpu.di), cpu.ax);
            cpu.di += 2;
            cpu.cx--;
          }
          break;
        default:
          break;
      }
      break;
    }

    default:
      bios_print("\r\nUnknown opcode ");
      {
        char buf[5];
        sprintf(buf, "%02X", op);
        bios_print(buf);
      }
      bios_print("\r\n");
      while (1) { renderTextScreen(); delay(1000); }
  }
}

// =========================
// BDA + IVT INIT
// =========================
void initBDAandIVT() {
  // BDA at 0040:0000
  uint32_t bda = segOff(0x0040, 0x0000);
  memWrite16(bda + 0x10, 0x0000); // equipment list
  memWrite16(bda + 0x13, 640);    // base memory in KB

  // IVT: we keep INTs in C, but set dummy vectors
  for (int i = 0; i < 256; i++) {
    memWrite16(i * 4 + 0, 0x0000);
    memWrite16(i * 4 + 2, 0xF000);
  }
}

// =========================
// BOOT SECTOR LOAD
// =========================
bool loadBootSectorTo0000_7C00() {
  uint8_t buf[SECTOR_SIZE];
  if (!readSector(0, buf)) return false;
  uint32_t base = segOff(0x0000, 0x7C00);
  for (int i = 0; i < SECTOR_SIZE; i++) {
    memWrite8(base + i, buf[i]);
  }
  return true;
}

// =========================
// SETUP (ADDED)
// =========================
void setup() {
  Serial.begin(115200);

  tft.init();                 // ST7789 init via TFT_eSPI
  tft.setRotation(1);         // 320x170 landscape (good for 80x25 clipping)
  tft.fillScreen(TFT_BLACK);

  clearScreenAttr(0x07);
  renderTextScreen();

  if (!SPIFFS.begin(true)) {
    bios_print("SPIFFS FAIL\r\n");
    renderTextScreen();
    return;
  }

  dosImg = SPIFFS.open(IMG_PATH, "r");
  if (!dosImg) {
    bios_print("NO IMG\r\n");
    renderTextScreen();
    return;
  }

  initBDAandIVT();

  if (!loadBootSectorTo0000_7C00()) {
    bios_print("BOOT LOAD FAIL\r\n");
    renderTextScreen();
    return;
  }

  cpuReset();
  bios_print("micro86 READY\r\n");
  renderTextScreen();
}

//LOOPS CURRENTLY////////////////////////////////////////
void loop() {
  // check for command to enter test module
  if (Serial.available()) {
    static char buf[8];
    static uint8_t idx = 0;
    char ch = Serial.read();

    if (ch == '\n' || ch == '\r') {
      buf[idx] = 0;
      idx = 0;

      if (strcmp(buf, "!TEST") == 0) {
        micro86_test_module();  // jump into last-section module
      }
    } 
    else if (idx < sizeof(buf) - 1) {
      buf[idx++] = ch;
    }
  }

  // normal DOS emulation loop
  for (int i = 0; i < 5000; i++) {
    cpuStep();
  }

  renderTextScreen();
  delay(16);
}

// =========================
// LAST SECTION: TEST MODULE
// =========================
void micro86_test_module() {
  clearScreenAttr(0x07);
  bios_print("micro86 TEST MODULE\r\n");
  bios_print("1 = step 1000\r\n");
  bios_print("2 = soft reboot (0000:7C00)\r\n");
  bios_print("3 = dump AX,BX,CX,DX\r\n");
  bios_print("Q = quit back\r\n> ");

  while (true) {
    if (Serial.available()) {
      char ch = Serial.read();

      if (ch == '1') {
        for (int i = 0; i < 1000; i++) cpuStep();
        bios_print("\r\n[stepped 1000]\r\n> ");
      } else if (ch == '2') {
        cpuReset();
        bios_print("\r\n[soft rebooted]\r\n> ");
      } else if (ch == '3') {
        char buf[64];
        sprintf(buf, "\r\nAX=%04X BX=%04X CX=%04X DX=%04X\r\n> ",
                cpu.ax, cpu.bx, cpu.cx, cpu.dx);
        bios_print(buf);
      } else if (ch == 'Q' || ch == 'q') {
        bios_print("\r\n[leaving test module]\r\n");
        return;  // back to normal loop()
      }
    }

    renderTextScreen();
    delay(10);
  }
}

// =========================
// OPCODE PACK BLOCK
// =========================

// 8-bit register access (AL,CL,DL,BL,AH,CH,DH,BH)
static uint8_t getReg8(uint8_t i) {
  switch (i & 7) {
    case 0: return (uint8_t)(cpu.ax & 0x00FF);       // AL
    case 1: return (uint8_t)(cpu.cx & 0x00FF);       // CL
    case 2: return (uint8_t)(cpu.dx & 0x00FF);       // DL
    case 3: return (uint8_t)(cpu.bx & 0x00FF);       // BL
    case 4: return (uint8_t)((cpu.ax >> 8) & 0x00FF); // AH
    case 5: return (uint8_t)((cpu.cx >> 8) & 0x00FF); // CH
    case 6: return (uint8_t)((cpu.dx >> 8) & 0x00FF); // DH
    case 7: return (uint8_t)((cpu.bx >> 8) & 0x00FF); // BH
  }
  return 0;
}

static void setReg8(uint8_t i, uint8_t v) {
  switch (i & 7) {
    case 0: cpu.ax = (cpu.ax & 0xFF00) | v; break;                 // AL
    case 1: cpu.cx = (cpu.cx & 0xFF00) | v; break;                 // CL
    case 2: cpu.dx = (cpu.dx & 0xFF00) | v; break;                 // DL
    case 3: cpu.bx = (cpu.bx & 0xFF00) | v; break;                 // BL
    case 4: cpu.ax = (cpu.ax & 0x00FF) | ((uint16_t)v << 8); break; // AH
    case 5: cpu.cx = (cpu.cx & 0x00FF) | ((uint16_t)v << 8); break; // CH
    case 6: cpu.dx = (cpu.dx & 0x00FF) | ((uint16_t)v << 8); break; // DH
    case 7: cpu.bx = (cpu.bx & 0x00FF) | ((uint16_t)v << 8); break; // BH
  }
}

// Effective address calculation for ModR/M (16-bit addressing)
static uint32_t eaCalc(uint8_t mod, uint8_t rm, uint16_t &segOut) {
  uint16_t base = 0;
  segOut = cpu.ds;

  switch (rm & 7) {
    case 0: base = cpu.bx + cpu.si; break; // [BX+SI]
    case 1: base = cpu.bx + cpu.di; break; // [BX+DI]
    case 2: base = cpu.bp + cpu.si; segOut = cpu.ss; break; // [BP+SI]
    case 3: base = cpu.bp + cpu.di; segOut = cpu.ss; break; // [BP+DI]
    case 4: base = cpu.si; break; // [SI]
    case 5: base = cpu.di; break; // [DI]
    case 6:
      if (mod == 0) {
        // direct [disp16]
        uint16_t disp = memRead16(segOff(cpu.cs, cpu.ip));
        cpu.ip += 2;
        base = disp;
        segOut = cpu.ds;
      } else {
        base = cpu.bp;
        segOut = cpu.ss;
      }
      break;
    case 7: base = cpu.bx; break; // [BX]
  }

  if (mod == 1) {
    int8_t disp8 = (int8_t)memRead8(segOff(cpu.cs, cpu.ip));
    cpu.ip++;
    base = (uint16_t)(base + disp8);
  } else if (mod == 2) {
    uint16_t disp16 = memRead16(segOff(cpu.cs, cpu.ip));
    cpu.ip += 2;
    base = (uint16_t)(base + disp16);
  }

  return segOff(segOut, base);
}

// Group1 ALU helpers (ADD, OR, ADC, SBB, AND, SUB, XOR, CMP) for 16-bit
static uint16_t alu16(uint8_t subop, uint16_t dst, uint16_t src) {
  uint32_t res = 0;
  switch (subop) {
    case 0: // ADD
      res = (uint32_t)dst + src;
      dst = (uint16_t)res;
      setZF_SF_CF(res, dst, src, false);
      break;
    case 1: // OR
      res = (uint32_t)dst | src;
      dst = (uint16_t)res;
      setZF_SF_CF(res, dst, src, false);
      cpu.flags &= ~FLAG_CF;
      break;
    case 4: // AND
      res = (uint32_t)dst & src;
      dst = (uint16_t)res;
      setZF_SF_CF(res, dst, src, false);
      cpu.flags &= ~FLAG_CF;
      break;
    case 5: // SUB
      res = (uint32_t)dst - src;
      dst = (uint16_t)res;
      setZF_SF_CF(res, dst, src, true);
      break;
    case 6: // XOR
      res = (uint32_t)dst ^ src;
      dst = (uint16_t)res;
      setZF_SF_CF(res, dst, src, false);
      cpu.flags &= ~FLAG_CF;
      break;
    case 7: // CMP
      res = (uint32_t)dst - src;
      setZF_SF_CF(res, dst, src, true);
      break;
    default:
      break;
  }
  return dst;
}

// Group1 ALU helpers for 8-bit
static uint8_t alu8(uint8_t subop, uint8_t dst, uint8_t src) {
  uint32_t res = 0;
  switch (subop) {
    case 0: // ADD
      res = (uint32_t)dst + src;
      dst = (uint8_t)res;
      cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
      if ((res & 0xFF) == 0) cpu.flags |= FLAG_ZF;
      if (res & 0x80) cpu.flags |= FLAG_SF;
      if (res & 0x100) cpu.flags |= FLAG_CF;
      break;
    case 1: // OR
      res = (uint32_t)dst | src;
      dst = (uint8_t)res;
      cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
      if (dst == 0) cpu.flags |= FLAG_ZF;
      if (dst & 0x80) cpu.flags |= FLAG_SF;
      break;
    case 4: // AND
      res = (uint32_t)dst & src;
      dst = (uint8_t)res;
      cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
      if (dst == 0) cpu.flags |= FLAG_ZF;
      if (dst & 0x80) cpu.flags |= FLAG_SF;
      break;
    case 5: // SUB
      res = (uint32_t)dst - src;
      dst = (uint8_t)res;
      cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
      if ((res & 0xFF) == 0) cpu.flags |= FLAG_ZF;
      if (res & 0x80) cpu.flags |= FLAG_SF;
      if (res & 0x100) cpu.flags |= FLAG_CF;
      break;
    case 6: // XOR
      res = (uint32_t)dst ^ src;
      dst = (uint8_t)res;
      cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
      if (dst == 0) cpu.flags |= FLAG_ZF;
      if (dst & 0x80) cpu.flags |= FLAG_SF;
      break;
    case 7: // CMP
      res = (uint32_t)dst - src;
      cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
      if ((res & 0xFF) == 0) cpu.flags |= FLAG_ZF;
      if (res & 0x80) cpu.flags |= FLAG_SF;
      if (res & 0x100) cpu.flags |= FLAG_CF;
      break;
    default:
      break;
  }
  return dst;
}

// Extended opcode handler block (to be merged logically with cpuStep switch)
void cpuStep_opcode_pack(uint8_t op) {
  switch (op) {

    // MOV r/m8, r8  / MOV r8, r/m8
    case 0x88:
    case 0x8A: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      uint16_t segEA;
      if (mod == 3) {
        uint8_t src, dst;
        if (op == 0x88) {
          src = getReg8(reg);
          setReg8(rm, src);
        } else {
          src = getReg8(rm);
          setReg8(reg, src);
        }
      } else {
        uint32_t ea = eaCalc(mod, rm, segEA);
        if (op == 0x88) {
          uint8_t v = getReg8(reg);
          memWrite8(ea, v);
        } else {
          uint8_t v = memRead8(ea);
          setReg8(reg, v);
        }
      }
      break;
    }

    // MOV r/m16, r16 / MOV r16, r/m16 (full, with memory)
    case 0x89:
    case 0x8B: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      uint16_t segEA;
      if (mod == 3) {
        if (op == 0x89) {
          R16(rm) = R16(reg);
        } else {
          R16(reg) = R16(rm);
        }
      } else {
        uint32_t ea = eaCalc(mod, rm, segEA);
        if (op == 0x89) {
          memWrite16(ea, R16(reg));
        } else {
          R16(reg) = memRead16(ea);
        }
      }
      break;
    }

    // MOV r/m8, imm8
    case 0xC6: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t sub = (modrm >> 3) & 7; // must be 0
      uint8_t rm  = modrm & 7;
      (void)sub;
      uint8_t imm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint16_t segEA;
      if (mod == 3) {
        setReg8(rm, imm);
      } else {
        uint32_t ea = eaCalc(mod, rm, segEA);
        memWrite8(ea, imm);
      }
      break;
    }

    // MOV r/m16, imm16
    case 0xC7: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t sub = (modrm >> 3) & 7; // must be 0
      uint8_t rm  = modrm & 7;
      (void)sub;
      uint16_t imm = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint16_t segEA;
      if (mod == 3) {
        R16(rm) = imm;
      } else {
        uint32_t ea = eaCalc(mod, rm, segEA);
        memWrite16(ea, imm);
      }
      break;
    }

    // MOV AX, [imm16] / MOV [imm16], AX
    case 0xA1: { // MOV AX, moffs16
      uint16_t off = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint32_t ea = segOff(getSegForData(), off);
      cpu.ax = memRead16(ea);
      break;
    }
    case 0xA3: { // MOV moffs16, AX
      uint16_t off = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint32_t ea = segOff(getSegForDest(), off);
      memWrite16(ea, cpu.ax);
      break;
    }

    // LEA r16, m
    case 0x8D: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      uint16_t segEA;
      uint32_t ea = eaCalc(mod, rm, segEA);
      uint16_t off = (uint16_t)(ea & 0xFFFF);
      R16(reg) = off;
      break;
    }

    // PUSH/POP segment registers
    case 0x06: // PUSH ES
      cpu.sp -= 2;
      memWrite16(segOff(cpu.ss, cpu.sp), cpu.es);
      break;
    case 0x0E: // PUSH CS
      cpu.sp -= 2;
      memWrite16(segOff(cpu.ss, cpu.sp), cpu.cs);
      break;
    case 0x16: // PUSH SS
      cpu.sp -= 2;
      memWrite16(segOff(cpu.ss, cpu.sp), cpu.ss);
      break;
    case 0x1E: // PUSH DS
      cpu.sp -= 2;
      memWrite16(segOff(cpu.ss, cpu.sp), cpu.ds);
      break;

    case 0x07: // POP ES
      cpu.es = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      break;
    case 0x17: // POP SS
      cpu.ss = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      break;
    case 0x1F: // POP DS
      cpu.ds = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      break;

    // INC/DEC r16
    case 0x40: case 0x41: case 0x42: case 0x43:
    case 0x44: case 0x45: case 0x46: case 0x47: {
      uint8_t reg = op - 0x40;
      uint16_t v = R16(reg);
      uint32_t res = (uint32_t)v + 1;
      v = (uint16_t)res;
      R16(reg) = v;
      setZF_SF_CF(res, v, 1, false);
      break;
    }
    case 0x48: case 0x49: case 0x4A: case 0x4B:
    case 0x4C: case 0x4D: case 0x4E: case 0x4F: {
      uint8_t reg = op - 0x48;
      uint16_t v = R16(reg);
      uint32_t res = (uint32_t)v - 1;
      v = (uint16_t)res;
      R16(reg) = v;
      setZF_SF_CF(res, v, 1, true);
      break;
    }

    // Group1 ALU r/m8, imm8
    case 0x80: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t sub = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      uint8_t imm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint16_t segEA;
      if (mod == 3) {
        uint8_t v = getReg8(rm);
        v = alu8(sub, v, imm);
        if (sub != 7) setReg8(rm, v);
      } else {
        uint32_t ea = eaCalc(mod, rm, segEA);
        uint8_t v = memRead8(ea);
        v = alu8(sub, v, imm);
        if (sub != 7) memWrite8(ea, v);
      }
      break;
    }

    // Group1 ALU r/m16, imm16
    case 0x81: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t sub = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      uint16_t imm = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint16_t segEA;
      if (mod == 3) {
        uint16_t v = R16(rm);
        v = alu16(sub, v, imm);
        if (sub != 7) R16(rm) = v;
      } else {
        uint32_t ea = eaCalc(mod, rm, segEA);
        uint16_t v = memRead16(ea);
        v = alu16(sub, v, imm);
        if (sub != 7) memWrite16(ea, v);
      }
      break;
    }

    // Group1 ALU r/m16, imm8 (sign-extended)
    case 0x83: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t sub = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      int8_t imm8 = (int8_t)memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint16_t imm = (uint16_t)imm8;
      uint16_t segEA;
      if (mod == 3) {
        uint16_t v = R16(rm);
        v = alu16(sub, v, imm);
        if (sub != 7) R16(rm) = v;
      } else {
        uint32_t ea = eaCalc(mod, rm, segEA);
        uint16_t v = memRead16(ea);
        v = alu16(sub, v, imm);
        if (sub != 7) memWrite16(ea, v);
      }
      break;
    }

    // RET imm16
    case 0xC2: {
      uint16_t imm = memRead16(segOff(cpu.cs, cpu.ip));
      cpu.ip += 2;
      uint16_t ip = memRead16(segOff(cpu.ss, cpu.sp));
      cpu.sp += 2;
      cpu.ip = ip;
      cpu.sp += imm;
      break;
    }

    default:
      // fall through to existing default handler in cpuStep
      break;
  }
}

// =========================
// OPCODE PACK BLOCK (PASTE AT END OF FILE)
// =========================

// 8-bit register access (AL,CL,DL,BL,AH,CH,DH,BH)
static uint8_t getReg8(uint8_t i) {
  switch (i & 7) {
    case 0: return (uint8_t)(cpu.ax & 0x00FF);        // AL
    case 1: return (uint8_t)(cpu.cx & 0x00FF);        // CL
    case 2: return (uint8_t)(cpu.dx & 0x00FF);        // DL
    case 3: return (uint8_t)(cpu.bx & 0x00FF);        // BL
    case 4: return (uint8_t)((cpu.ax >> 8) & 0x00FF); // AH
    case 5: return (uint8_t)((cpu.cx >> 8) & 0x00FF); // CH
    case 6: return (uint8_t)((cpu.dx >> 8) & 0x00FF); // DH
    case 7: return (uint8_t)((cpu.bx >> 8) & 0x00FF); // BH
  }
  return 0;
}

static void setReg8(uint8_t i, uint8_t v) {
  switch (i & 7) {
    case 0: cpu.ax = (cpu.ax & 0xFF00) | v; break;                  // AL
    case 1: cpu.cx = (cpu.cx & 0xFF00) | v; break;                  // CL
    case 2: cpu.dx = (cpu.dx & 0xFF00) | v; break;                  // DL
    case 3: cpu.bx = (cpu.bx & 0xFF00) | v; break;                  // BL
    case 4: cpu.ax = (cpu.ax & 0x00FF) | ((uint16_t)v << 8); break; // AH
    case 5: cpu.cx = (cpu.cx & 0x00FF) | ((uint16_t)v << 8); break; // CH
    case 6: cpu.dx = (cpu.dx & 0x00FF) | ((uint16_t)v << 8); break; // DH
    case 7: cpu.bx = (cpu.bx & 0x00FF) | ((uint16_t)v << 8); break; // BH
  }
}

// Effective address calculation for ModR/M (16-bit addressing)
static uint32_t eaCalc(uint8_t mod, uint8_t rm, uint16_t &segOut) {
  uint16_t base = 0;
  segOut = cpu.ds;

  switch (rm & 7) {
    case 0: base = cpu.bx + cpu.si; break;              // [BX+SI]
    case 1: base = cpu.bx + cpu.di; break;              // [BX+DI]
    case 2: base = cpu.bp + cpu.si; segOut = cpu.ss; break; // [BP+SI]
    case 3: base = cpu.bp + cpu.di; segOut = cpu.ss; break; // [BP+DI]
    case 4: base = cpu.si; break;                       // [SI]
    case 5: base = cpu.di; break;                       // [DI]
    case 6:
      if (mod == 0) {
        // direct 16-bit disp
        {
          uint16_t disp = memRead16(segOff(cpu.cs, cpu.ip));
          cpu.ip += 2;
          base = disp;
        }
      } else {
        base = cpu.bp;
        segOut = cpu.ss;
      }
      break;
    case 7: base = cpu.bx; break;                       // [BX]
  }

  if (mod == 1) {
    int8_t disp8 = (int8_t)memRead8(segOff(cpu.cs, cpu.ip));
    cpu.ip++;
    base = (uint16_t)(base + disp8);
  } else if (mod == 2) {
    uint16_t disp16 = memRead16(segOff(cpu.cs, cpu.ip));
    cpu.ip += 2;
    base = (uint16_t)(base + disp16);
  }

  return segOff(segOut, base);
}

// Memory helpers for r/m
static uint8_t readRm8(uint8_t mod, uint8_t rm) {
  if (mod == 3) {
    return getReg8(rm);
  } else {
    uint16_t seg;
    uint32_t ea = eaCalc(mod, rm, seg);
    return memRead8(ea);
  }
}

static void writeRm8(uint8_t mod, uint8_t rm, uint8_t v) {
  if (mod == 3) {
    setReg8(rm, v);
  } else {
    uint16_t seg;
    uint32_t ea = eaCalc(mod, rm, seg);
    memWrite8(ea, v);
  }
}

static uint16_t readRm16(uint8_t mod, uint8_t rm) {
  if (mod == 3) {
    return R16(rm);
  } else {
    uint16_t seg;
    uint32_t ea = eaCalc(mod, rm, seg);
    return memRead16(ea);
  }
}

static void writeRm16(uint8_t mod, uint8_t rm, uint16_t v) {
  if (mod == 3) {
    R16(rm) = v;
  } else {
    uint16_t seg;
    uint32_t ea = eaCalc(mod, rm, seg);
    memWrite16(ea, v);
  }
}

// Flag helpers for 8-bit ops
static void setFlagsLogic8(uint8_t r) {
  cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
  if (r == 0) cpu.flags |= FLAG_ZF;
  if (r & 0x80) cpu.flags |= FLAG_SF;
}

static void setFlagsAdd8(uint16_t res, uint8_t a, uint8_t b) {
  cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
  uint8_t r = (uint8_t)res;
  if (r == 0) cpu.flags |= FLAG_ZF;
  if (r & 0x80) cpu.flags |= FLAG_SF;
  if (res & 0x100) cpu.flags |= FLAG_CF;
}

static void setFlagsSub8(uint16_t res, uint8_t a, uint8_t b) {
  cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
  uint8_t r = (uint8_t)res;
  if (r == 0) cpu.flags |= FLAG_ZF;
  if (r & 0x80) cpu.flags |= FLAG_SF;
  if (res & 0x100) cpu.flags |= FLAG_CF;
}

// Flag helpers for 16-bit ops
static void setFlagsLogic16(uint16_t r) {
  cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
  if (r == 0) cpu.flags |= FLAG_ZF;
  if (r & 0x8000) cpu.flags |= FLAG_SF;
}

static void setFlagsAdd16(uint32_t res, uint16_t a, uint16_t b) {
  cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
  uint16_t r = (uint16_t)res;
  if (r == 0) cpu.flags |= FLAG_ZF;
  if (r & 0x8000) cpu.flags |= FLAG_SF;
  if (res & 0x10000) cpu.flags |= FLAG_CF;
}

static void setFlagsSub16(uint32_t res, uint16_t a, uint16_t b) {
  cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
  uint16_t r = (uint16_t)res;
  if (r == 0) cpu.flags |= FLAG_ZF;
  if (r & 0x8000) cpu.flags |= FLAG_SF;
  if (res & 0x10000) cpu.flags |= FLAG_CF;
}

// Extra opcode executor (to be used from cpuStep switch if wired)
static bool execOpcodePack(uint8_t op) {
  switch (op) {
    // MOV r8, imm8
    case 0xB0: case 0xB1: case 0xB2: case 0xB3:
    case 0xB4: case 0xB5: case 0xB6: case 0xB7: {
      uint8_t reg = op - 0xB0;
      uint8_t imm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      setReg8(reg, imm);
      return true;
    }

    // MOV r/m8, r8  (88 /r)
    // MOV r8, r/m8  (8A /r)
    case 0x88:
    case 0x8A: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (op == 0x88) {
        uint8_t v = getReg8(reg);
        writeRm8(mod, rm, v);
      } else {
        uint8_t v = readRm8(mod, rm);
        setReg8(reg, v);
      }
      return true;
    }

    // ADD r/m8, r8 (00 /r)
    // ADD r8, r/m8 (02 /r)
    case 0x00:
    case 0x02: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (op == 0x00) {
        uint8_t dst = readRm8(mod, rm);
        uint8_t src = getReg8(reg);
        uint16_t res = (uint16_t)dst + (uint16_t)src;
        setFlagsAdd8(res, dst, src);
        writeRm8(mod, rm, (uint8_t)res);
      } else {
        uint8_t dst = getReg8(reg);
        uint8_t src = readRm8(mod, rm);
        uint16_t res = (uint16_t)dst + (uint16_t)src;
        setFlagsAdd8(res, dst, src);
        setReg8(reg, (uint8_t)res);
      }
      return true;
    }

    // ADD r/m16, r16 (01 /r)
    // ADD r16, r/m16 (03 /r)
    case 0x01:
    case 0x03: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (op == 0x01) {
        uint16_t dst = readRm16(mod, rm);
        uint16_t src = R16(reg);
        uint32_t res = (uint32_t)dst + (uint32_t)src;
        setFlagsAdd16(res, dst, src);
        writeRm16(mod, rm, (uint16_t)res);
      } else {
        uint16_t dst = R16(reg);
        uint16_t src = readRm16(mod, rm);
        uint32_t res = (uint32_t)dst + (uint32_t)src;
        setFlagsAdd16(res, dst, src);
        R16(reg) = (uint16_t)res;
      }
      return true;
    }

    // SUB r/m8, r8 (28 /r)
    // SUB r8, r/m8 (2A /r)
    case 0x28:
    case 0x2A: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (op == 0x28) {
        uint8_t dst = readRm8(mod, rm);
        uint8_t src = getReg8(reg);
        uint16_t res = (uint16_t)dst - (uint16_t)src;
        setFlagsSub8(res, dst, src);
        writeRm8(mod, rm, (uint8_t)res);
      } else {
        uint8_t dst = getReg8(reg);
        uint8_t src = readRm8(mod, rm);
        uint16_t res = (uint16_t)dst - (uint16_t)src;
        setFlagsSub8(res, dst, src);
        setReg8(reg, (uint8_t)res);
      }
      return true;
    }

    // SUB r/m16, r16 (29 /r)
    // SUB r16, r/m16 (2B /r)
    case 0x29:
    case 0x2B: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (op == 0x29) {
        uint16_t dst = readRm16(mod, rm);
        uint16_t src = R16(reg);
        uint32_t res = (uint32_t)dst - (uint32_t)src;
        setFlagsSub16(res, dst, src);
        writeRm16(mod, rm, (uint16_t)res);
      } else {
        uint16_t dst = R16(reg);
        uint16_t src = readRm16(mod, rm);
        uint32_t res = (uint32_t)dst - (uint32_t)src;
        setFlagsSub16(res, dst, src);
        R16(reg) = (uint16_t)res;
      }
      return true;
    }

    // CMP r/m8, r8 (38 /r)
    // CMP r8, r/m8 (3A /r)
    case 0x38:
    case 0x3A: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      uint8_t a, b;
      if (op == 0x38) {
        a = readRm8(mod, rm);
        b = getReg8(reg);
      } else {
        a = getReg8(reg);
        b = readRm8(mod, rm);
      }
      uint16_t res = (uint16_t)a - (uint16_t)b;
      setFlagsSub8(res, a, b);
      return true;
    }

    // CMP r/m16, r16 (39 /r)
    // CMP r16, r/m16 (3B /r)
    case 0x39:
    case 0x3B: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      uint16_t a, b;
      if (op == 0x39) {
        a = readRm16(mod, rm);
        b = R16(reg);
      } else {
        a = R16(reg);
        b = readRm16(mod, rm);
      }
      uint32_t res = (uint32_t)a - (uint32_t)b;
      setFlagsSub16(res, a, b);
      return true;
    }

    // AND r/m8, r8 (20 /r)
    // AND r8, r/m8 (22 /r)
    case 0x20:
    case 0x22: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (op == 0x20) {
        uint8_t dst = readRm8(mod, rm);
        uint8_t src = getReg8(reg);
        uint8_t r = dst & src;
        setFlagsLogic8(r);
        writeRm8(mod, rm, r);
      } else {
        uint8_t dst = getReg8(reg);
        uint8_t src = readRm8(mod, rm);
        uint8_t r = dst & src;
        setFlagsLogic8(r);
        setReg8(reg, r);
      }
      return true;
    }

    // AND r/m16, r16 (21 /r)
    // AND r16, r/m16 (23 /r)
    case 0x21:
    case 0x23: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (op == 0x21) {
        uint16_t dst = readRm16(mod, rm);
        uint16_t src = R16(reg);
        uint16_t r = dst & src;
        setFlagsLogic16(r);
        writeRm16(mod, rm, r);
      } else {
        uint16_t dst = R16(reg);
        uint16_t src = readRm16(mod, rm);
        uint16_t r = dst & src;
        setFlagsLogic16(r);
        R16(reg) = r;
      }
      return true;
    }

    // OR r/m8, r8 (08 /r)
    // OR r8, r/m8 (0A /r)
    case 0x08:
    case 0x0A: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (op == 0x08) {
        uint8_t dst = readRm8(mod, rm);
        uint8_t src = getReg8(reg);
        uint8_t r = dst | src;
        setFlagsLogic8(r);
        writeRm8(mod, rm, r);
      } else {
        uint8_t dst = getReg8(reg);
        uint8_t src = readRm8(mod, rm);
        uint8_t r = dst | src;
        setFlagsLogic8(r);
        setReg8(reg, r);
      }
      return true;
    }

    // OR r/m16, r16 (09 /r)
    // OR r16, r/m16 (0B /r)
    case 0x09:
    case 0x0B: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (op == 0x09) {
        uint16_t dst = readRm16(mod, rm);
        uint16_t src = R16(reg);
        uint16_t r = dst | src;
        setFlagsLogic16(r);
        writeRm16(mod, rm, r);
      } else {
        uint16_t dst = R16(reg);
        uint16_t src = readRm16(mod, rm);
        uint16_t r = dst | src;
        setFlagsLogic16(r);
        R16(reg) = r;
      }
      return true;
    }

    // XOR r/m8, r8 (30 /r)
    // XOR r8, r/m8 (32 /r)
    case 0x30:
    case 0x32: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (op == 0x30) {
        uint8_t dst = readRm8(mod, rm);
        uint8_t src = getReg8(reg);
        uint8_t r = dst ^ src;
        setFlagsLogic8(r);
        writeRm8(mod, rm, r);
      } else {
        uint8_t dst = getReg8(reg);
        uint8_t src = readRm8(mod, rm);
        uint8_t r = dst ^ src;
        setFlagsLogic8(r);
        setReg8(reg, r);
      }
      return true;
    }

    // XOR r/m16, r16 (31 /r) already partially handled for reg-reg,
    // but we cover full r/m16 here if needed
    // XOR r16, r/m16 (33 /r)
    case 0x33: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      uint16_t dst = R16(reg);
      uint16_t src = readRm16(mod, rm);
      uint16_t r = dst ^ src;
      setFlagsLogic16(r);
      R16(reg) = r;
      return true;
    }

    // TEST r/m8, r8 (84 /r)
    // TEST r/m16, r16 (85 /r)
    case 0x84:
    case 0x85: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (op == 0x84) {
        uint8_t a = readRm8(mod, rm);
        uint8_t b = getReg8(reg);
        uint8_t r = a & b;
        setFlagsLogic8(r);
      } else {
        uint16_t a = readRm16(mod, rm);
        uint16_t b = R16(reg);
        uint16_t r = a & b;
        setFlagsLogic16(r);
      }
      return true;
    }

    // MOV r/m16, imm16 (C7 /0)
    case 0xC7: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (reg == 0) {
        uint16_t imm = memRead16(segOff(cpu.cs, cpu.ip));
        cpu.ip += 2;
        writeRm16(mod, rm, imm);
      }
      return true;
    }

    // MOV r/m8, imm8 (C6 /0)
    case 0xC6: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (reg == 0) {
        uint8_t imm = memRead8(segOff(cpu.cs, cpu.ip));
        cpu.ip++;
        writeRm8(mod, rm, imm);
      }
      return true;
    }

    // INC r16 (40+rw)
    case 0x40: case 0x41: case 0x42: case 0x43:
    case 0x44: case 0x45: case 0x46: case 0x47: {
      uint8_t reg = op - 0x40;
      uint16_t v = R16(reg);
      uint32_t res = (uint32_t)v + 1;
      setFlagsAdd16(res, v, 1);
      R16(reg) = (uint16_t)res;
      return true;
    }

    // DEC r16 (48+rw)
    case 0x48: case 0x49: case 0x4A: case 0x4B:
    case 0x4C: case 0x4D: case 0x4E: case 0x4F: {
      uint8_t reg = op - 0x48;
      uint16_t v = R16(reg);
      uint32_t res = (uint32_t)v - 1;
      setFlagsSub16(res, v, 1);
      R16(reg) = (uint16_t)res;
      return true;
    }

    // XCHG AX, r16 (90+rw)
    case 0x91: case 0x92: case 0x93:
    case 0x94: case 0x95: case 0x96: case 0x97: {
      uint8_t reg = op - 0x90;
      uint16_t tmp = R16(reg);
      R16(reg) = cpu.ax;
      cpu.ax = tmp;
      return true;
    }

    // LEA r16, m (8D /r)
    case 0x8D: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (mod == 3) {
        // LEA with register is undefined, ignore
        return true;
      }
      uint16_t seg;
      uint32_t ea = eaCalc(mod, rm, seg);
      R16(reg) = (uint16_t)(ea & 0xFFFF);
      return true;
    }

    // PUSH r/m16 (FF /6)
    // INC/DEC r/m16 etc not fully implemented, but PUSH r/m16 is common
    case 0xFF: {
      uint8_t modrm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      uint8_t mod = (modrm >> 6) & 3;
      uint8_t reg = (modrm >> 3) & 7;
      uint8_t rm  = modrm & 7;
      if (reg == 6) { // PUSH r/m16
        uint16_t v = readRm16(mod, rm);
        cpu.sp -= 2;
        memWrite16(segOff(cpu.ss, cpu.sp), v);
      }
      return true;
    }

    default:
      return false;
  }
}


/* ============================================================
   EXTENDED OPCODE PACK — DROP-IN BLOCK
   Paste at the VERY END of your .ino file
   ============================================================ */

// 8-bit register access
static uint8_t getReg8(uint8_t i) {
  switch (i & 7) {
    case 0: return cpu.ax & 0xFF;          // AL
    case 1: return cpu.cx & 0xFF;          // CL
    case 2: return cpu.dx & 0xFF;          // DL
    case 3: return cpu.bx & 0xFF;          // BL
    case 4: return (cpu.ax >> 8) & 0xFF;   // AH
    case 5: return (cpu.cx >> 8) & 0xFF;   // CH
    case 6: return (cpu.dx >> 8) & 0xFF;   // DH
    case 7: return (cpu.bx >> 8) & 0xFF;   // BH
  }
  return 0;
}

static void setReg8(uint8_t i, uint8_t v) {
  switch (i & 7) {
    case 0: cpu.ax = (cpu.ax & 0xFF00) | v; break;
    case 1: cpu.cx = (cpu.cx & 0xFF00) | v; break;
    case 2: cpu.dx = (cpu.dx & 0xFF00) | v; break;
    case 3: cpu.bx = (cpu.bx & 0xFF00) | v; break;
    case 4: cpu.ax = (cpu.ax & 0x00FF) | (v << 8); break;
    case 5: cpu.cx = (cpu.cx & 0x00FF) | (v << 8); break;
    case 6: cpu.dx = (cpu.dx & 0x00FF) | (v << 8); break;
    case 7: cpu.bx = (cpu.bx & 0x00FF) | (v << 8); break;
  }
}

// Effective address calc
static uint32_t eaCalc(uint8_t mod, uint8_t rm, uint16_t &segOut) {
  uint16_t base = 0;
  segOut = cpu.ds;

  switch (rm & 7) {
    case 0: base = cpu.bx + cpu.si; break;
    case 1: base = cpu.bx + cpu.di; break;
    case 2: base = cpu.bp + cpu.si; segOut = cpu.ss; break;
    case 3: base = cpu.bp + cpu.di; segOut = cpu.ss; break;
    case 4: base = cpu.si; break;
    case 5: base = cpu.di; break;
    case 6:
      if (mod == 0) {
        uint16_t disp16 = memRead16(segOff(cpu.cs, cpu.ip));
        cpu.ip += 2;
        base = disp16;
      } else {
        base = cpu.bp;
        segOut = cpu.ss;
      }
      break;
    case 7: base = cpu.bx; break;
  }

  if (mod == 1) {
    int8_t disp8 = (int8_t)memRead8(segOff(cpu.cs, cpu.ip));
    cpu.ip++;
    base += disp8;
  } else if (mod == 2) {
    uint16_t disp16 = memRead16(segOff(cpu.cs, cpu.ip));
    cpu.ip += 2;
    base += disp16;
  }

  return segOff(segOut, base);
}

// r/m8 helpers
static uint8_t readRM8(uint8_t mod, uint8_t rm) {
  if (mod == 3) return getReg8(rm);
  uint16_t seg; uint32_t ea = eaCalc(mod, rm, seg);
  return memRead8(ea);
}

static void writeRM8(uint8_t mod, uint8_t rm, uint8_t v) {
  if (mod == 3) { setReg8(rm, v); return; }
  uint16_t seg; uint32_t ea = eaCalc(mod, rm, seg);
  memWrite8(ea, v);
}

// r/m16 helpers
static uint16_t readRM16(uint8_t mod, uint8_t rm) {
  if (mod == 3) return R16(rm);
  uint16_t seg; uint32_t ea = eaCalc(mod, rm, seg);
  return memRead16(ea);
}

static void writeRM16(uint8_t mod, uint8_t rm, uint16_t v) {
  if (mod == 3) { R16(rm) = v; return; }
  uint16_t seg; uint32_t ea = eaCalc(mod, rm, seg);
  memWrite16(ea, v);
}

// 8-bit flag helper
static void setZF_SF_CF_8(uint32_t res, uint8_t a, uint8_t b, bool sub) {
  cpu.flags &= ~(FLAG_ZF | FLAG_SF | FLAG_CF);
  if ((res & 0xFF) == 0) cpu.flags |= FLAG_ZF;
  if (res & 0x80) cpu.flags |= FLAG_SF;
  if (sub) { if (res & 0x100) cpu.flags |= FLAG_CF; }
  else     { if (res & 0x100) cpu.flags |= FLAG_CF; }
}

// EXTENDED CPU STEP
static void cpuStep_full() {
  uint32_t addr = segOff(cpu.cs, cpu.ip);
  uint8_t op = memRead8(addr);
  cpu.ip++;

  switch (op) {

    // MOV r8, imm8
    case 0xB0: case 0xB1: case 0xB2: case 0xB3:
    case 0xB4: case 0xB5: case 0xB6: case 0xB7: {
      uint8_t reg = op - 0xB0;
      uint8_t imm = memRead8(segOff(cpu.cs, cpu.ip));
      cpu.ip++;
      setReg8(reg, imm);
      break;
    }

    // MOV r/m8, r8  | MOV r8, r/m8
    case 0x88:
    case 0x8A: {
      uint8_t m = memRead8(segOff(cpu.cs, cpu.ip)); cpu.ip++;
      uint8_t mod = (m >> 6) & 3, reg = (m >> 3) & 7, rm = m & 7;

      if (op == 0x88) writeRM8(mod, rm, getReg8(reg));
      else            setReg8(reg, readRM8(mod, rm));
      break;
    }

    // ADD r/m8,r8 | ADD r8,r/m8
    case 0x00:
    case 0x02: {
      uint8_t m = memRead8(segOff(cpu.cs, cpu.ip)); cpu.ip++;
      uint8_t mod = (m >> 6) & 3, reg = (m >> 3) & 7, rm = m & 7;

      if (op == 0x00) {
        uint8_t d = readRM8(mod, rm), s = getReg8(reg);
        uint32_t r = d + s;
        writeRM8(mod, rm, r);
        setZF_SF_CF_8(r, d, s, false);
      } else {
        uint8_t d = getReg8(reg), s = readRM8(mod, rm);
        uint32_t r = d + s;
        setReg8(reg, r);
        setZF_SF_CF_8(r, d, s, false);
      }
      break;
    }

    // SUB r/m8,r8 | SUB r8,r/m8
    case 0x28:
    case 0x2A: {
      uint8_t m = memRead8(segOff(cpu.cs, cpu.ip)); cpu.ip++;
      uint8_t mod = (m >> 6) & 3, reg = (m >> 3) & 7, rm = m & 7;

      if (op == 0x28) {
        uint8_t d = readRM8(mod, rm), s = getReg8(reg);
        uint32_t r = d - s;
        writeRM8(mod, rm, r);
        setZF_SF_CF_8(r, d, s, true);
      } else {
        uint8_t d = getReg8(reg), s = readRM8(mod, rm);
        uint32_t r = d - s;
        setReg8(reg, r);
        setZF_SF_CF_8(r, d, s, true);
      }
      break;
    }

    // CMP r/m8,r8 | CMP r8,r/m8
    case 0x38:
    case 0x3A: {
      uint8_t m = memRead8(segOff(cpu.cs, cpu.ip)); cpu.ip++;
      uint8_t mod = (m >> 6) & 3, reg = (m >> 3) & 7, rm = m & 7;

      uint8_t d = (op == 0x38) ? readRM8(mod, rm) : getReg8(reg);
      uint8_t s = (op == 0x38) ? getReg8(reg) : readRM8(mod, rm);
      uint32_t r = d - s;
      setZF_SF_CF_8(r, d, s, true);
      break;
    }

    // OR r/m8,r8 | OR r8,r/m8
    case 0x08:
    case 0x0A: {
      uint8_t m = memRead8(segOff(cpu.cs, cpu.ip)); cpu.ip++;
      uint8_t mod = (m >> 6) & 3, reg = (m >> 3) & 7, rm = m & 7;

      uint8_t d = (op == 0x08) ? readRM8(mod, rm) : getReg8(reg);
      uint8_t s = (op == 0x08) ? getReg8(reg) : readRM8(mod, rm);
      uint8_t r = d | s;

      if (op == 0x08) writeRM8(mod, rm, r);
      else            setReg8(reg, r);

      cpu.flags &= ~(FLAG_CF | FLAG_ZF | FLAG_SF);
      if (r == 0) cpu.flags |= FLAG_ZF;
      if (r & 0x80) cpu.flags |= FLAG_SF;
      break;
    }

    // AND r/m8,r8 | AND r8,r/m8
    case 0x20:
    case 0x22: {
      uint8_t m = memRead8(segOff(cpu.cs, cpu.ip)); cpu.ip++;
      uint8_t mod = (m >> 6) & 3, reg = (m >> 3) & 7, rm = m & 7;

      uint8_t d = (op == 0x20) ? readRM8(mod, rm) : getReg8(reg);
      uint8_t s = (op == 0x20) ? getReg8(reg) : readRM8(mod, rm);
      uint8_t r = d & s;

      if (op == 0x20) writeRM8(mod, rm, r);
      else            setReg8(reg, r);

      cpu.flags &= ~(FLAG_CF | FLAG_ZF | FLAG_SF);
      if (r == 0) cpu.flags |= FLAG_ZF;
      if (r & 0x80) cpu.flags |= FLAG_SF;
      break;
    }

    // XOR r/m8,r8 | XOR r8,r/m8
    case 0x30:
    case 0x32: {
      uint8_t m = memRead8(segOff(cpu.cs, cpu.ip)); cpu.ip++;
      uint8_t mod = (m >> 6) & 3, reg = (m >> 3) & 7, rm = m & 7;

      uint8_t d = (op == 0x30) ? readRM8(mod, rm) : getReg8(reg);
      uint8_t s = (op == 0x30) ? getReg8(reg) : readRM8(mod, rm);
      uint8_t r = d ^ s;

      if (op == 0x30) writeRM8(mod, rm, r);
      else            setReg8(reg, r);

      cpu.flags &= ~(FLAG_CF | FLAG_ZF | FLAG_SF);
      if (r == 0) cpu.flags |= FLAG_ZF;
      if (r & 0x80) cpu.flags |= FLAG_SF;
      break;
    }

    // fallback → original cpuStep
    default:
      cpu.ip--;
      cpuStep();
      break;
  }
}

// OVERRIDE cpuStep() with extended version
void cpuStep() {
  cpuStep_full();
}