#include <SoftwareSerial.h>
#include <SerialCommand.h>
#include <AFMotor.h>
#include <AccelStepper.h>

SerialCommand serial_command;

AF_Stepper motor1(200, 1);
AF_Stepper motor2(200, 2);

long m1 = 0;
long m2 = 0;

long m1target = 0;
long m2target = 0;

boolean dir = false;

void forwardstep1() {  
  motor1.onestep(FORWARD, SINGLE);
}
void backwardstep1() {  
  motor1.onestep(BACKWARD, SINGLE);
}
// wrappers for the second motor!
void forwardstep2() {  
  motor2.onestep(FORWARD, SINGLE);
}
void backwardstep2() {  
  motor2.onestep(BACKWARD, SINGLE);
}

AccelStepper stepper1(forwardstep1, backwardstep1);
AccelStepper stepper2(forwardstep2, backwardstep2);

void setup(){  

  Serial.begin(9600);

  serial_command.addCommand("steps", stepTo);
  serial_command.addDefaultHandler(unrecognized);

  stepper1.setMaxSpeed(600.0);
  stepper1.setAcceleration(60.0);

  stepper2.setMaxSpeed(600.0);
  stepper2.setAcceleration(60.0);

}

void loop(){

  // SERIAL COMMAND PROCESS
  serial_command.readSerial();

  stepper1.run();
  stepper2.run();
}

// STEP SERIAL COMMAND

void stepTo(){

  char *arg;
  int val;

  Serial.println("processing command...");

  arg = scom.next();

  if (arg != NULL) 
  {
    val = atoi(arg);    // Converts a char string to an integer
    Serial.print("motor #1 to: "); 
    Serial.println(val);
    m1target = val;
    stepper1.moveTo(m1target);

  } 
  else {
    Serial.println("no arguments"); 
  }

  arg = scom.next();

  if (arg != NULL) 
  {
    val = atoi(arg);    // Converts a char string to an integer
    Serial.print("motor #2 to: "); 
    Serial.println(val); 
    m2target = val;
    stepper2.moveTo(m2target);
  } 
  else {
    Serial.println("no arguments for motor #2"); 
  }
}

// DEFAULT SERIAL COMMAND

void unrecognized()
{
  Serial.println("What?"); 
}

// MOTOR STEPS

//void step1() {  
//  motor1.onestep(FORWARD, SINGLE);
//  m1++;
//}
//void backstep1() {  
//  motor1.onestep(BACKWARD, SINGLE);
//  m1--;
//}

//void step2() {  
//  motor2.onestep(FORWARD, SINGLE);
//  m2++;
//}
//void backstep2() {  
//  motor2.onestep(BACKWARD, SINGLE);
//  m2--;
//}