#include "A4988.h"

int dataArray[29];

int MS1 = 33;    //GPIO13---D7 of Nodemcu--MS1 for A4988
int MS2 = 35;    //GPIO16---D0 of Nodemcu--MS2 for A4988
int MS3 = 36;    //GPIO15---D8 of Nodemcu--MS3 for A4988
int Sleep = 53;  //GPIO12---D6 of Nodemcu-Control Sleep Mode on A4988
int rpm = 100;
int microsteps = 8;

int commandData[3];
// Define an array of A4988 stepper motor objects
A4988 stepper(200, 22, 23, MS1, MS2, MS3);

A4988 steppers[2] = {
  A4988(200, 22, 23, MS1, MS2, MS3),
  A4988(200, 22, 23, MS1, MS2, MS3)
};

void setup() {
  Serial.begin(9600);
  Serial.println("Hello World!");
  stepper.begin(200, 2);

  // put your setup code here, to run once:
}
String input;


int localMotor = 0;
int localDegrees = 0;
int localSpeed = 0;


void loop() {
  if (Serial.available() > 0) {
    String inputString = Serial.readStringUntil('\n');
    getDataFromRotationCommand(inputString);
    Serial.println(commandData[0]);
    Serial.println(commandData[1]);
    Serial.println(commandData[2]);
  }
}

void getDataFromRotationCommand(String inputString) {
  String token;
  int tokenIndex = 0;
  int spaceIndex = inputString.indexOf(' ');
  if (inputString.substring(0, spaceIndex) != "rotate") {
    return;
  }
  while (inputString.length() > 0) {

    spaceIndex = inputString.indexOf(' ');
    token = inputString.substring(0, spaceIndex);
    inputString = inputString.substring(spaceIndex + 1);
    commandData[tokenIndex-1] = token.toInt();


    //switch (tokenIndex) {
    //  case 1:
    //    localMotor = token.toInt();
    //    commandData[0] = token.toInt();
    //    break;
    //  case 2:
    //    localDegrees = token.toInt();
    //    commandData[1] = token.toInt();
    //    break;
    //  case 3:
    //    localSpeed = token.toInt();
    //    commandData[2] = token.toInt();
    //    break;
    //  default:
    //    break;
    //}
    if (!(spaceIndex > 0)) {
      inputString = "";
    }
    tokenIndex++;
  }
  Serial.print("Motor: ");
  Serial.println(localMotor);
  Serial.print("Degrees: ");
  Serial.println(localDegrees);
  Serial.print("Speed: ");
  Serial.println(localSpeed);
  stepper.begin(localSpeed, microsteps);
  stepper.rotate(localDegrees);
}