// purple is step wire
// white is direction wire

#define xStepPin 2
#define xDirPin 3
#define yStepPin 4
#define yDirPin 5
#define zStepPin 6
#define zDirPin 7
#define limitSenseA 32
#define limitSenseB 34

#define zNegBut 52
#define zPosBut 50
#define yNegBut 48
#define yPosBut 46
#define xNegBut 44
#define xPosBut 42

#define holdBut 40
#define eStop 38
#define emptyPin 36

#define relayPin1 8
#define relayPin2 9
#define relayPin3 10
#define relayPin4 11

void setup() {
  pinMode(xStepPin, OUTPUT);
  pinMode(xDirPin, OUTPUT);
  pinMode(yStepPin, OUTPUT);
  pinMode(yDirPin, OUTPUT);
  pinMode(zStepPin, OUTPUT);
  pinMode(zDirPin, OUTPUT);

  pinMode(limitSenseA, INPUT);
  pinMode(limitSenseB, OUTPUT);

  pinMode(zNegBut, INPUT_PULLUP);
  pinMode(zPosBut, INPUT_PULLUP);
  pinMode(yNegBut, INPUT_PULLUP);
  pinMode(yPosBut, INPUT_PULLUP);
  pinMode(xNegBut, INPUT_PULLUP);
  pinMode(xPosBut, INPUT_PULLUP);

  pinMode(holdBut, INPUT_PULLUP);
  pinMode(eStop, INPUT_PULLUP);

  pinMode(relayPin1, OUTPUT);
  pinMode(relayPin2, OUTPUT);
  pinMode(relayPin3, OUTPUT);
  pinMode(relayPin4, OUTPUT);

  Serial.begin(9600);

  resetRelays();
}

void loop() {
  //reportButtons();
  digitalWrite(relayPin1, HIGH);
  delay(1000);
  digitalWrite(relayPin1, LOW);
  delay(1000);
}

void resetRelays() {
  digitalWrite(relayPin1, HIGH);
  digitalWrite(relayPin2, HIGH);
  digitalWrite(relayPin3, HIGH);
  digitalWrite(relayPin4, HIGH);
}

int reportButtons() {
  String out = "";
  out = out + "Z-:" + digitalRead(zNegBut) + ", ";
  out = out + "Z+:" + digitalRead(zPosBut) + ", ";
  out = out + "Y-:" + digitalRead(yNegBut) + ", ";
  out = out + "Y+:" + digitalRead(yPosBut) + ", ";
  out = out + "X-:" + digitalRead(xNegBut) + ", ";
  out = out + "X+:" + digitalRead(xPosBut) + ", ";
  out = out + "H:" + digitalRead(holdBut) + ", ";
  out = out + "E:" + digitalRead(eStop);
  return Serial.println(out);
}

void stepTest() {
  digitalWrite(xDirPin, HIGH);

  for (int x = 0; x < 200; x++) {
    digitalWrite(xStepPin, HIGH);
    delayMicroseconds(500);
    digitalWrite(xStepPin, LOW);
    delayMicroseconds(500);
  }
  delay(250);

  digitalWrite(xDirPin, LOW);

  for (int x = 0; x < 200; x++) {
    digitalWrite(xStepPin, HIGH);
    delayMicroseconds(500);
    digitalWrite(xStepPin, LOW);
    delayMicroseconds(500);
  }
  delay(1000);

  digitalWrite(yDirPin, HIGH);

  for (int x = 0; x < 200; x++) {
    digitalWrite(yStepPin, HIGH);
    delayMicroseconds(500);
    digitalWrite(yStepPin, LOW);
    delayMicroseconds(500);
  }
  delay(250);

  digitalWrite(yDirPin, LOW);

  for (int x = 0; x < 200; x++) {
    digitalWrite(yStepPin, HIGH);
    delayMicroseconds(500);
    digitalWrite(yStepPin, LOW);
    delayMicroseconds(500);
  }
  delay(1000);

  digitalWrite(zDirPin, HIGH);

  for (int x = 0; x < 200; x++) {
    digitalWrite(zStepPin, HIGH);
    delayMicroseconds(500);
    digitalWrite(zStepPin, LOW);
    delayMicroseconds(500);
  }
  delay(250);

  digitalWrite(zDirPin, LOW);

  for (int x = 0; x < 200; x++) {
    digitalWrite(zStepPin, HIGH);
    delayMicroseconds(500);
    digitalWrite(zStepPin, LOW);
    delayMicroseconds(500);
  }
  delay(1000);
}

void jog() {
  if (digitalRead(xPosBut)) {
    xMotor(true, 2);
  }

  if (digitalRead(xNegBut)) {
    xMotor(false, 2);
  }

  if (digitalRead(yPosBut)) {
    yMotor(true, 2);
  }

  if (digitalRead(yNegBut)) {
    yMotor(false, 2);
  }

  if (digitalRead(zPosBut)) {
    zMotor(true, 2);
  }

  if (digitalRead(zNegBut)) {
    zMotor(false, 2);
  }
}

void xMotor(bool dir, int dist) {
  if (dir) {
    digitalWrite(xDirPin, HIGH);
  } else {
    digitalWrite(xDirPin, LOW);
  }

  for (int x = 0; x < dist; x++) {
    digitalWrite(xStepPin, HIGH);
    delayMicroseconds(500);
    digitalWrite(xStepPin, LOW);
    delayMicroseconds(500);
  }
}

void yMotor(bool dir, int dist) {
  if (dir) {
    digitalWrite(yDirPin, HIGH);
  } else {
    digitalWrite(yDirPin, LOW);
  }

  for (int x = 0; x < dist; x++) {
    digitalWrite(yStepPin, HIGH);
    delayMicroseconds(500);
    digitalWrite(yStepPin, LOW);
    delayMicroseconds(500);
  }
}

void zMotor(bool dir, int dist) {
  if (dir) {
    digitalWrite(zDirPin, HIGH);
  } else {
    digitalWrite(zDirPin, LOW);
  }

  for (int x = 0; x < dist; x++) {
    digitalWrite(zStepPin, HIGH);
    delayMicroseconds(500);
    digitalWrite(zStepPin, LOW);
    delayMicroseconds(500);
  }
}