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

Adafruit_SSD1306 display(128, 64, &Wire, -1, 3000000); // Define Display

uint64_t start = 0;

int16_t xy[] = {
  84,  20,
  44,  20,
  84,  44,
  44,  44,
  64,  8,
  64,  55,
  104, 32,
  24,  32,
  104, 55,
  24,  55,
  44,  68,
  84,  68,
  104, 8,
  24,  8,
  44,  -4,
  84,  -4,
  124, 20,
  4,   20,
  124, 44,
  4,   44,
  124, -4,
  4,   -4,
  124, 68,
  4,   68,
};

double distanceAndAtanFromCenter[sizeof(xy) / sizeof(int16_t)];

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

  display.clearDisplay();                           // Clear Display Buffer
  display.setTextSize(1);                           // Text size should be 1px
  display.setTextColor(SSD1306_INVERSE);            // Toggle pixels while drawing instead of setting to one specific color
  display.setCursor(0, 0);                          // Reset Cursor
  display.print("Booting...");
  display.display();                                // Display On Screen

  pinMode(2, INPUT);
  
  start = micros();

  for (int i = 0; i < sizeof(xy) / sizeof(xy[0]); i+=2) { //          24000.0 + 500000
    distanceAndAtanFromCenter[i] = distance(64, 32, xy[i], xy[i+1]) * 32000.0 + 400000;
    distanceAndAtanFromCenter[i+1] = atan2(xy[i+1] - 32, xy[i] - 64);
  }
}

void loop() {
  double at;
  double dist = 1.0;

  while (dist > 0.0) {
    display.clearDisplay();
    display.fillCircle(64, 32, 8, SSD1306_WHITE);

    for (int i = 0; i < sizeof(xy) / sizeof(xy[0]); i+=2) {
      at = distanceAndAtanFromCenter[i+1];
      dist = c(constrain((double)(micros() - start) / distanceAndAtanFromCenter[i], 0.0, 1.0))*150.0;

      display.drawCircle(xy[i] + cosf(at)*dist, xy[i+1] + sinf(at)*dist, 8, SSD1306_WHITE);
    }

    display.display();
  }
  while(!digitalRead(2));
  start = micros();
}

double c(double t) { return pow(1.0 - t, 5.0); /*-sin((t * PI) / 2) + 1; /* -(1.0 - pow(1.0 - t, 4.0)) + 1.0;*/ }
double distance(double x0, double y0, double x1, double y1) { return sqrt(pow(x1 - x0, 2) + pow(y1 - y0, 2)); }