#include <Servo.h>

// Create servo objects:
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
// Define the servo pins:
#define servo1Pin 2
#define servo2Pin 4
#define servo3Pin 6
#define servo4Pin 7

void setup() {
  // attach servo objects to their pins
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
  servo4.attach(servo4Pin);
  // Starts the serial communication
  Serial.begin(9600); 
}

void loop() {

   if(Serial.available() > 0) // Read from serial port
    {
      String incomingString = Serial.readStringUntil('\n');
      String servoStr = getValue(incomingString, ' ', 0); // parse out servo 
      String angleStr = getValue(incomingString, ' ', 1); // parse out angle

      int servo = parseServoStr(servoStr); // convert servo str to servo int

      // check to see if a valid command was entered 
      if (servo != -1) {
        int angle = parseAngleStr(angleStr); // convert angle str to angle int
        if (angle != -1) {
          // Command is valid, execute it and write back to Serial
          Serial.print("Moving Servo ");
          Serial.print(servo);
          Serial.print(" to ");
          Serial.print(angle);
          Serial.println(" degrees");
          moveServo(servo, angle);
        } else {
          //  Invalid angle
          Serial.println("Invalid cmd received");
        }
      } else {
        // Invalid servo
        Serial.println("Invalid cmd received");
      }
    }
}

// move servo to angle
void moveServo(int servo, int angle) {
  switch (servo) {
    case 1:
      servo1.write(angle);
      break;
    case 2:
      servo2.write(angle);
      break;
    case 3:
      servo3.write(angle);
      break;
    case 4:
      servo4.write(angle);
      break;
    default:
      break;
  }
}

// convert servo str to servo int, and only return if it
// is a valid servo idx. Otherwise return -1
int parseServoStr(String servoStr) {
  int servo = servoStr.toInt();
  if ((servo < 1) || (servo > 4)) {
    return -1;
  }
  return servo;
}

// convert angle str to angle int, and only return if it
// is a valid angle value. Otherwise return -1
int parseAngleStr(String angleStr) {
  int angle = angleStr.toInt();
  if ((angle < 0) || (angle > 180)) {
    return -1;
  }
  return angle;
}

// for a given seperator, split data according to it, 
// and return the 'index' element of the split
// for example:
// getValue("This is a test", ' ', 2) -> "a"
// getValue("This is a test", ' ', 0) -> "This"
String getValue(String data, char separator, int index) {
  int found = 0;
  int strIndex[] = {0, -1};
  int maxIndex = data.length()-1;

  for(int i=0; i<=maxIndex && found<=index; i++){
    if(data.charAt(i)==separator || i==maxIndex){
        found++;
        strIndex[0] = strIndex[1]+1;
        strIndex[1] = (i == maxIndex) ? i+1 : i;
    }
  }

  return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
}