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

Adafruit_SSD1306 display(128, 64, &Wire, -1, 1000000);

static const uint8_t PROGMEM SEGA[] = {
  0b00111111, 0b11100111, 0b11111100, 0b11111111, 0b10000111, 0b00000000,
  0b01100000, 0b00101100, 0b00000101, 0b10000000, 0b10001101, 0b10000000,
  0b11011111, 0b11111011, 0b11111111, 0b01111111, 0b10001010, 0b10000000,
  0b10110000, 0b00110110, 0b00000110, 0b11000000, 0b10001010, 0b10000000,
  0b10101111, 0b11110101, 0b11111110, 0b10111111, 0b10011010, 0b11000000,
  0b10101111, 0b11110101, 0b11111110, 0b10111111, 0b10010101, 0b01000000,
  0b10110000, 0b11110100, 0b00001110, 0b10100000, 0b10010101, 0b01000000,
  0b11011111, 0b01110111, 0b11111110, 0b10111110, 0b10110101, 0b01100000,
  0b01100001, 0b10110100, 0b00001110, 0b10100010, 0b10101010, 0b10100000,
  0b00111110, 0b10110101, 0b11111110, 0b10111010, 0b10101010, 0b10100000,
  0b11111110, 0b10110101, 0b11111110, 0b10111010, 0b11101010, 0b10110000,
  0b10000001, 0b10110110, 0b00000110, 0b11000010, 0b11010100, 0b01010000,
  0b11111111, 0b01111011, 0b11111111, 0b01111110, 0b11010111, 0b11010000,
  0b10000000, 0b11001100, 0b00000101, 0b10000000, 0b11010100, 0b00010000,
  0b11111111, 0b10000111, 0b11111100, 0b11111111, 0b11111111, 0b11110000,
  44, 15,
};

void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);        // Begin Communication With Display

  display.clearDisplay();                           // Clear Display Buffer
  display.setTextSize(1);                           // Text size should be 1px
  display.setTextColor(SSD1306_INVERSE);            // Toggle pixels
  display.setCursor(0, 0);                          // Reset Cursor
  display.print("Booting...");
  display.display();                                // Display On Screen

  Serial.begin(115200);
}

float stretchY = 2;
float aStretchY;
float scaledHeight;
const uint8_t h = SEGA[sizeof(SEGA)-1];
const uint8_t w = SEGA[sizeof(SEGA)-2];
const uint8_t b = ceil(w/8.0);
uint8_t r;
uint8_t l;

uint start = millis();
uint16_t delta = 16;

void loop () {
  display.clearDisplay();

  stretchY = sin(start / 2000.0) * 4.27;
  aStretchY = abs(stretchY);
  scaledHeight = floor(h*aStretchY);
  
  uint8_t arr[(uint)(sizeof(SEGA)*aStretchY)];

  for (int16_t i=0; i<scaledHeight; i++) {
    l = (((stretchY<0) ? (-i + scaledHeight) : i)/aStretchY);

    for (uint8_t j = 0; j<b; j++) arr[i*b +j] = SEGA[l*b +j];
  }

  display.drawBitmap(42, 32 - (scaledHeight / 2), const_cast<uint8_t*>(arr), 44, scaledHeight, SSD1306_WHITE);
  
  display.setCursor(0, 0);
  display.print(delta);

  display.display();

  delta = 1000.0 / (millis() - start);
  start = millis();
}