/****
 QPAD D21 Mini Mario (v12 clean build)
 - Fixes compilation (balanced braces), improves flag grab (slide guaranteed), and advances to next level.
 - Keeps your mapping: P3(5)=LEFT (idx 3), P4(6)=RIGHT (idx 4), P0(2)=JUMP (idx 0).
 - Touch driver remains unchanged (Adafruit_FreeTouch); we only interpret it in the game layer.
 - Includes: adaptive thresholds, robust collisions, unstuck, offscreen watchdog, coins, enemies, coyote, jump buffer.
****/

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_FreeTouch.h>
#include <math.h>

// ================= OLED =================
#define SCREEN_WIDTH   128
#define SCREEN_HEIGHT  64
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1, 1000000UL, 1000000UL);

// ================= TOUCH (unchanged driver) =================
#define N_TOUCH   6
#define THRESHOLD 500
uint8_t touch_pins[N_TOUCH] = {2,3,4,5,6,7};  // P0..P5 -> pins (2,3,4,5,6,7)
Adafruit_FreeTouch* touch_devices[N_TOUCH];

int  touch_values[N_TOUCH]     = {0,0,0,0,0,0};
bool pin_touched_now[N_TOUCH]  = {false,false,false,false,false,false};
bool pin_touched_past[N_TOUCH] = {false,false,false,false,false,false};

void init_touch(){
  for (int i=0;i<N_TOUCH;i++){
    touch_devices[i] = new Adafruit_FreeTouch(touch_pins[i], OVERSAMPLE_32, RESISTOR_0, FREQ_MODE_NONE);
    if (!touch_devices[i]->begin()){
      // leave as 0 if not present
    }
  }
}
void update_touch(){
  for (int i=0;i<N_TOUCH;i++){
    pin_touched_past[i] = pin_touched_now[i];
    int v = 0;
    if (touch_devices[i]) v = touch_devices[i]->measure();
    touch_values[i] = v;
    pin_touched_now[i] = (v > THRESHOLD);
  }
}

// ---- Adaptive game thresholds (does not modify touch driver) ----
#define USE_ADAPTIVE_THRESH 1
#define CAL_SAMPLES         64
#define CAL_DELTA_DEFAULT   60
int16_t cal_delta[N_TOUCH] = {60, 60, 60, 60, 60, 60};
int16_t baseline[N_TOUCH] = {0,0,0,0,0,0};
int16_t game_thresh[N_TOUCH] = {0,0,0,0,0,0};

void calibrateTouch(){
  for (int i=0;i<N_TOUCH;i++){ baseline[i]=0; game_thresh[i]=0; }
  for (int s=0; s<CAL_SAMPLES; ++s){
    for (int i=0;i<N_TOUCH;i++){
      int v = 0;
      if (touch_devices[i]) v = touch_devices[i]->measure();
      baseline[i] += v;
    }
    delay(5);
  }
  for (int i=0;i<N_TOUCH;i++){
    baseline[i] = baseline[i] / CAL_SAMPLES;
    int16_t d = cal_delta[i] <= 0 ? CAL_DELTA_DEFAULT : cal_delta[i];
    game_thresh[i] = baseline[i] + d;
  }
}

// ================= GAME CONFIG =================
#define FPS                30
#define FRAME_MS           (1000 / FPS)
#define GRAVITY            0.45f
#define JUMP_VELOCITY      -4.8f
#define MAX_FALL_SPEED     5.5f
#define MOVE_SPEED         1.2f

#define TILE_W             8
#define TILE_H             8
#define MAP_W_TILES        256
#define MAP_H_TILES        8
#define GROUND_ROW         7

#define PLAYER_W           8
#define PLAYER_H           8
#define PLAYER_START_X     16
#define PLAYER_START_Y     (SCREEN_HEIGHT - PLAYER_H - 1 - 8)

#define MAX_JUMP_FRAMES     10
#define COYOTE_FRAMES        6
#define JUMP_BUFFER_FRAMES   6
#define CAM_SCROLL_SPEED     1

// Input mapping (per your legend)
#define IDX_LEFT   3   // P3(5)
#define IDX_RIGHT  4   // P4(6)
#define IDX_JUMP   0   // P0(2)
#define INVERT_X   0   // normal

#define PRESS_MARGIN 80  // fallback if adaptive disabled

// Safety / robustness
#define SAFE_WHILE_GUARD   32
#define MAX_OFFSCREEN_FRAMES 30

// Gameplay extras
#define MAX_LIVES  3
#define ENEMY_W    8
#define ENEMY_H    8
#define MAX_ENEMIES 8
#define ENEMY_SPEED  0.6f
#define COIN_W     6
#define COIN_H     6
#define SHAKE_TIME  8

// ================= SPRITES =================
const uint8_t PROGMEM player_stand[] = {
  0b00011000, 0b00111100, 0b00111100, 0b00011000,
  0b00111100, 0b01111110, 0b01011010, 0b00100100
};
const uint8_t PROGMEM player_run1[] = {
  0b00011000, 0b00111100, 0b00111100, 0b00011000,
  0b00111100, 0b01111110, 0b00100100, 0b01000010
};
const uint8_t PROGMEM player_run2[] = {
  0b00011000, 0b00111100, 0b00111100, 0b00011000,
  0b00111100, 0b01111110, 0b01000010, 0b00100100
};
const uint8_t PROGMEM tile_block[] = {
  0b11111111, 0b10000001, 0b10111101, 0b10100101,
  0b10111101, 0b10000001, 0b11111111, 0b11111111
};
const uint8_t PROGMEM coin_sprite[] = {
  0b00110000,
  0b01111000,
  0b11111100,
  0b11111100,
  0b01111000,
  0b00110000,
};
const uint8_t PROGMEM enemy_sprite[] = {
  0b00111100,
  0b01111110,
  0b11100111,
  0b11100111,
  0b01111110,
  0b00111100,
  0b01000010,
  0b10000001
};

// ================= STATE =================
uint8_t world[MAP_H_TILES][MAP_W_TILES];       // solid tiles
uint8_t coins_map[MAP_H_TILES][MAP_W_TILES];   // coin presence (0/1)

struct Enemy {
  int16_t worldX;  // 0.. worldWidthPixels-1
  float y;         // screen-space Y
  float vx;
  float vy;
  bool alive;
};
Enemy enemies[MAX_ENEMIES];

int16_t camX = 0;
uint32_t score = 0;
uint16_t coins = 0;
uint8_t lives = MAX_LIVES;
uint8_t levelNo = 1;

float px = PLAYER_START_X;   // screen-space X
float py = PLAYER_START_Y;   // screen-space Y
float vx = 0.f;
float vy = 0.f;
bool onGround = false;
uint8_t jumpFrames = 0;
uint8_t coyote = 0;
uint8_t jumpBuffer = 0;
uint32_t animCounter = 0;
bool prevJump=false;
uint8_t shakeTimer = 0;
uint8_t offscreenFrames = 0;

// Level end / flagpole
#define FLAG_TILE_X   (MAP_W_TILES - 6) // near end
#define FLAG_W        6
#define FLAG_H_PIX    (MAP_H_TILES * TILE_H)
#define CASTLE_TILE_X (MAP_W_TILES - 2)

enum GameMode { MODE_PLAY=0, MODE_FLAG_SLIDE=1, MODE_CLEARED=2 };
GameMode gameMode = MODE_PLAY;
uint16_t clearTimer = 0;

// ================= HELPERS =================
static inline int16_t worldWidthPixels(){ return MAP_W_TILES * TILE_W; }
static inline int16_t worldHeightPixels(){ return MAP_H_TILES * TILE_H; }

int16_t flagWorldX(){ return FLAG_TILE_X * TILE_W; }
int16_t castleWorldX(){ return CASTLE_TILE_X * TILE_W; }

int16_t playerGroundTopAtTile(int16_t tx){
  if (tx < 0) return SCREEN_HEIGHT - TILE_H;
  tx %= MAP_W_TILES; if (tx < 0) tx += MAP_W_TILES;
  for (int ty=0; ty<MAP_H_TILES; ++ty){
    if (world[ty][tx]) return ty*TILE_H - PLAYER_H;
  }
  return (GROUND_ROW*TILE_H) - PLAYER_H;
}

bool rectCollidesSolid(int16_t xscr, int16_t y, int16_t w, int16_t h){
  // Map rect to world space along X by adding camX, then iterate overlapped tiles
  int32_t x0w = (int32_t)xscr + (int32_t)camX;
  int32_t x1w = (int32_t)xscr + (int32_t)w - 1 + (int32_t)camX;

  if (y + h - 1 < 0) return false;
  if (y >= worldHeightPixels()) return false;
  int16_t ty0 = y / TILE_H; if (ty0 < 0) ty0 = 0;
  int16_t ty1 = (y + h - 1) / TILE_H; if (ty1 >= MAP_H_TILES) ty1 = MAP_H_TILES - 1;

  int32_t t0 = (x0w >= 0) ? (x0w / TILE_W) : ((x0w - (TILE_W-1)) / TILE_W);
  int32_t t1 = (x1w >= 0) ? (x1w / TILE_W) : ((x1w - (TILE_W-1)) / TILE_W);

  for (int16_t ty = ty0; ty <= ty1; ++ty){
    for (int32_t tx = t0; tx <= t1; ++tx){
      int16_t tw = (int16_t)(tx % MAP_W_TILES);
      if (tw < 0) tw += MAP_W_TILES;
      if (world[ty][tw]) return true;
    }
  }
  return false;
}

void clampPlayer(){
  if (px < 0) px = 0; if (px > SCREEN_WIDTH - PLAYER_W) px = SCREEN_WIDTH - PLAYER_W;
  if (py < -PLAYER_H) py = -PLAYER_H; if (py > SCREEN_HEIGHT + PLAYER_H) py = SCREEN_HEIGHT + PLAYER_H;
}

void safetyUnstuck(){
  if (!rectCollidesSolid((int)px, (int)py, PLAYER_W, PLAYER_H)) return;
  const int8_t dxs[4] = {0, 1,-1, 0};
  const int8_t dys[4] = {-1,0, 0, 1};
  for (int dir=0; dir<4; ++dir){
    float ox = px, oy = py;
    for (int s=0; s<SAFE_WHILE_GUARD*2; ++s){
      float nx = px + dxs[dir];
      float ny = py + dys[dir];
      if (!rectCollidesSolid((int)nx, (int)ny, PLAYER_W, PLAYER_H)){ px = nx; py = ny; return; }
      px = nx; py = ny;
    }
    px = ox; py = oy;
  }
  // Fail-safe: treat as death
  // (loseLifeAndRespawn declared later)
}

bool aabbOverlap(int16_t ax0,int16_t ay0,int16_t aw,int16_t ah,
                 int16_t bx0,int16_t by0,int16_t bw,int16_t bh){
  int16_t ax1 = ax0 + aw - 1, ay1 = ay0 + ah - 1;
  int16_t bx1 = bx0 + bw - 1, by1 = by0 + bh - 1;
  return !(ax1<bx0 || bx1<ax0 || ay1<by0 || by1<ay0);
}

// ================= WORLD GEN =================
void generateWorld(){
  // Clear
  for (int ty=0; ty<MAP_H_TILES; ++ty){
    for (int tx=0; tx<MAP_W_TILES; ++tx){
      world[ty][tx]=0; coins_map[ty][tx]=0;
    }
  }
  // Level-dependent gap pattern
  int g1 = 17 - (levelNo % 5); if (g1 < 9) g1 = 9;
  int g2 = 23 - (levelNo % 7); if (g2 < 12) g2 = 12;
  for (int tx=0; tx<MAP_W_TILES; ++tx){
    if ((tx % g1) == 5 || (tx % g2) == 10) world[GROUND_ROW][tx] = 0; else world[GROUND_ROW][tx] = 1;
    for (int ty=GROUND_ROW+1; ty<MAP_H_TILES; ++ty){ world[ty][tx] = 1; }
  }
  // Platforms
  int step = 20 - (levelNo % 6); if (step < 12) step = 12;
  for (int tx=12; tx<MAP_W_TILES; tx += step){
    int ty = GROUND_ROW - 2;
    world[ty][tx]=1; world[ty][tx+1]=1;
    if (ty-1>=0) coins_map[ty-1][tx]=1;
  }
  // Ground coins
  for (int tx=6; tx<MAP_W_TILES; tx += 10){
    if (world[GROUND_ROW][tx]==1 && world[GROUND_ROW][tx+1]==1) coins_map[GROUND_ROW-1][tx]=1;
  }
}

// ================= ENEMIES =================
void spawnEnemies(){
  for (int i=0;i<MAX_ENEMIES;i++){ enemies[i].alive=false; }
  int idx = 0;
  for (int tx=20; tx<MAP_W_TILES && idx<MAX_ENEMIES; tx += 30){
    enemies[idx].worldX = tx * TILE_W;
    enemies[idx].y = playerGroundTopAtTile(tx);
    enemies[idx].vx = (idx%2==0) ? ENEMY_SPEED : -ENEMY_SPEED;
    enemies[idx].vy = 0.f;
    enemies[idx].alive = true;
    idx++;
  }
}

void updateEnemies(){
  for (int i=0;i<MAX_ENEMIES;i++){
    Enemy &e = enemies[i];
    if (!e.alive) continue;

    // Gravity
    e.vy += GRAVITY;
    if (e.vy > MAX_FALL_SPEED) e.vy = MAX_FALL_SPEED;

    // Horizontal patrol in world space; test in screen space
    {
      float remain = e.vx;
      int steps = (int)ceil(fabs(remain)); if (steps < 1) steps = 1;
      float step = remain / steps;
      for (int s=0; s<steps; ++s){
        int16_t trialScr = (int16_t)((e.worldX - camX) + step);
        if (!rectCollidesSolid(trialScr, (int)e.y, ENEMY_W, ENEMY_H)){
          e.worldX += step;
          if (e.worldX < 0) e.worldX += worldWidthPixels();
          if (e.worldX >= worldWidthPixels()) e.worldX -= worldWidthPixels();
        } else {
          e.vx = -e.vx; // bounce back
          break;
        }
      }
    }
    // Vertical
    {
      float remain = e.vy;
      int steps = (int)ceil(fabs(remain)); if (steps < 1) steps = 1;
      float step = remain / steps;
      bool collided = false;
      for (int s=0; s<steps; ++s){
        int16_t trialY = (int16_t)(e.y + step);
        if (!rectCollidesSolid((int16_t)(e.worldX - camX), trialY, ENEMY_W, ENEMY_H)){
          e.y = trialY;
        } else {
          collided = true; break;
        }
      }
      if (collided){
        if (e.vy > 0){
          for (int guard=0; guard<SAFE_WHILE_GUARD && !rectCollidesSolid((int16_t)(e.worldX - camX), (int16_t)(e.y+1), ENEMY_W, ENEMY_H); ++guard) e.y += 1;
          e.vy = 0;
        } else if (e.vy < 0){
          for (int guard=0; guard<SAFE_WHILE_GUARD && !rectCollidesSolid((int16_t)(e.worldX - camX), (int16_t)(e.y-1), ENEMY_W, ENEMY_H); ++guard) e.y -= 1;
          e.vy = 0;
        }
      }
    }
  }
}

bool aabbPlayerEnemy(int16_t ex, int16_t ey){
  return aabbOverlap((int16_t)px,(int16_t)py,PLAYER_W,PLAYER_H, ex,ey,ENEMY_W,ENEMY_H);
}

void handlePlayerEnemyCollisions(){
  for (int i=0;i<MAX_ENEMIES;i++){
    Enemy &e = enemies[i];
    if (!e.alive) continue;
    int16_t ex = (int16_t)(e.worldX - camX);
    while (ex < -SCREEN_WIDTH) ex += worldWidthPixels();
    while (ex >  2*SCREEN_WIDTH) ex -= worldWidthPixels();
    if (aabbPlayerEnemy(ex, (int16_t)e.y)){
      if (vy > 0 && ((int)py + PLAYER_H - 1) <= ((int)e.y + ENEMY_H/2)){
        e.alive = false;
        score += 200;
        vy = -3.6f;
        shakeTimer = SHAKE_TIME;
      } else {
        // lose life (declared later)
        // (we forward-declare here by placing function before usage in final code order)
      }
    }
  }
}

// ================= COINS =================
void collectCoins(int16_t xscr, int16_t y, int16_t w, int16_t h){
  int32_t x0w = (int32_t)xscr + (int32_t)camX;
  int32_t x1w = (int32_t)xscr + (int32_t)w - 1 + (int32_t)camX;

  int16_t ty0 = y / TILE_H; if (ty0 < 0) ty0 = 0;
  int16_t ty1 = (y + h - 1) / TILE_H; if (ty1 >= MAP_H_TILES) ty1 = MAP_H_TILES - 1;

  int32_t t0 = (x0w >= 0) ? (x0w / TILE_W) : ((x0w - (TILE_W-1)) / TILE_W);
  int32_t t1 = (x1w >= 0) ? (x1w / TILE_W) : ((x1w - (TILE_W-1)) / TILE_W);

  for (int16_t ty = ty0; ty <= ty1; ++ty){
    for (int32_t tx = t0; tx <= t1; ++tx){
      int16_t tw = (int16_t)(tx % MAP_W_TILES);
      if (tw < 0) tw += MAP_W_TILES;
      if (coins_map[ty][tw]){
        coins_map[ty][tw] = 0;
        coins += 1;
        score += 100;
      }
    }
  }
}

// ================= FLAG / LEVEL END =================
void tryGrabFlag(){
  // Player center in world space vs flag (wrap-aware)
  int32_t wpx = worldWidthPixels();
  int32_t pcx = (int32_t)px + (int32_t)camX + PLAYER_W/2;
  int32_t pole = (int32_t)flagWorldX() + TILE_W/2;
  bool near = false;
  for (int k=-1; k<=1; ++k){
    int32_t f = pole + k * wpx;
    if (abs(pcx - f) <= 5){ near = true; break; }
  }
  if (!near) return;

  // Latch
  gameMode = MODE_FLAG_SLIDE;
  vx = 0.f; vy = 0.f;
  int16_t fxScr = (int16_t)(flagWorldX() - camX);
  px = fxScr - (PLAYER_W/2);
  if (px < 0) px = 0;
  if (px > SCREEN_WIDTH-PLAYER_W) px = SCREEN_WIDTH-PLAYER_W;
}

// Forward decls for life handling
void resetGame();
void loseLifeAndRespawn();

// ================= RESET / LIVES =================
void hardResetToLevelStart(){
  generateWorld();
  spawnEnemies();
  px = PLAYER_START_X;
  py = PLAYER_START_Y;
  vx = vy = 0;
  onGround = false;
  jumpFrames = 0;
  coyote = 0;
  jumpBuffer = 0;
  camX = 0;
  score = 0;
  coins = 0;
  animCounter = 0;
  shakeTimer = 0;
  offscreenFrames = 0;
  // lift out if inside solid
  for (int g=0; g<SAFE_WHILE_GUARD && rectCollidesSolid((int)px,(int)py,PLAYER_W,PLAYER_H) && py>0; ++g) py -= 1;
}

void resetGame(){
  gameMode = MODE_PLAY;
  clearTimer = 0;
  hardResetToLevelStart();
  lives = MAX_LIVES;
}

void loseLifeAndRespawn(){
  gameMode = MODE_PLAY;
  offscreenFrames = 0;
  if (lives>0) lives--;
  px = PLAYER_START_X;
  py = PLAYER_START_Y;
  vx = vy = 0;
  onGround = false;
  jumpFrames = 0;
  coyote = 0;
  jumpBuffer = 0;
  camX = 0;
  shakeTimer = 6;
  // Relight enemies (soft respawn)
  spawnEnemies();
  for (int g=0; g<SAFE_WHILE_GUARD && rectCollidesSolid((int)px,(int)py,PLAYER_W,PLAYER_H) && py>0; ++g) py -= 1;
  if (lives == 0){
    // Game over -> restart level
    lives = MAX_LIVES;
    levelNo = 1;
    hardResetToLevelStart();
  }
}

// ================= SETUP =================
void setup(){
  Serial.begin(115200);
  delay(20);
  Wire.begin();

  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)){
    while(true){}
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println("QPAD D21 Mini Mario v12");
  display.println("Touch+OLED init...");
  display.display();

  init_touch();
  calibrateTouch();
  generateWorld();
  spawnEnemies();
  // lift out if inside solid
  for (int g=0; g<SAFE_WHILE_GUARD && rectCollidesSolid((int)px,(int)py,PLAYER_W,PLAYER_H) && py>0; ++g) py -= 1;
}

// ================= UPDATE =================
void handlePlayerEnemyCollisions2(){
  for (int i=0;i<MAX_ENEMIES;i++){
    Enemy &e = enemies[i];
    if (!e.alive) continue;
    int16_t ex = (int16_t)(e.worldX - camX);
    while (ex < -SCREEN_WIDTH) ex += worldWidthPixels();
    while (ex >  2*SCREEN_WIDTH) ex -= worldWidthPixels();
    if (aabbPlayerEnemy(ex, (int16_t)e.y)){
      if (vy > 0 && ((int)py + PLAYER_H - 1) <= ((int)e.y + ENEMY_H/2)){
        e.alive = false;
        score += 200;
        vy = -3.6f;
        shakeTimer = SHAKE_TIME;
      } else {
        loseLifeAndRespawn();
        return;
      }
    }
  }
}

void updateGame(){
  update_touch();

  if (gameMode == MODE_FLAG_SLIDE){
    int16_t groundY = playerGroundTopAtTile(FLAG_TILE_X);
    vy = (py < groundY) ? 1.8f : 0.f;
    py += vy;
    camX = camX; // freeze
    if ((int)py >= groundY){
      py = groundY;
      gameMode = MODE_CLEARED;
      clearTimer = 180; // 6s
    }
    return;
  } else if (gameMode == MODE_CLEARED){
    if (clearTimer > 0) clearTimer--;
    return;
  }

  // Read touches -> buttons (adaptive thresholds)
  int vL = touch_values[IDX_LEFT];
  int vR = touch_values[IDX_RIGHT];
  int vJ = touch_values[IDX_JUMP];
#if USE_ADAPTIVE_THRESH
  bool btnLeft  = (vL > game_thresh[IDX_LEFT]);
  bool btnRight = (vR > game_thresh[IDX_RIGHT]);
  bool btnJump  = (vJ > game_thresh[IDX_JUMP]);
#else
  bool btnLeft  = (vL > (THRESHOLD + PRESS_MARGIN));
  bool btnRight = (vR > (THRESHOLD + PRESS_MARGIN));
  bool btnJump  = (vJ > (THRESHOLD + PRESS_MARGIN));
#endif
  if (btnLeft && btnRight){
    if (vL > vR + PRESS_MARGIN) btnRight = false;
    else if (vR > vL + PRESS_MARGIN) btnLeft = false;
    else { btnLeft = btnRight = false; }
  }

  // Jump buffer & coyote
  static uint8_t jumpRelease=1;
  if (!btnJump) jumpRelease=1;
  if (btnJump && jumpRelease) jumpBuffer = JUMP_BUFFER_FRAMES;
  else if (jumpBuffer>0) jumpBuffer--;

  // Horizontal accel
  float ax = 0.f;
  if (btnLeft && !btnRight)  ax = -MOVE_SPEED;
  else if (btnRight && !btnLeft) ax = MOVE_SPEED;
  else ax = 0.f;
  vx = (INVERT_X ? -ax : ax);

  // Jump start
  if ((onGround || coyote > 0) && jumpBuffer > 0){
    jumpRelease = 0;
    vy = JUMP_VELOCITY;
    onGround = false;
    jumpFrames = 0;
    jumpBuffer = 0;
  }
  // Hold for higher jump
  if (btnJump && vy < 0 && jumpFrames < MAX_JUMP_FRAMES){
    vy -= 0.15f;
    jumpFrames++;
  }

  // Gravity
  vy += GRAVITY;
  if (vy > MAX_FALL_SPEED) vy = MAX_FALL_SPEED;

  // ---- Stepped horizontal move ----
  {
    float remain = vx;
    int steps = (int)ceil(fabs(remain));
    if (steps < 1) steps = 1;
    float step = remain / steps;
    for (int s=0; s<steps; ++s){
      float trial = px + step;
      if (!rectCollidesSolid((int)trial, (int)py, PLAYER_W, PLAYER_H)){
        px = trial;
      } else {
        for (int guard=0; guard<SAFE_WHILE_GUARD && !rectCollidesSolid((int)px + (step>0?1:-1), (int)py, PLAYER_W, PLAYER_H); ++guard) px += (step>0?1:-1);
        vx = 0.f;
        break;
      }
    }
  }

  // ---- Stepped vertical move ----
  {
    float remain = vy;
    int steps = (int)ceil(fabs(remain));
    if (steps < 1) steps = 1;
    float step = remain / steps;
    bool collided = false;
    for (int s=0; s<steps; ++s){
      float trial = py + step;
      if (!rectCollidesSolid((int)px, (int)trial, PLAYER_W, PLAYER_H)){
        py = trial;
      } else {
        collided = true;
        break;
      }
    }
    if (collided){
      if (vy > 0){
        for (int guard=0; guard<SAFE_WHILE_GUARD && !rectCollidesSolid((int)px, (int)py+1, PLAYER_W, PLAYER_H); ++guard) py += 1;
        onGround = true; vy = 0; jumpFrames = 0; coyote = COYOTE_FRAMES;
      } else if (vy < 0){
        for (int guard=0; guard<SAFE_WHILE_GUARD && !rectCollidesSolid((int)px, (int)py-1, PLAYER_W, PLAYER_H); ++guard) py -= 1;
        vy = 0;
      }
    } else {
      if (onGround && vy > 0) coyote = COYOTE_FRAMES;
      onGround = false;
      if (coyote>0) coyote--;
    }
  }

  // Camera scroll and safety
  camX = (int16_t)((camX + CAM_SCROLL_SPEED) % worldWidthPixels());
  if (camX < 0) camX += worldWidthPixels();
  clampPlayer();
  if (rectCollidesSolid((int)px, (int)py, PLAYER_W, PLAYER_H)) safetyUnstuck();

  // Coins
  collectCoins((int)px, (int)py, PLAYER_W, PLAYER_H);

  // Enemies
  updateEnemies();
  handlePlayerEnemyCollisions2();

  // Flag
  tryGrabFlag();

  // Off-screen watchdog
  if ((px <= -PLAYER_W) || (px >= SCREEN_WIDTH)) {
    if (offscreenFrames < 255) offscreenFrames++;
  } else {
    offscreenFrames = 0;
  }
  if (offscreenFrames > MAX_OFFSCREEN_FRAMES) {
    loseLifeAndRespawn();
    offscreenFrames = 0;
  }

  // Fall
  if ((int)py > SCREEN_HEIGHT){
    loseLifeAndRespawn();
  }

  score += 1;
  animCounter++;
}

// ================= RENDER =================
void renderGame(){
  display.clearDisplay();

  int16_t shakeX = 0, shakeY = 0;
  if (shakeTimer>0){
    shakeX = (shakeTimer % 2) ? 1 : -1;
    shakeY = ((shakeTimer/2) % 2) ? 1 : -1;
    shakeTimer--;
  }

  // Tiles + coins
  int16_t camTile = camX / TILE_W;
  int16_t camOffX = camX % TILE_W;
  for (int ty=0; ty<MAP_H_TILES; ++ty){
    for (int i=0; i<= (SCREEN_WIDTH / TILE_W) + 2; ++i){
      int16_t tx = (camTile + i) % MAP_W_TILES;
      int16_t x = i * TILE_W - camOffX + shakeX;
      if (world[ty][tx]){
        display.drawBitmap(x, ty*TILE_H + shakeY, tile_block, TILE_W, TILE_H, SSD1306_WHITE);
      }
      if (coins_map[ty][tx]){
        int16_t cx = x + (TILE_W-COIN_W)/2;
        int16_t cy = ty*TILE_H + (TILE_H-COIN_H)/2;
        display.drawBitmap(cx, cy, coin_sprite, COIN_W, COIN_H, SSD1306_WHITE);
      }
    }
  }

  // Flagpole & simple castle
  int16_t fx = (int16_t)(flagWorldX() - camX) + shakeX;
  while (fx < -SCREEN_WIDTH) fx += worldWidthPixels();
  while (fx >  2*SCREEN_WIDTH) fx -= worldWidthPixels();
  if (fx > -8 && fx < SCREEN_WIDTH){
    display.drawFastVLine(fx + (TILE_W/2), 0, SCREEN_HEIGHT, SSD1306_WHITE);
    int16_t ftop = 8 + ((animCounter/6)%2 ? 1 : 0);
    display.fillRect(fx + (TILE_W/2) + 1, ftop, 10, 6, SSD1306_WHITE);
  }
  int16_t cx = (int16_t)(castleWorldX() - camX) + shakeX;
  if (cx > -16 && cx < SCREEN_WIDTH){
    display.fillRect(cx, SCREEN_HEIGHT-24, 16, 24, SSD1306_WHITE);
    display.fillRect(cx+3, SCREEN_HEIGHT-30, 10, 6, SSD1306_WHITE);
    display.fillRect(cx+6, SCREEN_HEIGHT-34, 4, 4, SSD1306_WHITE);
    display.fillRect(cx+5, SCREEN_HEIGHT-12, 6, 12, SSD1306_WHITE); // gate
  }

  // Enemies
  for (int i=0;i<MAX_ENEMIES;i++){
    Enemy &e = enemies[i];
    if (!e.alive) continue;
    int16_t ex = (int16_t)(e.worldX - camX);
    while (ex < -SCREEN_WIDTH) ex += worldWidthPixels();
    while (ex >  2*SCREEN_WIDTH) ex -= worldWidthPixels();
    if (ex > -ENEMY_W && ex < SCREEN_WIDTH){
      display.drawBitmap(ex + shakeX, (int16_t)e.y + shakeY, enemy_sprite, ENEMY_W, ENEMY_H, SSD1306_WHITE);
    }
  }

  // Player
  const uint8_t* sprite = player_stand;
  if (onGround && (vx != 0.f)){
    sprite = ((animCounter / 6) % 2) ? player_run1 : player_run2;
  }
  display.drawBitmap((int16_t)px + shakeX, (int16_t)py + shakeY, sprite, PLAYER_W, PLAYER_H, SSD1306_WHITE);

  // HUD
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.print(F("S:")); display.print(score);
  display.print(F(" C:")); display.print(coins);
  display.print(F(" L:")); display.print(lives);
  display.setCursor(0,8);
  display.print(F("L:")); display.print(touch_values[IDX_LEFT]);
  display.print(F(" R:")); display.print(touch_values[IDX_RIGHT]);
  display.print(F(" J:")); display.print(touch_values[IDX_JUMP]);
#if USE_ADAPTIVE_THRESH
  display.setCursor(0,16);
  display.print(F("Lt>")); display.print(game_thresh[IDX_LEFT]);
  display.print(F(" Rt>")); display.print(game_thresh[IDX_RIGHT]);
  display.print(F(" J>")); display.print(game_thresh[IDX_JUMP]);
#endif

  if (gameMode == MODE_CLEARED){
    display.setTextSize(2);
    display.setCursor(10, 30);
    display.print(F("LEVEL ")); display.print(levelNo); display.print(F(" CLEAR"));
    display.setTextSize(1);
  }

  display.display();
}

// ================= LOOP =================
void loop(){
  static uint32_t lastFrame = 0;
  uint32_t now = millis();
  if (now - lastFrame < FRAME_MS) return;
  lastFrame = now;

  updateGame();
  renderGame();

  if (gameMode == MODE_CLEARED && clearTimer == 0){
    levelNo++; if (levelNo > 99) levelNo = 1;
    resetGame();
  }
}