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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

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

// Constants for arm lengths (in millimeters)
const float arm1Length = 20.0; // Adjusted for visibility on the screen
const float arm2Length = 40.0; // Adjusted for visibility on the screen
const float arm3Length = 40.0; // Adjusted for visibility on the screen
const float arm4Length = 12.0; // Adjusted for visibility on the screen

// Analog input pins for potentiometers
const int potPin1 = A0; // Joint 1
const int potPin2 = A1; // Joint 2
const int potPin3 = A2; // Joint 3
const int potPin4 = A3; // Base rotation

// Variables to store potentiometer readings
int potValue1, potValue2, potValue3, potValue4;

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

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }

  display.display(); // Clear the display buffer.

  delay(2000); // Delay to see the initial display before it updates
}

void loop() {
  // Read potentiometer values
  potValue1 = analogRead(potPin1);
  potValue2 = analogRead(potPin2);
  potValue3 = analogRead(potPin3);
  potValue4 = analogRead(potPin4);

  // Calculate angles in degrees
  float angle1_deg = map(potValue1, 0, 1023, 0, 180);
  float angle2_deg = map(potValue2, 0, 1023, 0, 180);
  float angle3_deg = map(potValue3, 0, 1023, 0, 180);
  float baseAngle_deg = map(potValue4, 0, 1023, 0, 360); // Assuming 360 degrees for base rotation

  // Convert angles to radians for trigonometric calculations
  float angle1_rad = radians(angle1_deg);
  float angle2_rad = radians(angle2_deg);
  float angle3_rad = radians(angle3_deg);
  float baseAngle_rad = radians(baseAngle_deg);

  // Calculate coordinates
  float x1 = 0;
  float y1 = SCREEN_HEIGHT;
  float x2 = x1 + arm1Length * sin(angle1_rad);
  float y2 = y1 - arm1Length * cos(angle1_rad);
  float x3 = x2 + arm2Length * sin(angle1_rad + angle2_rad);
  float y3 = y2 - arm2Length * cos(angle1_rad + angle2_rad);
  float x4 = x3 + arm3Length * sin(angle1_rad + angle2_rad + angle3_rad);
  float y4 = y3 - arm3Length * cos(angle1_rad + angle2_rad + angle3_rad);
  float x5 = x4 + arm4Length * cos(baseAngle_rad);
  float y5 = y4 + arm4Length * sin(baseAngle_rad);

  // Clear display
  display.clearDisplay();

  // Draw arm segments
  display.drawLine(x1, y1, x2, y2, SSD1306_WHITE);
  display.drawLine(x2, y2, x3, y3, SSD1306_WHITE);
  display.drawLine(x3, y3, x4, y4, SSD1306_WHITE);
  display.drawLine(x4, y4, x5, y5, SSD1306_WHITE);

  // Display
  display.display();

  // Delay for stability or add additional logic based on your requirements
  delay(500);
}