#include <SoftwareSerial.h>
#include <AccelStepper.h>
#include <MultiStepper.h>

#define MAX_SPEED 500
#define HOME_SPEED 100

#define MULTIPLIER 250

AccelStepper motor1(1, 11, 12);
AccelStepper motor2(1, 9, 10);
AccelStepper motor3(1, 7, 8);
AccelStepper motor4(1, 5, 6);

MultiStepper motors;

SoftwareSerial Serial1(3,2);

const int limit[4] = {A2, A3, A4, A5};

long positions[4] = {0, 0, 0, 0};
char serial_read[4] = {'0', '0', '0', '0'};

bool flag = false;

void setup() {
  Serial1.begin(115200);
  delay(1000);

  motor1.setMaxSpeed(MAX_SPEED);
  motor2.setMaxSpeed(MAX_SPEED);
  motor3.setMaxSpeed(MAX_SPEED);
  motor4.setMaxSpeed(MAX_SPEED);

  for (int i = 0; i < 4; i++) {
    pinMode(limit[i], INPUT_PULLUP);
  }

  motors.addStepper(motor1);
  motors.addStepper(motor2);
  motors.addStepper(motor3);
  motors.addStepper(motor4);

}

void loop() {
  String data;
  if (Serial1.available()) {
    if (Serial1.read() == '*') {

      data = Serial1.readStringUntil('!');

      int hash = data.indexOf('#');

      //read for number
      String number = data.substring(0, hash);
      int num = number.toInt();

      String stream = data.substring(hash + 1);

      num -= 4;

      if (num) {
        String serialTx = "*" + String(num) + "#";
        serialTx += stream.substring(0, num);
        serialTx += "!";
        Serial1.println(serialTx);

      }

      String serialRead = stream.substring(num);
      serialRead.toCharArray(serial_read, 5);

      flag = true;

    }
  }


  if (flag) {
    setPositions();
    runMotors();
    flag = false;
  }
}

void setPositions() {

  for (int i = 0; i < 4; i++) {
    switch (serial_read[i]) {
      case '0': positions[i] = positions[i];
        break;
      case '1':
      case '2':
      case '3':
      case '4':
      case '5':
      case '6':
      case '7':
      case '8':
      case '9': positions[i] = (serial_read[i] - '0') * MULTIPLIER;
        break;
      case 'r': positions[i] = 0;
        break;
      case 'h': homeDrop(i);
        break;
    }
  }
}

void runMotors() {
  motors.moveTo(positions);
  motors.runSpeedToPosition(); // Blocks until all are in position
}

void homeDrop(int i) {
  int limit_switch = limit[i];

  switch (i) {
    case 0: motor1.setSpeed(-HOME_SPEED);
      while (digitalRead(limit_switch) == 1) {
        motor1.runSpeed();
      }
      motor1.setCurrentPosition(-20);

      break;
    case 1: motor2.setSpeed(-HOME_SPEED);
      while (digitalRead(limit_switch) == 1) {
        motor2.runSpeed();
      }
      motor2.setCurrentPosition(-20);
      break;
    case 2: motor3.setSpeed(-HOME_SPEED);
      while (digitalRead(limit_switch) == 1) {
        motor3.runSpeed();
      }
      motor3.setCurrentPosition(-20);
      break;
    case 3: motor4.setSpeed(-HOME_SPEED);
      while (digitalRead(limit_switch) == 1) {
        motor4.runSpeed();
      }
      motor4.setCurrentPosition(-20);
      break;
  }
  positions[i] = 20;
}