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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const int PIN_SDA = 22;
const int PIN_SCL = 23;
const int JOY_X = 5;
const int JOY_Y = 4;

const int mapW = 8;
const int mapH = 8;
const char worldMap[mapH][mapW+1] = {
  "11111111",
  "1......1",
  "1..11..1",
  "1......1",
  "1..11..1",
  "1......1",
  "1......1",
  "11111111"
};

float posX = 3.5, posY = 3.5;
float dir = 0;
float fov = 3.14159/4;

int joyXCenter = 2048;
int joyYCenter = 2048;

void setup() {
  Wire.begin(PIN_SDA, PIN_SCL);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.display();
  long sumX=0, sumY=0;
  for(int i=0;i<100;i++){
    sumX += analogRead(JOY_X);
    sumY += analogRead(JOY_Y);
    delay(5);
  }
  joyXCenter = sumX/100;
  joyYCenter = sumY/100;
}

float readJoy(int pin, int center){
  int val = analogRead(pin);
  int diff = val - center;
  int dead = 200;
  if(abs(diff)<dead) return 0;
  return (float)diff/4095.0;
}

void loop() {
  float joyX = readJoy(JOY_X, joyXCenter);
  float joyY = readJoy(JOY_Y, joyYCenter);

  dir += joyX * 0.15;

  float step = joyY * 0.05;
  float newX = posX + cos(dir)*step;
  float newY = posY + sin(dir)*step;
  if(worldMap[int(newY)][int(newX)] == '.') { posX=newX; posY=newY; }

  display.clearDisplay();
  for(int x=0;x<SCREEN_WIDTH;x++){
    float rayDir = dir - fov/2 + fov*((float)x/SCREEN_WIDTH);
    float dist=0; bool hit=false;
    float rx=posX, ry=posY;
    while(!hit && dist<16){
      rx+=cos(rayDir)*0.05; ry+=sin(rayDir)*0.05; dist+=0.05;
      if(worldMap[int(ry)][int(rx)]=='1') hit=true;
    }
    int h = (int)(SCREEN_HEIGHT/(dist+0.001));
    int y1 = SCREEN_HEIGHT/2 - h/2;
    int y2 = SCREEN_HEIGHT/2 + h/2;
    if(y1<0) y1=0; if(y2>=SCREEN_HEIGHT) y2=SCREEN_HEIGHT-1;
    display.drawLine(x,y1,x,y2,SSD1306_WHITE);
  }

  int scale = 4;
  display.fillRect(0,0,mapW*scale,mapH*scale,SSD1306_BLACK);
  for(int y=0;y<mapH;y++){
    for(int x=0;x<mapW;x++){
      if(worldMap[y][x]=='1') display.fillRect(x*scale,y*scale,scale,scale,SSD1306_WHITE);
    }
  }
  display.fillRect(posX*scale-1,posY*scale-1,2,2,SSD1306_WHITE);

  float playerSize = 2;
  float angle = dir;
  int px = posX*scale;
  int py = posY*scale;
  int dx = cos(angle)*playerSize;
  int dy = sin(angle)*playerSize;
  display.drawLine(px, py, px + dx, py + dy, SSD1306_WHITE);

  display.display();
}