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

#define OLED_RESET -1 
#define SCREEN_ADDRESS 0x3C
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const uint8_t pot = A2;
int16_t tps = 0;

  float phi = 0;

  const uint8_t RMajor = 32;
  const uint8_t Rminor = 25;
  const uint8_t centerNeedleA = 63;
  const uint8_t centerNeedleB = 32;
  const uint8_t rMajorRear = 25;
  const uint8_t rMinorRear = 25;
  const uint8_t rMajorTail = 20;
  const uint8_t rMinorTail = 20;

  uint8_t RMajorEndX = 0;
  uint8_t RMajorEndY = 0;
  uint8_t RMinor1EndX = 0;
  uint8_t RMinor1EndY = 0;
  uint8_t RMinor2EndX = 0;
  uint8_t RMinor2EndY = 0;

  uint8_t rMajorRearEndX;
  uint8_t rMajorRearEndY;
  uint8_t rMinorRear1EndX;
  uint8_t rMinorRear1EndY;
  uint8_t rMinorRear2EndX;
  uint8_t rMinorRear2EndY;
  

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

  // initialize OLED display with I2C address 0x3C
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("failed to start SSD1306 OLED"));
    while (1);
  }

  //delay(2000);         // wait two seconds for initializing
  //oled.clearDisplay(); // clear display

  oled.setTextSize(1);         // set text size
  oled.setTextColor(WHITE);    // set text color
  //oled.setCursor(30, 20);       // set position to display
  //oled.println("Hello World"); // set text
  //oled.display();              // display on OLED
  //delay(500);
  oled.clearDisplay(); // clear display
  oled.display();              // display on OLED
  
}


void loop() {
  // put your main code here, to run repeatedly:
  oled.clearDisplay();

  //phi = (float)90 * (float)0.0174533 * 1.5;
  phi = 90.0 * 0.0174533; // solution one
  phi = (float)90 / (float)180 * 3.1415927; // solution two

  //phi = analogRead(pot)/(float)1023*6.2831853;

  // this works, but needs limiting against values outside range
  tps = analogRead(pot)/(float)1023*(float)100;
  phi = map(tps, 0, 100, 40, 140) * 0.0174533;

  tps = analogRead(pot);
  tps = map(tps, 0, 1023, 40, 140);
  tps = min(tps, 140);
  tps = max(tps, 40);
  phi = tps * 0.01745329252;

  oled.setCursor(0, 0);       // set position to display
  oled.println(phi);

  RMajorEndX = centerNeedleA          - cos(phi) * RMajor;
  RMajorEndY = centerNeedleB          - sin(phi) * RMajor;
  RMinor1EndX = centerNeedleA     - 1 - cos(phi) * Rminor;
  RMinor1EndY = centerNeedleB         - sin(phi) * Rminor;
  RMinor2EndX = centerNeedleA     + 1 - cos(phi) * Rminor;
  RMinor2EndY = centerNeedleB         - sin(phi) * Rminor;

  rMajorRearEndX = centerNeedleA      + cos(phi) * rMajorRear;
  rMajorRearEndY = centerNeedleB      + sin(phi) * rMajorRear;
  rMinorRear1EndX = centerNeedleA - 1 + cos(phi) * rMinorRear;
  rMinorRear1EndY = centerNeedleB     + sin(phi) * rMinorRear;
  rMinorRear2EndX = centerNeedleA + 1 + cos(phi) * rMinorRear;
  rMinorRear2EndY = centerNeedleB     + sin(phi) * rMinorRear;

  oled.drawLine(centerNeedleA, centerNeedleB, RMajorEndX, RMajorEndY, WHITE);
  oled.drawLine(centerNeedleA - 1, centerNeedleB, RMinor1EndX, RMinor1EndY, WHITE);
  oled.drawLine(centerNeedleA + 1, centerNeedleB, RMinor2EndX, RMinor2EndY, WHITE);

  oled.drawLine(centerNeedleA, centerNeedleB + 1, rMajorRearEndX, rMajorRearEndY, WHITE);
  oled.drawLine(centerNeedleA - 1, centerNeedleB + 1, rMinorRear1EndX, rMinorRear1EndY, WHITE);
  oled.drawLine(centerNeedleA + 1, centerNeedleB + 1, rMinorRear2EndX, rMinorRear2EndY, WHITE);

  oled.drawCircle(centerNeedleA, centerNeedleB, 6, WHITE);
  oled.display();
}