#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();

// =========================
// 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;
}


//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);
  }
}