#include <Wire.h> // me: library to comunicate with 12C things aka. the display
#include <Adafruit_GFX.h> // me: general graphic display library
#include <Adafruit_SSD1306.h> // me: library specific to this display I'm using (I think)
#include <math.h>

#define SCREEN_WIDTH 128 // pro: OLED display width, in pixels
#define SCREEN_HEIGHT 64 // pro: LED display height, in pixels
// pro: Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

#define calcAccel(deg, gravity) (cos(deg * (PI / 180)) * gravity)
#define calcX(deg, lenght) (lenght * cos(deg * (PI / 180)))
#define calcY(deg, lenght) (lenght * sin(deg * (PI / 180)))

float resetDeg = 45.000f;
int btnPin = A3;
float time = 0.000f;
float interval = 0.5f;

struct Object{
  float deg = resetDeg;
  float vel = 0.000f;
  float grav = 9.83f;
  float dist = 0.000f;
  float len = 64.0f;
  float accel = calcAccel(deg, grav);
  int originPos[2] = {0, SCREEN_HEIGHT - 1};
  int movingPos[2] = {ceil(calcX(deg, len)), ceil(calcY(deg, len))};
};

struct Object plank;

void setup() {
  pinMode(A3, INPUT);
  // me: start a serial
  Serial.begin(9600);
  // me: check if initialization of the screen succeded
  if(!display.begin(SSD1306_SWITCHCAPVCC,0x3C)){
    Serial.println("Failed to connect");
  while(1);
  }
  delay(2000);
}

void drawScreen() {
  display.display();
  delay(10);
  display.clearDisplay();
  // for debuging
  // Serial.println(analogRead(btnPin)); 
  if (analogRead(btnPin) == 1023){
    time = 0;
    plank.deg = resetDeg;
    plank.vel = 0;
    plank.accel = 0;
  }
  display.drawLine(plank.originPos[0], plank.originPos[1], plank.movingPos[0], 63 - plank.movingPos[1], WHITE);
}

void updatePos() {
  if (plank.deg <= 0){
    plank.deg = 0;
  }
  plank.movingPos[0] = ceil(calcX(plank.deg, plank.len));
  plank.movingPos[1] = ceil(calcY(plank.deg, plank.len));
  //Serial.println("X: ");
  //Serial.println(plank.movingPos[0]);
  //Serial.println("Y: ");
  //Serial.println(plank.movingPos[1]);

}

void debug(){
  Serial.println("Velocity");
  Serial.println(plank.vel, 4);
  Serial.println("Degrees");
  Serial.println(plank.deg, 4);
  //Serial.println("Acceleration");
  //Serial.println(plank.accel);
  //Serial.println("time + interval");
  //Serial.println(time + interval, 4);
  //Serial.println("Accel * (time + interval)");
  //Serial.println(plank.accel * (time + interval));
  //Serial.println("Accel * time");
  //Serial.println(plank.accel * time);
  Serial.println("-------------------");
}

void updateRotation(){
  plank.vel += (plank.accel * interval)/2;
  time += interval;
  plank.dist = plank.vel * interval;
  plank.deg -= asin(plank.dist/(plank.len/2)) * (180 / PI);
  plank.accel = calcAccel(plank.deg, plank.grav);
  //debug();
}

void loop() {
  updateRotation();
  updatePos();
  drawScreen();
}