#include <string.h>
#include <ServoShield.h>

// Define global variables
#define NUMBER_OF_SERVOS 16  // The total number of robot's servos

ServoShield shieldServos; // ServoShield interfacing object

int servominpos[NUMBER_OF_SERVOS] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000};      // Servos Minimum Positions
int servomaxpos[NUMBER_OF_SERVOS] = {2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000};  // Servos Maximum Positions
int servomidpos[NUMBER_OF_SERVOS] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};  // Servos Initial Positions


void setup()
{

  Serial.begin(19200);
  delay(1000);

  InitializeAllServos();

  if (shieldServos.start())
  {
    Serial.println("Servo Shield started succefully...");
  }
  else
  {
    Serial.println("Servo Shield failed to start, DO NOT send any servo command, check your Servo shield connectivity...");
  }

  delay(2000);
}


void loop()
{
  char servoCommand[256];
  char delim[] = ",";
  char *motion = NULL;

  while (1)
  {

    *motion = NULL;

    // Read the servo command from the serial port, Servo Command Format: SxxPyyyy,SxxPyyyy,SxxPyyyy,.... example S01P1000,S02P0500,S03P20,...
    // Notes: DO NOT OMMIT the leading zeroes e.g. S1P1000 --> Wrong, S01P1000 --> Correct, S02P500-->Wrong, S02P0500--> Correct
    // You can use any number of your servos in any order, just use the comma for separator e.g. S01P2000,S03P0600,S02P1750,S11P2300
    GetStringFromSerial(servoCommand, sizeof(servoCommand));

    //If we send 'x' instead of servo command the programm will end
    //if (servoCommand[0] =='x')
    //{
      //break;
    //}

    Serial.println(servoCommand);

    // Start the Servo Command parsing
    motion = strtok( servoCommand, delim);

    while( motion != NULL )
    {
      //extract motion data (servo id and position) from servo command
      String extractedMotion = motion;
      char servoid[extractedMotion.substring(1,3).length() + 1];
      char servopos[extractedMotion.substring(4,8).length() + 1];
      extractedMotion.substring(1,3).toCharArray(servoid, sizeof(servoid));
      extractedMotion.substring(4,8).toCharArray(servopos, sizeof(servopos));

      // Convert extracted strings to integers
      int sid = atoi(servoid);
      int spos = atoi(servopos);

      //Print some debug info
      char debugMsg[256];
      sprintf(debugMsg,"Motion string: %s   Servo Id: %d   Servo Position: %d", motion, sid, spos);
      Serial.println(debugMsg);

      // Move the
      shieldServos.setposition(sid , spos);
      delay(2000);


      // Continue parsing
      motion= strtok( NULL, delim );
    }

    delay(1);
  }

}


void InitializeAllServos()
{
  // Initialize all servos and set the max and min and starting positions
  for (int i=0;i<NUMBER_OF_SERVOS;i++)
  {
    shieldServos.setbounds(i, servominpos[i], servomaxpos[i]);  //Set the minimum and maximum pulse duration of the servo
    delay(150);
    shieldServos.setposition(i, servomidpos[i]);      //Set the initial position of the servo
    delay(150);
  }
}


void GetStringFromSerial(char *buf, int bufsize)
{
  int i;
  for (i=0; i<bufsize - 1; ++i)
  {
    while (Serial.available() == 0); // wait for character to arrive
    buf[i] = Serial.read();
    if (buf[i] == 10 || buf[i] == 13) // if LF or CR character found (end of string) break
      break;
  }
  buf[i] = 0; // 0 string terminator just in case
}