#include <Servo.h>
#include "servomove.h"

// ServoMover objects
ServoMover servoA; // Servo on pin 9
ServoMover servoB; // Servo on pin 8

// Buffer to accumulate incoming serial data
String receivedData = "";

void setup() {
  servoA.attach(9);  // Attach servo A
  servoB.attach(8);  // Attach servo B
  Serial.begin(9600);
  Serial.println("Starting Serial");
}

void loop() {
  // Read serial data
  while (Serial.available() > 0) {
    char inChar = (char)Serial.read();
    if (inChar == '\n') {           // End of command
      parseCommands(receivedData);   // Parse and execute command
      receivedData = "";             // Reset buffer for next command
    } else {
      receivedData += inChar;        // Accumulate characters
    }
  }

  // Update servo positions continuously
  servoA.update();
  servoB.update();
}

// Parse command format: "A:pos,speed;B:pos,speed"
void parseCommands(const String& commands) {
  String data = commands;
  data.trim();           // remove whitespace and \r
  data.replace(";", ""); // remove trailing semicolon if present

  int idxA = data.indexOf("A:");
  int idxB = data.indexOf("B:");

  if (idxA != -1) {
    int comma1 = data.indexOf(",", idxA);
    int posA = data.substring(idxA + 2, comma1).toInt();
    int speedA = data.substring(comma1 + 1, idxB != -1 ? idxB : data.length()).toInt();
    servoA.moveTo(posA, speedA);
  }

  if (idxB != -1) {
    int comma1 = data.indexOf(",", idxB);
    int posB = data.substring(idxB + 2, comma1).toInt();
    int speedB = data.substring(comma1 + 1).toInt();
    servoB.moveTo(posB, speedB);
  }
}