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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Servo servo1; // Servo objects
Servo servo2;
Servo servo3;
Servo servo4;

const int pot1 = A0; // Potentiometer pins
const int pot2 = A1;
const int pot3 = A2;
const int pot4 = A3;
const int pot5 = A4; // Added potentiometer pin

int pot1Val; // Potentiometer values
int pot2Val;
int pot3Val;
int pot4Val;
int pot5Val; // Added potentiometer value

int pot1Angle;
int pot2Angle;
int pot3Angle;
int pot4Angle;

int servoSpeed; // Servo speed
int prevServoSpeed = 20; // Previous servo speed

void setup() {
  servo1.attach(5); // Attach servos and define the pin modes
  servo2.attach(6);
  servo3.attach(9);
  servo4.attach(10);

  Serial.begin(9600);

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

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1.2);
  display.setCursor(0, 0);
  display.println("LOADING");
  display.display();
  delay(1000);
}

void loop() {
  pot1Val = analogRead(pot1); // Read the potentiometer values
  pot2Val = analogRead(pot2);
  pot3Val = analogRead(pot3);
  pot4Val = analogRead(pot4);
  pot5Val = analogRead(pot5); // Read the speed control potentiometer value

  pot1Angle = map(pot1Val, 0, 1023, 0, 179); // Map the potentiometer values to servo angles
  pot2Angle = map(pot2Val, 0, 1023, 0, 179);
  pot3Angle = map(pot3Val, 0, 1023, 0, 179);
  pot4Angle = map(pot4Val, 0, 1023, 0, 179);

  servoSpeed = map(pot5Val, 0, 1023, 40, 300); // Map the speed control potentiometer value to servo speed (1-100 degrees per second)

  if (servoSpeed != prevServoSpeed) { // Check if servo speed has changed
    prevServoSpeed = servoSpeed;
  }

  moveServosGradually(servo1, pot1Angle); // Move servos gradually
  moveServosGradually(servo2, pot2Angle);
  moveServosGradually(servo3, pot3Angle);
  moveServosGradually(servo4, pot4Angle);

  updateDisplay(); // Update OLED display
  delay(100);
}

void moveServosGradually(Servo &servo, int targetAngle) {
  int currentAngle = servo.read(); // Read current angle

  if (currentAngle < targetAngle) { // Gradually increase angle
    for (int i = currentAngle; i <= targetAngle; i++) {
      servo.write(i);
      delay(1000 / servoSpeed);
    }
  } else if (currentAngle > targetAngle) { // Gradually decrease angle
    for (int i = currentAngle; i >= targetAngle; i--) {
      servo.write(i);
      delay(1000 / servoSpeed);
    }
  }
}

void updateDisplay() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print(" BASE: ");
  display.println(pot1Angle);
  display.setCursor(0, 10);
  display.print(" HINGE1: ");
  display.println(pot2Angle);
  display.setCursor(0, 20);
  display.print(" HINGE2: ");
  display.println(pot3Angle);
  display.setCursor(0, 30);
  display.print(" CLAW: ");
  display.println(pot4Angle);
  display.setCursor(0, 40);
  display.print("\n SPEED: ");
  display.println(servoSpeed);
  display.display();
}