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

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

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

// Touch pad GPIO pins
const int TOUCH0_PIN = 3;
const int TOUCH1_PIN = 4;

const int TOUCH_THRESHOLD = 6;
const int TOUCH_MAX = 200;

// Rotation angle in radians
float angleX = 0.0;

// How quickly the model rotates while touching
const float ROTATION_SPEED = 0.08;

struct Point3D {
  float x;
  float y;
  float z;
};

Point3D protein[] = {
  {-20,  0,  0},
  {-15,  8,  5},
  {-10, 15, 12},
  { -2, 10, 20},
  {  8,  0, 16},
  { 15, -8,  5},
  { 20,  0, -5}
};

const int NUM_POINTS = sizeof(protein) / sizeof(protein[0]);

bool touch0 = false;
bool touch1 = false;

// Measure one touch pad
int measureTouch(int pin) {
  int timeCount = 0;

  // Discharge the touch pad
  pinMode(pin, OUTPUT);
  digitalWrite(pin, LOW);

  delayMicroseconds(25);

  // Let the pad charge through the internal pull-up
  pinMode(pin, INPUT_PULLUP);

  // Count how long it takes to become HIGH
  while (!digitalRead(pin) && timeCount < TOUCH_MAX) {
    timeCount++;
  }

  return timeCount;
}

// Update the states of both touch pads
void updateTouch() {
  int value0 = measureTouch(TOUCH0_PIN);
  int value1 = measureTouch(TOUCH1_PIN);

  touch0 = value0 > TOUCH_THRESHOLD;
  touch1 = value1 > TOUCH_THRESHOLD;
}

// Rotate a point around the X axis
Point3D rotateAroundX(Point3D point, float angle) {
  Point3D rotated;

  rotated.x = point.x;
  rotated.y = point.y * cos(angle) - point.z * sin(angle);
  rotated.z = point.y * sin(angle) + point.z * cos(angle);

  return rotated;
}

// Convert a rotated point to an OLED x-coordinate
int screenX(Point3D point) {
  return 64 + point.x;
}

// Convert a rotated point to an OLED y-coordinate
int screenY(Point3D point) {
  return 32 - point.y;
}

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

  delay(50);

  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println("OLED initialization failed");

    while (true) {
      // Stop if the display cannot be initialized
    }
  }

  display.clearDisplay();
  display.display();
}

void loop() {
  // Read the touch pads
  updateTouch();

  // Rotate while TOUCH0 is being held
  if (touch0) {
    angleX += ROTATION_SPEED;
  }

  // Rotate in the opposite direction while TOUCH1 is being held
  if (touch1) {
    angleX -= ROTATION_SPEED;
  }

  // Keep angleX within one full rotation
  if (angleX > 2 * PI) {
    angleX -= 2 * PI;
  }

  if (angleX < -2 * PI) {
    angleX += 2 * PI;
  }

  // Clear the OLED drawing buffer
  display.clearDisplay();

  // Draw connected protein segments
  for (int i = 0; i < NUM_POINTS - 1; i++) {
    Point3D pointA = rotateAroundX(protein[i], angleX);
    Point3D pointB = rotateAroundX(protein[i + 1], angleX);

    int x1 = screenX(pointA);
    int y1 = screenY(pointA);

    int x2 = screenX(pointB);
    int y2 = screenY(pointB);

    display.drawLine(x1, y1, x2, y2, SSD1306_WHITE);
    display.fillCircle(x1, y1, 2, SSD1306_WHITE);
  }

  // Draw the final point
  Point3D finalPoint =
    rotateAroundX(protein[NUM_POINTS - 1], angleX);

  display.fillCircle(
    screenX(finalPoint),
    screenY(finalPoint),
    2,
    SSD1306_WHITE
  );

  // Send the completed frame to the OLED
  display.display();

  delay(30);
}