#include <Servo.h>

// Servo objects
Servo servo1, servo2;

// Initial positions (start state)
int pos1 = 0; // Servo1 starts at 90 degrees
int pos2 = 0; // Servo2 starts at 90 degrees

// Target positions and intervals
int targetPos1 = 90, targetPos2 = 90; // Default target positions
unsigned long interval1 = 0, interval2 = 0; // Update intervals (in ms)

// Timer variables
unsigned long previousMillis_1 = 0;
unsigned long previousMillis_2 = 0;

void setup() {
    // Attach servos to pins
    servo1.attach(3); // Servo1 on pin 3
    servo2.attach(2); // Servo2 on pin 2

    // Set initial positions
    servo1.write(pos1);
    servo2.write(pos2);

    // Initialize serial communication
    Serial.begin(115200);
    Serial.println("Enter servo commands in the format: servo_number,interval,position");
    Serial.println("Example: 1,10,180 (Move servo1 with 10ms interval to position 180)");
}

void loop() {
    // Check if data is available on the serial port
    if (Serial.available() > 0) {
        String input = Serial.readStringUntil('\n'); // Read the input string
        input.trim(); // Remove any extra whitespace or newline characters

        // Debugging: Print the raw input
        Serial.print("Raw input: ");
        Serial.println(input);

        // Parse the input string
        int servoNumber, targetPosition;
        unsigned long interval;
        if (sscanf(input.c_str(), "%d,%lu,%d", &servoNumber, &interval, &targetPosition) == 3) {
            // Validate servo number
            if (servoNumber == 1 || servoNumber == 2) {
                // Validate position (0 to 180)
                if (targetPosition >= 0 && targetPosition <= 180) {
                    // Validate interval (must be positive)
                    if (interval > 0) {
                        if (servoNumber == 1) {
                            targetPos1 = targetPosition;
                            interval1 = interval;
                            Serial.print("Servo1 will move to position ");
                            Serial.print(targetPos1);
                            Serial.print(" with interval ");
                            Serial.print(interval1);
                            Serial.println("ms.");
                        } else if (servoNumber == 2) {
                            targetPos2 = targetPosition;
                            interval2 = interval;
                            Serial.print("Servo2 will move to position ");
                            Serial.print(targetPos2);
                            Serial.print(" with interval ");
                            Serial.print(interval2);
                            Serial.println("ms.");
                        }
                    } else {
                        Serial.println("Invalid interval. Interval must be greater than 0.");
                    }
                } else {
                    Serial.println("Invalid position. Position must be between 0 and 180.");
                }
            } else {
                Serial.println("Invalid servo number. Use 1 or 2.");
            }
        } else {
            Serial.println("Invalid input format. Use servo_number,interval,position.");
        }
    }

    // Move Servo1
    unsigned long currentMillis = millis();
    if (interval1 > 0 && currentMillis - previousMillis_1 >= interval1) {
        previousMillis_1 = currentMillis;

        if (pos1 < targetPos1) {
            pos1 += 1; // Increment position
        } else if (pos1 > targetPos1) {
            pos1 -= 1; // Decrement position
        }

        servo1.write(pos1); // Update servo1 position

        // Stop movement when target position is reached
        if (pos1 == targetPos1) {
            interval1 = 0; // Reset interval to stop further updates
        }
    }

    // Move Servo2
    if (interval2 > 0 && currentMillis - previousMillis_2 >= interval2) {
        previousMillis_2 = currentMillis;

        if (pos2 < targetPos2) {
            pos2 += 1; // Increment position
        } else if (pos2 > targetPos2) {
            pos2 -= 1; // Decrement position
        }

        servo2.write(pos2); // Update servo2 position

        // Stop movement when target position is reached
        if (pos2 == targetPos2) {
            interval2 = 0; // Reset interval to stop further updates
        }
    }
}