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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C

#define PIN_RED 17
#define PIN_GREEN 16
#define PIN_BLUE 25
#define N_TOUCH 6
#define THRESHOLD 6

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

static const uint8_t Y_MAX = 128;
static const uint8_t Y_MIN = 0;

static const uint8_t PIN_LEFT   = D1;
static const uint8_t PIN_RIGHT  = D7;
static const uint8_t PIN_DOWN   = D8;
static const uint8_t PIN_UP = D0;
static const uint8_t PIN_A      = D9; 
static const uint8_t PIN_B      = D10;  

int touch_pins[N_TOUCH]   = { PIN_LEFT, PIN_RIGHT, PIN_DOWN, PIN_UP, PIN_A, PIN_B };
int touch_values[N_TOUCH] = { 0, 0, 0, 0, 0, 0 };
char key_map[N_TOUCH] = { 0, 0, 's', 'w', 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 update_touch() {
  int t;
  int t_max = 200;
  int p;

  for (int i = 0; i < N_TOUCH; i++) {
    p = touch_pins[i];

    // set to low
    pinMode(p, OUTPUT);
    digitalWriteFast(p, LOW);

    // settle
    delayMicroseconds(25);

    // enable pull-up
    pinMode(p, INPUT_PULLUP);

    // measure time to rise
    t = 0;
    while (!digitalReadFast(p) && t < t_max) {
      t++;
    }
    touch_values[i] = t;

    // update state
    pin_touched_past[i] = pin_touched_now[i];
    pin_touched_now[i] = touch_values[i] > THRESHOLD;
  }
}

void update_keyboard() {
  for (int i = 0; i < N_TOUCH; i++) {
    if (key_map[i] == 0) continue;  // skip pads handled elsewhere (mouse)
    if (pin_touched_now[i] && !pin_touched_past[i]) {
      Keyboard.press(key_map[i]);
    } else if (!pin_touched_now[i] && pin_touched_past[i]) {
      Keyboard.release(key_map[i]);
    }
  }
}

const int8_t MOUSE_SPEED = 12;  // pixels per frame while held

void update_mouse() {
  int8_t dx = 0;
  if (pin_touched_now[0]) dx -= MOUSE_SPEED;  // LEFT
  if (pin_touched_now[1]) dx += MOUSE_SPEED;  // RIGHT
  // LEFT CLICK
  if (pin_touched_now[4] && !pin_touched_past[4]) {
    Mouse.press(MOUSE_LEFT);
  } else if (!pin_touched_now[4] && pin_touched_past[4]) {
    Mouse.release(MOUSE_LEFT);
  }
  if (dx != 0) Mouse.move(dx, 0);
}

const int HORIZON = 32;
const int VP_X = 64;  // vanishing point x, in the middle of the screen
const int FAR_ROW = HORIZON + 6;

const float TILE = 2.0; //3d distance between cross lines
const float K = 62.0; //row scale
const float NEAR_DZ = K / (SCREEN_HEIGHT - 1 - HORIZON); // bottom of screen z-coord
const float MOVE_SPEED = 3.0;

const float FOCAL = 32.0;
const float WORLD_RANGE = 12.0;
const float TURN_SPEED = 1.8;
const int CX = SCREEN_WIDTH / 2;

float camX = 0.0;
float camZ = 0.0;
float camY = 0.0;
float yaw = 0.0;
unsigned long lastMicros = 0;

const float EYE_HEIGHT = 1.0;
const float JUMP_VELOCITY = 6.0; //initial jump velocity
const float GRAVITY = 18.0; // gravitational constnat (accerleration)

float velY = 0.0;

// Translate/rotate a world point into camera space (x = right, z = forward/depth).
void toCameraSpace(float wx, float wz, float s, float c, float &x, float &z) {
  float dx = wx - camX;
  float dz = wz - camZ;
  x = dx * c - dz * s;
  z = dx * s + dz * c;
}

// Project an already-camera-space point to screen coordinates.
// Caller must ensure z >= NEAR_DZ.
void projectCamera(float x, float z, float kEff, int &sx, int &sy) {
  sx = CX + (int)roundf(FOCAL * x / z);
  sy = HORIZON + (int)roundf(kEff / z);
}

void drawWorldLine(float wx0, float wz0, float wx1, float wz1, float s, float c, float kEff) {
  float x0, z0, x1, z1;
  toCameraSpace(wx0, wz0, s, c, x0, z0);
  toCameraSpace(wx1, wz1, s, c, x1, z1);

  if (z0 < NEAR_DZ && z1 < NEAR_DZ) return;  // whole segment too close
  if (z0 < NEAR_DZ) { float t = (NEAR_DZ - z0) / (z1 - z0); x0 += t * (x1 - x0); z0 = NEAR_DZ; }
  if (z1 < NEAR_DZ) { float t = (NEAR_DZ - z1) / (z0 - z1); x1 += t * (x0 - x1); z1 = NEAR_DZ; }

  int sx0, sy0, sx1, sy1;
  projectCamera(x0, z0, kEff, sx0, sy0);
  projectCamera(x1, z1, kEff, sx1, sy1);
  display.drawLine(sx0, sy0, sx1, sy1, SSD1306_WHITE);
}

void drawGrid(float kEff) {
  float s = sinf(yaw); // x-component of yaw
  float c = cosf(yaw); // z-component of yaw

  // x and z line rendering boundaries
  int ix0 = (int)floorf((camX - WORLD_RANGE) / TILE);
  int ix1 = (int)ceilf((camX + WORLD_RANGE) / TILE);
  int iz0 = (int)floorf((camZ - WORLD_RANGE) / TILE);
  int iz1 = (int)ceilf((camZ + WORLD_RANGE) / TILE);

  // lines running away from you (constant x)
  for (int i = ix0; i <= ix1; i++) {
    float wx = i * TILE;
    drawWorldLine(wx, camZ - WORLD_RANGE, wx, camZ + WORLD_RANGE, s, c, kEff);
  }

  // lines running across (constant z)
  for (int i = iz0; i <= iz1 ;i++) {
    float wz = i * TILE;
    drawWorldLine(camX - WORLD_RANGE, wz, camX +WORLD_RANGE, wz, s, c, kEff);
  }
}

void setup() {
  Serial.begin(115200);

  delay(50);

  Keyboard.begin();
  Mouse.begin();

  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println("SSD1306 allocation failed");
    while (true);
  }

  display.clearDisplay();

  lastMicros = micros();
}

void loop() {
  update_touch();
  update_keyboard();
  update_mouse();

  // time since last frame, so speed isn't dependent on frame rate
  unsigned long now = micros();
  float dt = (now - lastMicros) * 1e-6f;
  lastMicros = now;

  // wrap yaw
  if (yaw > PI)  yaw -= 2 * PI;
  if (yaw < -PI) yaw += 2 * PI;

  if (pin_touched_now[0]) yaw -= TURN_SPEED * dt; // LEFT
  if (pin_touched_now[1]) yaw += TURN_SPEED * dt; // RIGHT

  float s = sinf(yaw), c = cosf(yaw);

  // jump
  bool jumpPressed = pin_touched_now[5] && !pin_touched_past[5];
  if (jumpPressed && camY <= 0.0f) {
    velY = JUMP_VELOCITY;
  }

  // basic y-physics engine
  camY += velY * dt;
  velY -= GRAVITY * dt;
  if (camY <= 0.0f) {camY = 0.0f; velY = 0.0f; } //landed  

  float kEff = K * (EYE_HEIGHT + camY) / EYE_HEIGHT;

  if (pin_touched_now[3]) { camX += s * MOVE_SPEED * dt; camZ += c * MOVE_SPEED * dt; } // UP
  if (pin_touched_now[2]) { camX -= s * MOVE_SPEED * dt; camZ -= c * MOVE_SPEED * dt; } // DOWN

  display.clearDisplay();
  // display.drawFastHLine(0, HORIZON + 4, SCREEN_WIDTH, SSD1306_WHITE);
  drawGrid(kEff);
  display.display();
}