#include <Stepper.h>

#define COMMAND_FORWARD 'w'
#define COMMAND_BACK 's'
#define COMMAND_STOP ' '

int stepsPerRevolution = 200;

int num_of_steps = 1;

Stepper myStepper1(stepsPerRevolution, 10, 11, 12, 13);
Stepper myStepper2(stepsPerRevolution, 6, 7, 8, 9);
Stepper myStepper3(stepsPerRevolution, 2, 3, 4, 5);

char lastCall = ' ';

void forwardStep(int steps){
  Serial.println("forward");
  myStepper1.step(1);
  myStepper2.step(1);
  myStepper3.step(1);
  delay(10);
}

void backwardStep(int steps){
  Serial.println("backward");
  myStepper1.step(-1);
  myStepper2.step(-1);
  myStepper3.step(-1);
  delay(10);
}

void leftStep(int steps){
  Serial.println("left");
  myStepper1.step(1);
  myStepper2.step(-1);
  myStepper3.step(-1);
  delay(10);
}

void rightStep(int steps){
  Serial.println("right");
  myStepper1.step(-1);
  myStepper2.step(1);
  myStepper3.step(-1);
  delay(10);
}



void allStop(){
  Serial.println("stop");
  PORTD = B00000000;
  PORTC = B00000000;
  PORTB = B00000000;
  delay(10);
}

void setup() {

  myStepper1.setSpeed(60);
  myStepper2.setSpeed(60);
  myStepper3.setSpeed(60);
  // initialize the serial port:
  Serial.begin(9600);
}

void loop() {
  if(Serial.available()) {
  char data =(char)Serial.read();

  switch(data) {
    case COMMAND_FORWARD:
     forwardStep(num_of_steps);
     break;
    case COMMAND_BACK:
     backwardStep(num_of_steps);
     break;
    case COMMAND_LEFT:
     leftStep(num_of_steps);
     break;
    case COMMAND_RIGHT:
     rightStep(num_of_steps);
     break; 
    case COMMAND_STOP:
     allStop();
     break;  
  }
  lastCall = data;
  }

  else{
  
  char data = lastCall;

  switch(data) {
    case COMMAND_FORWARD:
     forwardStep(num_of_steps);
     break;
    case COMMAND_BACK:
     backwardStep(num_of_steps);
     break;
    case COMMAND_LEFT:
     leftStep(num_of_steps);
     break;
    case COMMAND_RIGHT:
     rightStep(num_of_steps);
     break;
    case COMMAND_TREYWAY:
     treywayStep(num_of_steps);
     break; 
    case COMMAND_STOP:
     allStop();
     break;    
  }
  lastCall = data;
  }
}