#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; // Servos
Servo servo2;
Servo servo3;
Servo servo4;

const int pot1 = A0; // Potentiometers
const int pot2 = A1;
const int pot3 = A2;
const int pot4 = A3;
const int pot5 = A4; // Potentiometer for speed control

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

int pot1Angle;
int pot2Angle;
int pot3Angle;
int pot4Angle;
int pot5Angle; // Mapped speed control value

unsigned long startupTime; // Variable to track startup time

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

  servo1.write(0); // Initialize servos to 0 degrees
  servo2.write(0);
  servo3.write(0);
  servo4.write(0);

  Serial.begin(9600);
  
  // Initialize the OLED display with the SSD1306 driver
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  
  // Clear the display buffer.
  display.clearDisplay();
  
  // Set text color, size, and position
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0,0);
  
  // Run the startup sequence
  runStartupSequence();
}

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

  // Map the potentiometer values to servo angles
  pot1Angle = map(pot1Val, 0, 511, 0, 90); // Map only half the range for slower speed
  pot2Angle = map(pot2Val, 0, 511, 0, 90);
  pot3Angle = map(pot3Val, 0, 511, 0, 90);
  pot4Angle = map(pot4Val, 0, 511, 0, 90);
  pot5Angle = map(pot5Val, 0, 1023, 50, 1000); // Map the speed control to a suitable range

  // Control the servos with mapped angles
  servo1.write(pot1Angle);
  servo2.write(pot2Angle);
  servo3.write(pot3Angle);
  servo4.write(pot4Angle);

  // Update the OLED display with potentiometer angles
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("BASE: ");
  display.println(pot1Angle);
  display.setCursor(0, 10);
  display.print("ROT1: ");
  display.println(pot2Angle);
  display.setCursor(0, 20);
  display.print("ROT2: ");
  display.println(pot3Angle);
  display.setCursor(0, 30);
  display.print("CLAW: ");
  if (pot4Angle == 0) {
    display.println("OPEN");
  } else if (pot4Angle == 180) {
    display.println("CLOSED");
  } else {
    display.println(pot4Angle);
  }
  display.setCursor(0, 40);
  display.print("SPEED: ");
  display.println(pot5Angle);
  
  display.display();
  
  // Delay before next loop iteration
  delay(300);
}

void runStartupSequence() {
  startupTime = millis(); // Record the startup time
  
  while (millis() - startupTime < 6000) { // Run for 6 seconds
    // Display "Initializing..." with blinking dots
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(0,0);
    display.print("Initializing");
    for (int j = 0; j < 2; j++) { // Loop twice
    for (int i = 0; i < 3; i++) {
      display.print(".");
      display.display();
      delay(200);
      display.print(" ");
      display.display();
      delay(200);
    }}
    display.display();
    
    // Move BASE to 0 degrees
    display.clearDisplay();
    display.setCursor(0,0);
    display.print("Base Rotating");
    for (int j = 0; j < 2; j++) { // Loop twice
    for (int i = 0; i < 3; i++) {
      display.print(".");
      display.display();
      delay(200);
      display.print(" ");
      display.display();
      delay(200);
    }}
    moveServoSmooth(servo1, 0, pot5Angle); // Smoothly move servo1 to 0 degrees
    delay(1000);
    
    // Move ROT1 and ROT2 to 90 degrees
    display.clearDisplay();
    display.setCursor(0,0);
    display.print("Leap of Faith");
    for (int j = 0; j < 2; j++) { // Loop twice
    for (int i = 0; i < 3; i++) {
      display.print(".");
      display.display();
      delay(200);
      display.print(" ");
      display.display();
      delay(200);
    }}
    moveServoSmooth(servo2, 90, pot5Angle); // Smoothly move servo2 to 90 degrees
    moveServoSmooth(servo3, 90, pot5Angle); // Smoothly move servo3 to 90 degrees
    delay(1000);
    
    // Move CLAW to 0 degrees
    display.clearDisplay();
    display.setCursor(0,0);
    display.print("Open Handedly");
    for (int j = 0; j < 2; j++) { // Loop twice
    for (int i = 0; i < 3; i++) {
      display.print(".");
      display.display();
      delay(200);
      display.print(" ");
      display.display();
      delay(200);
    }}
    moveServoSmooth(servo4, 0, pot5Angle); // Smoothly move servo4 to 0 degrees
    delay(1000);
    
    // Move CLAW to 180 degrees
    display.clearDisplay();
    display.setCursor(0,0);
    display.print("Grabs the can \n of Pop");
    for (int j = 0; j < 2; j++) { // Loop twice
    for (int i = 0; i < 3; i++) {
      display.print(".");
      display.display();
      delay(200);
      display.print(" ");
      display.display();
      delay(200);
    }}
    moveServoSmooth(servo4, 180, pot5Angle); // Smoothly move servo4 to 180 degrees
    delay(1000);
    
    // Move ROT1 and ROT2 to 0 degrees
    display.clearDisplay();
    display.setCursor(0,0);
    display.print("Pulling Out \n >__>");
    for (int j = 0; j < 2; j++) { // Loop twice
    for (int i = 0; i < 3; i++) {
      display.print(".");
      display.display();
      delay(200);
      display.print(" ");
      display.display();
      delay(200);
    }}
    moveServoSmooth(servo2, 0, pot5Angle); // Smoothly move servo2 to 0 degrees
    moveServoSmooth(servo3, 0, pot5Angle); // Smoothly move servo3 to 0 degrees
    delay(1000);
    
    // Move BASE to 180 degrees
    display.clearDisplay();
    display.setCursor(0,0);
    display.print("Weeee");
    for (int j = 0; j < 2; j++) { // Loop twice
    for (int i = 0; i < 3; i++) {
      display.print(".");
      display.display();
      delay(200);
      display.print(" ");
      display.display();
      delay(200);
    }}
    moveServoSmooth(servo1, 180, pot5Angle); // Smoothly move servo1 to 180 degrees
    delay(1000);
    
    // Move ROT1 and ROT2 to 90 degrees
    display.clearDisplay();
    display.setCursor(0,0);
    display.print("Here you go");
    for (int j = 0; j < 2; j++) { // Loop twice
    for (int i = 0; i < 3; i++) {
      display.print(".");
      display.display();
      delay(200);
      display.print(" ");
      display.display();
      delay(200);
    }}
    moveServoSmooth(servo2, 90, pot5Angle); // Smoothly move servo2 to 90 degrees
    moveServoSmooth(servo3, 90, pot5Angle); // Smoothly move servo3 to 90 degrees
    delay(1000);
    
    // Move CLAW to 0 degrees
    display.clearDisplay();
    display.setCursor(0,0);
    display.print("Yum \n :P");
    for (int j = 0; j < 2; j++) { // Loop twice
    for (int i = 0; i < 3; i++) {
      display.print(".");
      display.display();
      delay(200);
      display.print(" ");
      display.display();
      delay(200);
    }}
    moveServoSmooth(servo4, 0, pot5Angle); // Smoothly move servo4 to 0 degrees
    delay(1000);
    
    // Move all servos to 0 degrees and claw to 180 degrees closed
    display.clearDisplay();
    display.setCursor(0,0);
    display.print("All Systems \n Ready!");
    for (int j = 0; j < 2; j++) { // Loop twice
    for (int i = 0; i < 3; i++) {
      display.print(".");
      display.display();
      delay(200);
      display.print(" ");
      display.display();
      delay(200);
    }}
    moveServoSmooth(servo1, 0, pot5Angle); // Smoothly move servo1 to 0 degrees
    moveServoSmooth(servo2, 0, pot5Angle); // Smoothly move servo2 to 0 degrees
    moveServoSmooth(servo3, 0, pot5Angle); // Smoothly move servo3 to 0 degrees
    moveServoSmooth(servo4, 180, pot5Angle); // Smoothly move servo4 to 180 degrees
    delay(1000);
  }
}

void moveServoSmooth(Servo servo, int targetAngle, int speed) {
  int currentAngle = servo.read();
  if (currentAngle < targetAngle) {
    for (int i = currentAngle; i <= targetAngle; i++) {
      servo.write(i);
      delay(speed); // Adjust the delay based on the speed control
    }
  } else {
    for (int i = currentAngle; i >= targetAngle; i--) {
      servo.write(i);
      delay(speed); // Adjust the delay based on the speed control
    }
  }
}