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

// OLED Display Settings
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Servo Setup
Servo servo1, servo2, servo3, servo4, servo5;
int pos[5] = {90, 90, 90, 90, 90};  // เริ่มต้นที่ 90 องศา
int speedDelay = 10;  // หน่วงเวลาการเคลื่อนที่

// Joystick Pins
#define JOYX A0
#define JOYY A1
#define BUTTON 2

void setup() {
    Serial.begin(9600);
    
    // Attach Servo Motors
    servo1.attach(3);
    servo2.attach(2);
    servo3.attach(4);
    servo4.attach(5);
    servo5.attach(6);
    
    // Set Joystick Button
    pinMode(BUTTON, INPUT_PULLUP);

    // Initialize OLED Display
    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        Serial.println("OLED init failed!");
        while (1);
    }
    display.clearDisplay();
    updateDisplay();

    Serial.println("Enter: servo(1-5) position(0-180) speed(ms)");
}

void loop() {
    // ✅ อ่านค่าจาก Serial Monitor
    if (Serial.available()) {
        int servoNum = Serial.parseInt();
        int newPos = Serial.parseInt();
        int newSpeed = Serial.parseInt();
        
        if (servoNum >= 1 && servoNum <= 5 && newPos >= 0 && newPos <= 180) {
            Serial.print("Moving Servo ");
            Serial.print(servoNum);
            Serial.print(" to ");
            Serial.println(newPos);

            if (newSpeed > 0) speedDelay = newSpeed;

            moveServo(servoNum, newPos);
            updateDisplay();
        } else {
            Serial.println("Invalid Input!");
        }
    }

    // ✅ ควบคุมด้วยจอยสติก (Manual Mode)
    manualControl();
}

// 🔹 ฟังก์ชันเคลื่อนที่แบบ Smooth
void moveServo(int servoNum, int newPos) {
    Servo *servos[] = {&servo1, &servo2, &servo3, &servo4, &servo5};
    int index = servoNum - 1;

    if (newPos > pos[index]) {
        for (; pos[index] <= newPos; pos[index]++) {
            servos[index]->write(pos[index]);
            delay(speedDelay);
        }
    } else {
        for (; pos[index] >= newPos; pos[index]--) {
            servos[index]->write(pos[index]);
            delay(speedDelay);
        }
    }
}

// 🔹 ฟังก์ชันควบคุมแบบจอยสติก
void manualControl() {
    int joyX = analogRead(JOYX);
    int joyY = analogRead(JOYY);
    bool buttonPress = digitalRead(BUTTON) == LOW;

    if (joyX < 400) pos[0]--;  // ซ้าย
    if (joyX > 600) pos[0]++;  // ขวา
    if (joyY < 400) pos[1]--;  // ลง
    if (joyY > 600) pos[1]++;  // ขึ้น
    if (buttonPress) pos[4] = 180 - pos[4]; // สลับการจับสิ่งของ

    pos[0] = constrain(pos[0], 0, 180);
    pos[1] = constrain(pos[1], 0, 180);
    pos[4] = constrain(pos[4], 0, 180);

    servo1.write(pos[0]);
    servo2.write(pos[1]);
    servo5.write(pos[4]);

    updateDisplay();
    delay(100);
}

// 🔹 ฟังก์ชันแสดงผลบน OLED
void updateDisplay() {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    
    display.setCursor(0, 0);
    display.println("Arm Position:");
    for (int i = 0; i < 5; i++) {
        display.print("S");
        display.print(i + 1);
        display.print(": ");
        display.print(pos[i]);
        display.println(" deg");
    }
    
    display.display();
}