#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SH110X.h>
#include <avr/pgmspace.h>

// ---------------- EKRAN SH1106 ----------------
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define i2c_Address 0x3c

Adafruit_SH1106G display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// ---------------- PINY KONSOLI ----------------
#define BTN_UP      7
#define BTN_RIGHT   8
#define BTN_DOWN    9
#define BTN_LEFT    10
#define BTN_FIRE    4

// ---------------- MAPA (8x8) ----------------
#define MAP_SIZE 8
const uint8_t worldMap[MAP_SIZE * MAP_SIZE] PROGMEM = {
  1,1,1,1,1,1,1,1,
  1,0,0,0,0,0,0,1,
  1,0,1,1,0,1,0,1,
  1,0,1,0,0,0,0,1,
  1,0,0,0,0,1,0,1,
  1,1,0,1,0,1,0,1,
  1,0,0,0,0,0,0,1,
  1,1,1,1,1,1,1,1
};

// ---------------- STANY GRY ----------------
enum GameState { STATE_MENU, STATE_PLAYING, STATE_WIN, STATE_LOSE };
GameState currentState = STATE_MENU;

// ---------------- GRACZ ----------------
float playerX = 1.5;
float playerY = 1.5;
float playerAngle = 0.0;
float fov = 3.14159 / 3.0;
int playerHP = 100;
bool firePrev = false;
int muzzleFlash = 0;

// ---------------- POCISKI ----------------
struct Bullet {
  float x, y, angle;
  bool active;
};
Bullet playerBullet = {0, 0, 0, false};
Bullet enemyBullet = {0, 0, 0, false};
unsigned long lastEnemyShot = 0;

// ---------------- PRZECIWNICY ----------------
#define NUM_ENEMIES 5
struct Enemy {
  float x, y;
  bool alive;
};
Enemy enemies[NUM_ENEMIES];

// OPTYMALIZACJA RAM: Używamy uint8_t zamiast float (oszczędza ok. 192 bajtów!)
uint8_t zBuffer[SCREEN_WIDTH / 2];

// ==========================================
// FUNKCJE POMOCNICZE
// ==========================================
uint8_t getMap(int x, int y) {
  if (x < 0 || x >= MAP_SIZE || y < 0 || y >= MAP_SIZE) return 1;
  return pgm_read_byte(&worldMap[y * MAP_SIZE + x]);
}

bool checkLOS(float x1, float y1, float x2, float y2) {
  float dx = x2 - x1;
  float dy = y2 - y1;
  float dist = sqrt(dx*dx + dy*dy);
  if (dist < 0.1) return true; // Zabezpieczenie
  
  int steps = (int)(dist * 4.0); 
  if (steps < 1) steps = 1;

  for (int i = 1; i < steps; i++) {
    float checkX = x1 + (dx * i) / steps;
    float checkY = y1 + (dy * i) / steps;
    if (getMap((int)checkX, (int)checkY) == 1) return false;
  }
  return true;
}

void resetGame() {
  playerX = 1.5;
  playerY = 1.5;
  playerAngle = 0.0;
  playerHP = 100;
  
  playerBullet.active = false;
  enemyBullet.active = false;
  lastEnemyShot = millis(); // Resetujemy czas strzału wroga
  
  enemies[0] = {5.5, 1.5, true};
  enemies[1] = {6.5, 6.5, true};
  enemies[2] = {3.5, 3.5, true};
  enemies[3] = {1.5, 6.5, true};
  enemies[4] = {4.5, 6.5, true};

  currentState = STATE_PLAYING;
}

// ==========================================
// SETUP
// ==========================================
void setup() {
  pinMode(BTN_UP, INPUT_PULLUP);
  pinMode(BTN_DOWN, INPUT_PULLUP);
  pinMode(BTN_LEFT, INPUT_PULLUP);
  pinMode(BTN_RIGHT, INPUT_PULLUP);
  pinMode(BTN_FIRE, INPUT_PULLUP);

  display.begin(i2c_Address, true);
  display.setRotation(2); 
}

// ==========================================
// GŁÓWNA PĘTLA GRY
// ==========================================
void loop() {
  bool firePressed = (digitalRead(BTN_FIRE) == LOW);

  // --- EKRAN STARTOWY ---
  if (currentState == STATE_MENU) {
    display.clearDisplay();
    display.setTextColor(SH110X_WHITE);
    display.setTextSize(2);
    display.setCursor(5, 5);
    display.print(F("MEILESTEIN"));

    display.setTextColor(SH110X_WHITE);
    display.setTextSize(2);
    display.setCursor(55, 25);
    display.print(F("3D"));
    
    display.setTextSize(1);
    display.setCursor(18, 45);
    display.print(F("strzel aby zaczac"));
    display.display();
    
    if (firePressed && !firePrev) {
      resetGame();
    }
    firePrev = firePressed;
    return;
  }

  // --- EKRANY KOŃCOWE ---
  if (currentState == STATE_WIN || currentState == STATE_LOSE) {
    display.clearDisplay();
    display.setTextColor(SH110X_WHITE);
    display.setTextSize(2);
    display.setCursor(15, 15);
    
    if (currentState == STATE_WIN) {
      display.print(F("WYGRANA!"));
    } else {
      display.print(F("ZGINALES"));
    }
    
    display.setTextSize(1);
    display.setCursor(0, 45);
    display.print(F("strzel by zgrac znowu"));
    display.display();

    if (firePressed && !firePrev) {
      resetGame();
    }
    firePrev = firePressed;
    return;
  }

  // ==========================================
  // WŁAŚCIWA ROZGRYWKA
  // ==========================================
  unsigned long currentMillis = millis();
  float moveSpeed = 0.15;
  float rotSpeed = 0.15;

  // 1. STEROWANIE
  if (digitalRead(BTN_LEFT) == LOW)  playerAngle -= rotSpeed;
  if (digitalRead(BTN_RIGHT) == LOW) playerAngle += rotSpeed;

  if (digitalRead(BTN_UP) == LOW) {
    float nextX = playerX + cos(playerAngle) * moveSpeed;
    float nextY = playerY + sin(playerAngle) * moveSpeed;
    if (getMap((int)nextX, (int)playerY) == 0) playerX = nextX;
    if (getMap((int)playerX, (int)nextY) == 0) playerY = nextY;
  }

  if (digitalRead(BTN_DOWN) == LOW) {
    float nextX = playerX - cos(playerAngle) * moveSpeed;
    float nextY = playerY - sin(playerAngle) * moveSpeed;
    if (getMap((int)nextX, (int)playerY) == 0) playerX = nextX;
    if (getMap((int)playerX, (int)nextY) == 0) playerY = nextY;
  }

  // OPTYMALIZACJA: Bezpieczne zawijanie kątów bez ryzykownych pętli while
  if (playerAngle < -PI) playerAngle += 2*PI;
  if (playerAngle > PI) playerAngle -= 2*PI;

  // 2. STRZELANIE GRACZA
  if (firePressed && !firePrev && !playerBullet.active) {
    playerBullet.x = playerX;
    playerBullet.y = playerY;
    playerBullet.angle = playerAngle;
    playerBullet.active = true;
    muzzleFlash = 3;
  }
  firePrev = firePressed;

  if (playerBullet.active) {
    playerBullet.x += cos(playerBullet.angle) * 0.4;
    playerBullet.y += sin(playerBullet.angle) * 0.4;
    
    if (getMap((int)playerBullet.x, (int)playerBullet.y) == 1 || playerBullet.x < 0 || playerBullet.y < 0) {
      playerBullet.active = false;
    } else {
      for (int i = 0; i < NUM_ENEMIES; i++) {
        if (enemies[i].alive) {
          float dx = playerBullet.x - enemies[i].x;
          float dy = playerBullet.y - enemies[i].y;
          if ((dx*dx + dy*dy) < 0.15) { 
            enemies[i].alive = false; 
            playerBullet.active = false;
            
            bool allDead = true;
            for (int j = 0; j < NUM_ENEMIES; j++) {
              if (enemies[j].alive) allDead = false;
            }
            if (allDead) currentState = STATE_WIN;
            break;
          }
        }
      }
    }
  }

  // 3. AI PRZECIWNIKÓW
  if (currentMillis - lastEnemyShot > 5000 && !enemyBullet.active) {
    for (int i = 0; i < NUM_ENEMIES; i++) {
      if (enemies[i].alive) {
        float dx = playerX - enemies[i].x;
        float dy = playerY - enemies[i].y;
        float dist = sqrt(dx*dx + dy*dy);
        
        if (dist < 6.0 && checkLOS(enemies[i].x, enemies[i].y, playerX, playerY)) {
          enemyBullet.x = enemies[i].x;
          enemyBullet.y = enemies[i].y;
          enemyBullet.angle = atan2(dy, dx);
          enemyBullet.active = true;
          lastEnemyShot = currentMillis;
          break; 
        }
      }
    }
  }

  if (enemyBullet.active) {
    // --- ZMIENIONA PRĘDKOŚĆ KULI WROGA ---
    enemyBullet.x += cos(enemyBullet.angle) * 0.1; 
    enemyBullet.y += sin(enemyBullet.angle) * 0.1;

    if (getMap((int)enemyBullet.x, (int)enemyBullet.y) == 1 || enemyBullet.x < 0 || enemyBullet.y < 0) {
      enemyBullet.active = false;
    } else {
      float dx = enemyBullet.x - playerX;
      float dy = enemyBullet.y - playerY;
      if ((dx*dx + dy*dy) < 0.15) { 
        playerHP -= 25; 
        enemyBullet.active = false;
        
        display.fillRect(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT, SH110X_WHITE);
        display.display();
        delay(60);

        if (playerHP <= 0) currentState = STATE_LOSE;
      }
    }
  }

  // 4. RYSOWANIE ŚCIAN
  display.clearDisplay();
  for (int x = 0; x < SCREEN_WIDTH; x += 2) {
    float rayAngle = (playerAngle - fov / 2.0) + ((float)x / (float)SCREEN_WIDTH) * fov;
    float eyeX = cos(rayAngle);
    float eyeY = sin(rayAngle);
    
    float distanceToWall = 0;
    bool hitWall = false;

    while (!hitWall && distanceToWall < 8.0) {
      distanceToWall += 0.1;
      if (getMap((int)(playerX + eyeX * distanceToWall), (int)(playerY + eyeY * distanceToWall)) == 1) {
        hitWall = true;
      }
    }

    float correctedDist = distanceToWall * cos(rayAngle - playerAngle);
    if (correctedDist < 0.1) correctedDist = 0.1;

    int distInt = (int)(correctedDist * 10.0);
    if (distInt > 255) distInt = 255;
    zBuffer[x / 2] = distInt; 
    
    int wallHeight = (int)(SCREEN_HEIGHT / correctedDist);
    if (wallHeight > SCREEN_HEIGHT) wallHeight = SCREEN_HEIGHT;
    int ceiling = (SCREEN_HEIGHT / 2) - (wallHeight / 2);
    
    if (hitWall) {
      if ((x / 2) % 2 == 0) {
        display.drawFastVLine(x, ceiling, wallHeight, SH110X_WHITE);
        display.drawFastVLine(x+1, ceiling, wallHeight, SH110X_WHITE);
      } else {
        for(int dy = 0; dy < wallHeight; dy+=2) {
           display.drawPixel(x, ceiling + dy, SH110X_WHITE);
           display.drawPixel(x+1, ceiling + dy + 1, SH110X_WHITE);
        }
      }
    }
  }

  // 5. RYSOWANIE WROGÓW
  int order[NUM_ENEMIES];
  for(int i=0; i<NUM_ENEMIES; i++) order[i] = i;
  for(int i=0; i<NUM_ENEMIES-1; i++) {
    for(int j=i+1; j<NUM_ENEMIES; j++) {
      float d1 = (enemies[order[i]].x - playerX)*(enemies[order[i]].x - playerX) + (enemies[order[i]].y - playerY)*(enemies[order[i]].y - playerY);
      float d2 = (enemies[order[j]].x - playerX)*(enemies[order[j]].x - playerX) + (enemies[order[j]].y - playerY)*(enemies[order[j]].y - playerY);
      if(d1 < d2) { int temp = order[i]; order[i] = order[j]; order[j] = temp; }
    }
  }

  for (int i = 0; i < NUM_ENEMIES; i++) {
    int idx = order[i];
    if (!enemies[idx].alive) continue;

    float dx = enemies[idx].x - playerX;
    float dy = enemies[idx].y - playerY;
    float dist = sqrt(dx * dx + dy * dy);
    if (dist < 0.1) dist = 0.1;

    float angleDiff = atan2(dy, dx) - playerAngle;
    if (angleDiff < -PI) angleDiff += 2*PI;
    if (angleDiff > PI) angleDiff -= 2*PI;

    if (fabs(angleDiff) < fov / 1.2) {
      int screenX = (int)((0.5 * (angleDiff / (fov / 2.0)) + 0.5) * SCREEN_WIDTH);
      int spriteH = (int)(SCREEN_HEIGHT / dist);
      int spriteW = spriteH / 2;
      int topY = (SCREEN_HEIGHT - spriteH) / 2;

      int startX = screenX - spriteW / 2;
      int endX = screenX + spriteW / 2;

      for (int sx = startX; sx <= endX; sx++) {
        if (sx >= 0 && sx < SCREEN_WIDTH && (zBuffer[sx / 2] / 10.0) > dist) {
          int localX = sx - startX;

          if (localX == 0 || localX == spriteW) {
            display.drawFastVLine(sx, topY, spriteH, SH110X_BLACK);
            continue;
          }

          int headBottom = spriteH * 0.3;
          int bodyBottom = spriteH * 0.65;
          bool isLegGap = (localX > spriteW * 0.35 && localX < spriteW * 0.65);

          display.drawFastVLine(sx, topY + 1, headBottom - 2, SH110X_WHITE);
          display.drawFastVLine(sx, topY + headBottom + 1, (bodyBottom - headBottom) - 2, SH110X_WHITE);
          if (!isLegGap) {
            display.drawFastVLine(sx, topY + bodyBottom + 1, (spriteH - bodyBottom) - 1, SH110X_WHITE);
          }

          if ((localX == spriteW / 4 || localX == spriteW - spriteW / 4) && spriteH > 10) {
            display.drawFastVLine(sx, topY + headBottom / 2 - 1, 3, SH110X_BLACK);
          }
        }
      }
    }
  }

  // 6. RYSOWANIE POCISKÓW W LOCIE
  auto drawBullet = [](Bullet& b, bool isEnemy) {
    if (!b.active) return;
    float dx = b.x - playerX;
    float dy = b.y - playerY;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist < 0.1) dist = 0.1;

    float angleDiff = atan2(dy, dx) - playerAngle;
    if (angleDiff < -PI) angleDiff += 2*PI;
    if (angleDiff > PI) angleDiff -= 2*PI;

    if (fabs(angleDiff) < fov / 1.2) {
      int screenX = (int)((0.5 * (angleDiff / (fov / 2.0)) + 0.5) * SCREEN_WIDTH);
      if (screenX >= 0 && screenX < SCREEN_WIDTH && (zBuffer[screenX / 2] / 10.0) > dist) {
        int bulletY = (SCREEN_HEIGHT / 2) + (SCREEN_HEIGHT / dist) / 6;
        int bulletSize = 20 / dist; 
        if (bulletSize < 1) bulletSize = 1;
        if (bulletSize > 4) bulletSize = 4;
        
        if (isEnemy) {
          display.drawCircle(screenX, bulletY, bulletSize, SH110X_WHITE);
        } else {
          display.fillCircle(screenX, bulletY, bulletSize, SH110X_WHITE);
          display.drawCircle(screenX, bulletY, bulletSize + 1, SH110X_BLACK);
        }
      }
    }
  };

  drawBullet(playerBullet, false);
  drawBullet(enemyBullet, true);

  // 7. HUD I MODEL BRONI
  display.fillRect(0, 0, 36, 10, SH110X_BLACK);
  display.setTextSize(1);
  display.setCursor(2, 1);
  display.print(F("HP:"));
  display.print(playerHP);

  int wX = SCREEN_WIDTH / 2;
  int wY = SCREEN_HEIGHT;
  int recoil = (muzzleFlash > 0) ? 4 : 0;

  display.fillRect(wX - 4, wY - 15 + recoil, 8, 15, SH110X_WHITE);
  display.fillRect(wX - 2, wY - 20 + recoil, 4, 5, SH110X_WHITE);
  display.drawFastHLine(wX - 6, wY - 5 + recoil, 12, SH110X_BLACK);

  if (muzzleFlash > 0) {
    display.fillCircle(wX, wY - 22, muzzleFlash * 2, SH110X_WHITE);
    muzzleFlash--;
  } else {
    display.drawPixel(wX, SCREEN_HEIGHT/2, SH110X_WHITE);
    display.drawPixel(wX - 2, SCREEN_HEIGHT/2, SH110X_WHITE);
    display.drawPixel(wX + 2, SCREEN_HEIGHT/2, SH110X_WHITE);
    display.drawPixel(wX, SCREEN_HEIGHT/2 - 2, SH110X_WHITE);
    display.drawPixel(wX, SCREEN_HEIGHT/2 + 2, SH110X_WHITE);
  }

  display.display();
}