#include <FastLED.h>

#define LED_PIN 6
#define WIDTH 8
#define HEIGHT 8
#define NUM_LEDS (WIDTH * HEIGHT)
CRGB leds[NUM_LEDS];


int index(int x, int y) {
  if (y % 2 == 0)
    return y * WIDTH + WIDTH - x - 1;
  else
    return y * WIDTH + x;
}


class keyboard_t {
private:
  int row_pins[4];
  int col_pins[4];

public:
  keyboard_t(int rows[4], int cols[4]) {
    for (int i = 0; i < 4; i++) {
      int row_pin = rows[i];
      row_pins[i] = row_pin;
      pinMode(row_pin, INPUT_PULLUP);
    }

    for (int i = 0; i < 4; i++) {
      int col_pin = cols[i];
      col_pins[i] = col_pin;
      pinMode(col_pin, INPUT);
    }
  }

  bool isPressed(int x, int y) {
    int col_pin_no = this->col_pins[x];
    pinMode(col_pin_no, OUTPUT);
    digitalWrite(col_pin_no, LOW);

    int row_pin_no = this->row_pins[y];
    int return_value = digitalRead(row_pin_no);

    pinMode(col_pin_no, INPUT);
    if (return_value != 0)
      return 0;
    else
      return 1;
  }
};

int row_pins[] = { 2, 3, 4, 5 };
int col_pins[] = { A0, A1, A2, A3 };
keyboard_t kbd(row_pins, col_pins);


void setup() {
  Serial.begin(115200);
  FastLED.addLeds<NEOPIXEL, 6>(leds, NUM_LEDS);
}

#define SNAKE_SIZE 5
int pixel_x[SNAKE_SIZE] = { 0 };
int pixel_y[SNAKE_SIZE] = { 0 };
int pixel_vx = 0;
int pixel_vy = 0;

int deadline = 0;

// https://pastebin.com/Djeaivk4
void loop() {
  if (kbd.isPressed(1, 0)) {
    pixel_vx = 0;
    pixel_vy = 1;
  } else if (kbd.isPressed(0, 1)) {
    pixel_vx = -1;
    pixel_vy = 0;
  } else if (kbd.isPressed(2, 1)) {
    pixel_vx = +1;
    pixel_vy = 0;
  } else if (kbd.isPressed(1, 2)) {
    pixel_vx = 0;
    pixel_vy = -1;
  }

  int now = millis();
  if (now >= deadline) {
    for (int i = 0; i < NUM_LEDS; i++)
      leds[i].setRGB(0, 0, 0);

    for (int i = SNAKE_SIZE - 1; i > 0; i--) {
      pixel_x[i] = pixel_x[i - 1];
      pixel_y[i] = pixel_y[i - 1];
    }
    pixel_x[0] = pixel_x[1] + pixel_vx;
    pixel_y[0] = pixel_y[1] + pixel_vy;
    if (pixel_x[0] < 0)
      pixel_x[0] = WIDTH - 1;
    if (pixel_x[0] >= WIDTH)
      pixel_x[0] = 0;
    if (pixel_y[0] < 0)
      pixel_y[0] = HEIGHT - 1;
    if (pixel_y[0] >= HEIGHT)
      pixel_y[0] = 0;

    for (int i = 0; i < 5; i++)
      if (i == 0) {
        leds[index(pixel_x[i], pixel_y[i])].setRGB(255, 0, 0);
      } else if(i == 4) {
        leds[index(pixel_x[i], pixel_y[i])].setRGB(255, 255, 0);
      }
      else{
        leds[index(pixel_x[i], pixel_y[i])].setRGB(0, 255, 0);
      }
    FastLED.show();

    deadline = now + 500;
  }
}