#define dirPin_Mx 2 // right motor
#define stepPin_Mx 3 // right motor
#define limitBut_Mx 4
#define dirPin_My 7 // left motor
#define stepPin_My 8 // left motor
#define limitBut_My 9
#define homeBut 12
#define horzPin A1 // left thumbstick
#define vertPin A2 // right thumbstick

 


int xPosCur = 100; // current number of x steps from origin
int yPosCur = 100; // current number of y steps from origin
int xPosTarg = 100; // target number of x steps from origin
int yPosTarg = 100; // target number of y steps from origin
int xLimit = 200; // farthest possible no. of x steps from origin
int yLimit = 200; // farthest possible no. of y steps form origin
unsigned long homePress = 0;
unsigned long limitBut_MxPress = 0;
unsigned long limitBut_MyPress = 0;
int debHomeBut = 50;
int deblimit_But_My = 50;
int deblimit_But_Mx = 50;



int speed_Mx = 3000;
int speed_My = 3000;
int speedHome = 6000;
bool dir_Mx;
bool dir_My;


void calcDir () {
  if ((xPosCur - xPosTarg) < 1) {
    digitalWrite (dirPin_Mx, LOW);
  }
  if ((xPosCur - xPosTarg) >= 1) {
    digitalWrite (dirPin_Mx, HIGH);
  }
  if ((yPosCur - yPosTarg) < 1) {
    digitalWrite (dirPin_My, LOW);
  }
  if ((yPosCur - yPosTarg) >= 1) {
    digitalWrite (dirPin_My, HIGH);
  }
}

void stepMove_Mx () {    
  digitalWrite(stepPin_Mx, HIGH);
  delayMicroseconds(speed_Mx);
  digitalWrite(stepPin_Mx, LOW);
  delayMicroseconds(speed_Mx);
  if ((xPosCur - xPosTarg) < 1) {
  xPosCur ++;
  }
  if ((xPosCur - xPosTarg) >= 1) {
  xPosCur --;
  } 
}

void stepMove_My () {    
  digitalWrite(stepPin_My, HIGH);
  delayMicroseconds(speed_My);
  digitalWrite(stepPin_My, LOW);
  delayMicroseconds(speed_My);
  if ((yPosCur - yPosTarg) < 1) {
  yPosCur ++;
  }
  if ((yPosCur - yPosTarg) >= 1) {
  yPosCur --;
  } 
}

void homeX() {
  for(int i = 0; (digitalRead(limitBut_Mx) == HIGH) 
  && (millis() - limitBut_MxPress > deblimit_But_Mx); i++) {
    digitalWrite (dirPin_Mx, LOW);
    digitalWrite(stepPin_Mx, HIGH);
    delayMicroseconds(speedHome);
    digitalWrite(stepPin_Mx, LOW);
    delayMicroseconds(speedHome);
  }

  for(int i = 0; i < (xLimit / 2); i++) {
    digitalWrite (dirPin_Mx, HIGH);
    digitalWrite(stepPin_Mx, HIGH);
    delayMicroseconds(speedHome);
    digitalWrite(stepPin_Mx, LOW);
    delayMicroseconds(speedHome);
  }
}


void homeY() {
  for(int i = 0; (digitalRead(limitBut_My) == HIGH) 
  && (millis() - limitBut_MyPress > deblimit_But_My); i++) {
    digitalWrite (dirPin_My, LOW);
    digitalWrite(stepPin_My, HIGH);
    delayMicroseconds(7000);
    digitalWrite(stepPin_My, LOW);
    delayMicroseconds(7000);
  }

  for(int i = 0; i < (yLimit / 2); i++) {
    digitalWrite (dirPin_My, HIGH);
    digitalWrite(stepPin_My, HIGH);
    delayMicroseconds(7000);
    digitalWrite(stepPin_My, LOW);
    delayMicroseconds(7000);
  }
}

void setup() {
  pinMode(stepPin_Mx, OUTPUT);
  pinMode(dirPin_Mx, OUTPUT);
  pinMode(limitBut_Mx, INPUT_PULLUP);
  pinMode(stepPin_My, OUTPUT);
  pinMode(dirPin_My, OUTPUT);
  pinMode(limitBut_My, INPUT_PULLUP);
  pinMode(homeBut, INPUT_PULLUP);
  pinMode(horzPin, INPUT);
  pinMode(vertPin, INPUT);

  //Serial.begin(9600);

}

void loop() {
  
  /* Serial.print(analogRead(horzPin));
  Serial.print("\t");
  Serial.print(analogRead(vertPin));
  Serial.print("\t");
  Serial.print(digitalRead(dirPin_Mx));
  Serial.print("\t");
  Serial.print(digitalRead(dirPin_My));
  Serial.print("\t"); */

  
  xPosTarg = map(analogRead(horzPin),0,1023,0,xLimit);
  yPosTarg = map(analogRead(vertPin),0,1023,0,yLimit);

  /*Serial.print(xPosCur);
  Serial.print("\t");
  Serial.print(xPosTarg); 
  Serial.print("\t");
  Serial.print(yPosCur);
  Serial.print("\t");
  Serial.print(yPosTarg);
  Serial.print("\t");
  Serial.println();*/


  if(xPosTarg != xPosCur) {
    calcDir ();
    stepMove_Mx ();
  }

  if(yPosTarg != yPosCur) {
    calcDir ();
    stepMove_My ();
  }

  if((digitalRead(homeBut) == LOW) && (millis() - homePress > debHomeBut)) {
    homePress = millis();
    homeX();
    homeY();
  }

}