#include <Servo.h>

// ServoPin combines a Servo object with the
// pin it is connected to. This must be matched
// by the physical hardware setup.
typedef struct {
  Servo servo;
  int   pin;
} ServoPin;

// servos connects the servo index to a Servo object and
// the pin on the board the servo is connected to.
ServoPin servos[] = {
  { Servo(), 3},     // servo 1
  { Servo(), 5},     // servo 2 ...
  { Servo(), 6},
  { Servo(), 9},
  { Servo(), 10},
  { Servo(), 11}
};

// N_SERVOS is used to iterate over servos and validate
// provided servo indices.
const int N_SERVOS = sizeof(servos)/sizeof(ServoPin);

// Onboard LED used to signal that there was an error.
// It blinks to indicate error. Only really used during debugging.
const int LED_PIN = 13;

// OP_CALIBRATE command puts the servo into a defined position.
// This defined position allows the lever to be added to the servo
// in a well-known position such that it can activate the power switch.
const char* OP_CALIBRATE = "calibrate";

// OP_RESET runs through the reset routine.
const char* OP_RESET     = "reset";

// Position of the servo at which the lever hovers over the power button.
const int HOVER_POSITION = 20;

// Position of the servo at which the lever pushes onto the power button.
const int PUSH_POSITION = 10;

// Duration during which the servo pushes on the power button during reset.
const int RESET_DELAY = 12000;

void setup(){
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);

  // Initialize the serial interface to run at 9600 baud.
  Serial.begin(9600);
  for(int i=0; i < N_SERVOS; i++) {
    servos[i].servo.attach(servos[i].pin);
    calibrateServo(i);
  }
}

boolean validPositiveInt(const String& intStr, int& result) {
  if (intStr.length() == 0) {
    return false;
  }
  for(int i=0; i<intStr.length(); i++) {
    if (!isDigit(intStr[i])) {
      return false;
    }
  }

  result = intStr.toInt();
  return true;
}

void loop(){
  if (Serial.available())  {
     String cmd, arg;
     readCommand(&cmd, &arg);

     if (cmd == "") {
       return;
     }

     int intArg;
     if (!validPositiveInt(arg, intArg) || (intArg < 1) ||
         (intArg > N_SERVOS)) {
       blinkLED(10);
       return;
     }

     if (cmd == OP_CALIBRATE) {
       calibrateServo(intArg-1);
     } else if (cmd == OP_RESET) {
       resetServo(intArg-1);
     } else {
       blinkLED(3);
     }
  }
  delay(100);
}

// Calibrate the servo.
void calibrateServo(int idx) {
  servos[idx].servo.write(HOVER_POSITION);
}

// Reset the device.
void resetServo(int idx) {
 servos[idx].servo.write(PUSH_POSITION);
 delay(RESET_DELAY);
 servos[idx].servo.write(HOVER_POSITION);
}

// Blinks the LED so many times to indicate error.
void blinkLED(int times) {
  delay(100);
  for(int i=0; i<times; i++) {
    digitalWrite(LED_PIN, HIGH);
    delay(500);
    digitalWrite(LED_PIN, LOW);
    delay(300);
  }
}

// readCommand reads in characters from the Serial port until a newline
// is reached. The first word (delimited by a space) is returned via in the
// cmd argument and the remainder of the string is returned via the 'arg'
// argument.
void readCommand(String* cmd, String* arg) {
  const int IN_BUF_SIZE = 256;
  char in_buf[IN_BUF_SIZE];

  int n = Serial.readBytesUntil('\n', in_buf, IN_BUF_SIZE-1);
   in_buf[n] = 0;

   String inStr = String(in_buf);
   inStr.trim();
   int sep = inStr.indexOf(' ');
   if (sep == -1) {
     *cmd = String(inStr);
     cmd->trim();
     *arg = String("");
     return;
   }
   if (sep == -1) {
     sep = inStr.length();
   }
   *cmd = inStr.substring(0, sep);
   *arg = inStr.substring(sep+1, inStr.length());
}