#include <TFT_eSPI.h>
#include <SPI.h>

TFT_eSPI tft = TFT_eSPI();

#define SCREEN_WIDTH 120
#define SCREEN_HEIGHT 160
#define MAP_WIDTH 8
#define MAP_HEIGHT 8
#define FOV 60
#define HALF_FOV 30
#define PLAYER_SPEED 0.05
#define ROTATION_SPEED 0.05

#define BUTTON_PIN 0
#define LEFT_PIN 2
#define RIGHT_PIN 4

unsigned long previousMillis = 0;
unsigned long currentMillis = 0;
int frameCount = 0;
float fps = 0;

uint8_t worldMap[MAP_WIDTH][MAP_HEIGHT] = {
  {1,1,1,1,1,1,1,1},
  {1,0,0,0,0,0,0,1},
  {1,0,1,0,1,0,0,1},
  {1,0,0,0,0,0,0,1},
  {1,0,1,0,1,0,0,1},
  {1,0,0,0,0,0,0,1},
  {1,0,0,0,0,0,0,1},
  {1,1,1,1,1,1,1,1}
};

float playerX = 1.5;
float playerY = 1.5;
float playerAngle = 0;

void setup() {
  Serial.begin(115200);
  
  tft.init();
  tft.setRotation(0);
  tft.fillScreen(TFT_BLUE);
  tft.setTextColor(TFT_GREEN, TFT_BLACK);
  tft.setTextSize(1);

  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(LEFT_PIN, INPUT_PULLUP);
  pinMode(RIGHT_PIN, INPUT_PULLUP);
}

void drawLine(int x, int h, uint16_t color) {
  int start = (SCREEN_HEIGHT - h) / 2;
  int end = start + h;
  tft.drawFastVLine(x, start, h, color);
}

void castRays() {
  for (int x = 0; x < SCREEN_WIDTH; x++) {
    float rayAngle = (playerAngle - HALF_FOV) + (x / float(SCREEN_WIDTH)) * FOV;
    float sinA = sin(rayAngle);
    float cosA = cos(rayAngle);

    float rayDirX = cosA;
    float rayDirY = sinA;

    int mapX = int(playerX);
    int mapY = int(playerY);

    float sideDistX, sideDistY;
    float deltaDistX = abs(1 / rayDirX);
    float deltaDistY = abs(1 / rayDirY);
    float perpWallDist;

    int stepX, stepY;

    if (rayDirX < 0) {
      stepX = -1;
      sideDistX = (playerX - mapX) * deltaDistX;
    } else {
      stepX = 1;
      sideDistX = (mapX + 1.0 - playerX) * deltaDistX;
    }
    if (rayDirY < 0) {
      stepY = -1;
      sideDistY = (playerY - mapY) * deltaDistY;
    } else {
      stepY = 1;
      sideDistY = (mapY + 1.0 - playerY) * deltaDistY;
    }

    bool hit = false;
    int side;

    while (!hit) {
      if (sideDistX < sideDistY) {
        sideDistX += deltaDistX;
        mapX += stepX;
        side = 0;
      } else {
        sideDistY += deltaDistY;
        mapY += stepY;
        side = 1;
      }

      if (worldMap[mapX][mapY] > 0) hit = true;
    }

    if (side == 0) perpWallDist = (mapX - playerX + (1 - stepX) / 2) / rayDirX;
    else           perpWallDist = (mapY - playerY + (1 - stepY) / 2) / rayDirY;

    int lineHeight = int(SCREEN_HEIGHT / perpWallDist);

    uint16_t color = TFT_WHITE;
    if (perpWallDist > 4) color = TFT_DARKGREY;
    else if (perpWallDist > 2) color = TFT_LIGHTGREY;

    drawLine(x, lineHeight, color);
  }
}

void displayFPS() {
  tft.setCursor(5, 5);
  tft.print("FPS: ");
  tft.print(fps);
}

void calculateFPS() {
  frameCount++;
  currentMillis = millis();
  if (currentMillis - previousMillis >= 1000) {
    fps = frameCount / ((currentMillis - previousMillis) / 1000.0);
    frameCount = 0;
    previousMillis = currentMillis;
    
    Serial.print("FPS: ");
    Serial.println(fps);
  }
}

void loop() {
  tft.fillScreen(TFT_BLACK);
  castRays();
  
  if (digitalRead(BUTTON_PIN) == LOW) {
    playerX += cos(playerAngle) * PLAYER_SPEED;
    playerY += sin(playerAngle) * PLAYER_SPEED;
  }
  
  if (digitalRead(LEFT_PIN) == LOW) {
    playerAngle -= ROTATION_SPEED;
  }
  
  if (digitalRead(RIGHT_PIN) == LOW) {
    playerAngle += ROTATION_SPEED;
  }

  calculateFPS();
  displayFPS();
}