#define xp 2
#define xm 3
#define yp 4
#define ym 5
#define jxp 6
#define jxm 7
#define jyp 8
#define jym 9
#define pwr 12
#define enx 10
#define eny 11
int dy1 = 1000;
int spd = 1;
int acr = A0;
unsigned long mv = 0;
 unsigned long x_move=0;
 unsigned long y_move=0;
int a_v;
String cms = "";         // a String to hold incoming data
String cmm = "";
String rx="";
String ry="";
bool stringComplete = false;  // whether the string is complete
void setup() {
  pinMode(xp, OUTPUT);
  pinMode(xm, OUTPUT);
  pinMode(yp, OUTPUT);
  pinMode(ym, OUTPUT);
  pinMode(jxp, INPUT);
  pinMode(jxm, INPUT);
  pinMode(jyp, INPUT);
  pinMode(jym, INPUT);
  pinMode(pwr, OUTPUT);
  pinMode(enx, OUTPUT);
  pinMode(eny, OUTPUT);
  digitalWrite(jxp, HIGH);
  digitalWrite(jxm, HIGH);
  digitalWrite(jyp, HIGH);
  digitalWrite(jym, HIGH);   
  digitalWrite(enx, LOW);
  digitalWrite(eny, LOW);
  // initialize serial:
  Serial.begin(9600);
    digitalWrite(pwr, HIGH);
     delay(500);
      digitalWrite(pwr, LOW);
       delay(500);
        digitalWrite(pwr, HIGH);
         delay(500);
          digitalWrite(pwr, LOW);
           delay(500);
            digitalWrite(pwr, HIGH);
  // reserve 200 bytes for the cms:
  cms.reserve(200);
  Serial.println("MS_ALIGNER_TPY_L2");
}

void loop() {
  a_v = analogRead(acr);            // reads the value of the potentiometer (value between 0 and 1023)
  dy1 = map(a_v, 0, 1023, 8000, 100); 
  if(dy1<=8000){
    while(digitalRead(jxp) == LOW){
      digitalWrite(enx, HIGH);
digitalWrite(xp, HIGH);
  delayMicroseconds(dy1);
  digitalWrite(xp, LOW);
  delayMicroseconds(dy1);
    }
    while(digitalRead(jxm) == LOW){
       digitalWrite(enx, HIGH);
digitalWrite(xm, HIGH);
  delayMicroseconds(dy1);
  digitalWrite(xm, LOW);
  delayMicroseconds(dy1);
    }
    while(digitalRead(jyp) == LOW){
       digitalWrite(eny, HIGH);
digitalWrite(yp, HIGH);
  delayMicroseconds(dy1);
  digitalWrite(yp, LOW);
  delayMicroseconds(dy1);
    }
    while(digitalRead(jym) == LOW){
       digitalWrite(eny, HIGH);
digitalWrite(ym, HIGH);
  delayMicroseconds(dy1);
  digitalWrite(ym, LOW);
  delayMicroseconds(dy1);
    }
 digitalWrite(xp, LOW);
 digitalWrite(xm, LOW);
 digitalWrite(yp, LOW);
 digitalWrite(ym, LOW);
  digitalWrite(enx, LOW);
   digitalWrite(eny, LOW);
  }
 // else digitalWrite(xp, LOW);
// print the string when a newline arrives:
  if (stringComplete) {
   cms.trim();
   int ix = cms.indexOf('-');
   if (ix == -1) 
    {
      Serial.println("error");
    }
    else
    {
      cmm=cms.substring(ix+1,cms.length());
      mv=cmm.toInt();      
    if (cms.indexOf("mX-") > -1 || cms.indexOf("mx-") > -1) {
      x_move=mv;
      if (cms.indexOf("mX-") > -1) rx="mX-";
      if (cms.indexOf("mx-") > -1) rx="mx-";
     // Serial.println(x_move);

      } 
    if (cms.indexOf("mY-") > -1 || cms.indexOf("my-") > -1){
y_move=mv;
 if (cms.indexOf("mY-") > -1) ry="mY-";
      if (cms.indexOf("my-") > -1) ry="my-";
 //Serial.println(y_move);
    } 
    if ((rx.indexOf("mX-") > -1 || rx.indexOf("mx-") > -1) && (ry.indexOf("mY-") > -1 || ry.indexOf("my-") > -1)) move_stage(rx,x_move,ry,y_move); 
     //if (cms.indexOf("mx-") > -1)  move_stage(xm,mv);    
     //if (cms.indexOf("mY-") > -1)  move_stage(yp,mv);
     //if (cms.indexOf("my-") > -1)  move_stage(ym,mv);   
 
       
    cms = "";
    stringComplete = false;
    //x_move=0;
    //y_move=0;
  }
}
}
void move_stage(String x_dir,unsigned long x_mv,String y_dir,unsigned long y_mv)
{
  // if LED off
  if(x_mv>0)
  {
    digitalWrite(enx, HIGH);
     if(x_dir=="mx-"){
       for (unsigned long i=0; i<x_mv; i++)
    {
digitalWrite(xm, HIGH);
  delay(spd);
  digitalWrite(xm, LOW);
  delay(spd);
    }
     }
     if(x_dir=="mX-"){
       for (unsigned long i=0; i<x_mv; i++)
    {
digitalWrite(xp, HIGH);
  delay(spd);
  digitalWrite(xp, LOW);
  delay(spd);
    }
     }
  }
    if(y_mv>0)
  {
    digitalWrite(eny, HIGH);
     if(y_dir=="my-"){
       for (unsigned long i=0; i<y_mv; i++)
    {
digitalWrite(ym, HIGH);
  delay(spd);
  digitalWrite(ym, LOW);
  delay(spd);
    }
     }
     if(y_dir=="mY-"){
       for (unsigned long i=0; i<y_mv; i++)
    {
digitalWrite(yp, HIGH);
  delay(spd);
  digitalWrite(yp, LOW);
  delay(spd);
    }
     }
  }  
  Serial.print(x_dir + x_mv + '#' + y_dir + y_mv + '#' + "\n");
  x_mv=0;
  y_mv=0;
  cms="";
  rx="";
  ry="";
  x_move=0;
  y_move=0;
}
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the cms:
    cms += inChar;
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '#') {
      stringComplete = true;
    }
  }
}