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

// globals
#define SCREEN_WIDTH 128  // OLED display width, in pixels
#define SCREEN_HEIGHT 64  // OLED display height, in pixels

// Initialize the display using the standard Wire (I2C) interface.
// On the XIAO RP2040, Wire defaults to SDA = GPIO 6, SCL = GPIO 7.
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

bool screen_state[SCREEN_HEIGHT][SCREEN_WIDTH];

#define N_TOUCH 6
#define THRESHOLD 6

#define IND_UP 5
#define IND_DOWN 2
#define IND_LEFT 3
#define IND_RIGHT 4
#define IND_ADD 0
#define IND_REM 1

int touch_pins[N_TOUCH] = { 3, 4, 2, 27, 1, 26 };
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 };

int curr_pix_x = 0;
int curr_pix_y = 0;
bool curr_pix_on;

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 print_touch() {
  char print_buffer[30];
  for (int i = 0; i < N_TOUCH; i++) {
    sprintf(print_buffer, "%4d ", touch_values[i]);
    Serial.print(print_buffer);
  }
  Serial.println("");
}

void setup() {
  // initialize Serial port
  Serial.begin(0);
  while(!Serial);
  Serial.println("meow");

  // Initialize the OLED display at I2C address 0x3C
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED allocation failed");
    for (;;)
      ;  // Loop forever if OLED is not found
  }

  delay(20000);
}

void loop() {
  display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_WHITE);
  display.display();

  int t;

  for (t = 0; t <= 250; t += 50) {
    update_touch();
    if (pin_touched_now[IND_UP] && !pin_touched_past[IND_UP]) {
      if (!curr_pix_on)
        display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_BLACK);
      curr_pix_y -= 10;
      curr_pix_on = false;
      display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_WHITE);
      display.display();
    }

    if (pin_touched_now[IND_DOWN] && !pin_touched_past[IND_DOWN]) {
      if (!curr_pix_on)
        display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_BLACK);
      curr_pix_y += 10;
      curr_pix_on = false;
      display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_WHITE);
      display.display();
    }

    if (pin_touched_now[IND_LEFT] && !pin_touched_past[IND_LEFT]) {
      if (!curr_pix_on)
        display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_BLACK);
      curr_pix_x -= 10;
      curr_pix_on = false;
      display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_WHITE);
      display.display();
    }

    if (pin_touched_now[IND_RIGHT] && !pin_touched_past[IND_RIGHT]) {
      if (!curr_pix_on)
        display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_BLACK);
      curr_pix_x += 10;
      curr_pix_on = false;
      display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_WHITE);
      display.display();
    }

    if (pin_touched_now[IND_ADD] && !pin_touched_past[IND_ADD]) {
      curr_pix_on = true;
    }

    delay(50);
  }

  display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_BLACK);
  display.display();
  for (t = 0; t <= 250; t += 50) {
    update_touch();
    if (pin_touched_now[IND_UP] && !pin_touched_past[IND_UP]) {
      if (curr_pix_on)
        display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_WHITE);
      curr_pix_y -= 10;
      curr_pix_on = false;
    }

    if (pin_touched_now[IND_DOWN] && !pin_touched_past[IND_DOWN]) {
      if (curr_pix_on)
        display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_WHITE);
      curr_pix_y += 10;
      curr_pix_on = false;
    }

    if (pin_touched_now[IND_LEFT] && !pin_touched_past[IND_LEFT]) {
      if (curr_pix_on)
        display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_WHITE);
      curr_pix_x -= 10;
      curr_pix_on = false;
    }

    if (pin_touched_now[IND_RIGHT] && !pin_touched_past[IND_RIGHT]) {
      if (curr_pix_on)
        display.drawPixel(curr_pix_x, curr_pix_y, SSD1306_WHITE);
      curr_pix_x += 10;
      curr_pix_on = false;
    }

    if (pin_touched_now[IND_ADD] && !pin_touched_past[IND_ADD]) {
      curr_pix_on = true;
    }

    delay(50);
  }
  Serial.println(20);
}