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

//#-500#300#16#855#0#$
// Define some steppers and the pins the will use
AccelStepper stepperArray[] = {{1,12,13}, {1,10,11}, {1,46,9}, {1,8,3}, {1,17,18}, {1,15,16}};//int pul=12, dir=13, en=14
//AccelStepper stepperArray[] = {{1,36,35}, {1,48,47}};//{{1,Pins[1][1],Pins[1][2]}, {1,Pins[2][1],Pins[2][2]}};
const int NumberOfSteppers = sizeof(stepperArray) / sizeof(stepperArray[0]);
//AccelStepper stepper1(1,12,13);
MultiStepper steppers;

float tim=0;
int Stage = 0;

int ActDerajat[NumberOfSteppers];
float konversi[] = {1, 1, 1, 1, 1, 1};//{3.125, 3.125, 0.1953125, 3.125, 0, 0};
long Target[NumberOfSteppers];
long Target1[NumberOfSteppers];
long Dev[NumberOfSteppers];
int RangeTarget = -1;
float MaxSpeed = 5000;
float Accel = 500;
float Speed = 1400;
float speed1=0;
int Min1= 20;
int Max1 = 500;
int StepperLead = 0;
int MaxStep = 0;

bool parsing = false;
String sData; 
String data[NumberOfSteppers+1];


float speed2 = 0;

void setup()

{
  Serial.begin(9600);
  for (int x = 0; x < NumberOfSteppers; x++)  {
    stepperArray[x].setMaxSpeed(MaxSpeed);
    stepperArray[x].setAcceleration(Accel);

    steppers.addStepper(stepperArray[x]);
}
  tim=millis();
  Stage = 1;

}



void loop()
{

 if (millis()-tim>=50){ActPos();}

//GET COMMAND============================================
while (Serial.available()) {
char inChar = Serial.read();
sData += inChar;
if (inChar == '$') {
  parsing = true;
}
if (parsing) {
int q = 0;
for (int i = 0; i < sData.length(); i++) {
  if (sData[i] == '#') {
    q++;
    data[q] = "";
    } else {
    data[q] += sData[i];
  }
}
//Rubah String to Target
for (int t = 0; t < NumberOfSteppers; t++) {
Target[t] = data[t+1].toInt();
//Finish[t]=0;
}
MasterMotor();
}
}



if (Stage==1 && (Dev[StepperLead]>=abs(RangeTarget)) || (Dev[StepperLead]<=RangeTarget)){
    if (abs(stepperArray[StepperLead].distanceToGo())>=abs(Target1[StepperLead]*0.4)){
        speed1=map(abs(stepperArray[StepperLead].distanceToGo()), abs(Target1[StepperLead]), abs(Target1[StepperLead]*0.8), Min1, Max1);
        if (speed1>Max1){
        speed1=Max1;
        }
        stepperArray[StepperLead].setMaxSpeed(speed1);
    }
    else {
        stepperArray[StepperLead].setMaxSpeed(map(abs(stepperArray[StepperLead].distanceToGo()), abs(Target1[StepperLead])*0.4, 0, speed1, Min1));
    }
    steppers.moveTo(Target1);
    steppers.run();
}
else {
    Stage = 2;
}



if (Stage == 2){
for (int x = 0; x < NumberOfSteppers; x++)  {
    if ((Dev[x]>=abs(RangeTarget)) || (Dev[x]<=abs(RangeTarget))) {
      stepperArray[x].moveTo(Target1[x]);
      stepperArray[x].run();
}}
}
/*
    //steppers.moveTo(Target);
    //steppers.runSpeedToPosition();
    //steppers.run();


stepper1.run();
//stepperArray[1].run();
*/
}







//CHEK AKTUAL POSISI DERAJAT===============================================
void ActPos() {
for (int x = 0; x < NumberOfSteppers; x++)  {
ActDerajat[x] = stepperArray[x].currentPosition();
Dev[x] = Target[x] - ActDerajat[x];
}
Serial.print("#");
Serial.print(stepperArray[0].speed ());
Serial.print("#");
Serial.print(stepperArray[1].speed ());
Serial.print("#");
Serial.print(stepperArray[2].speed ());
Serial.print("#");
Serial.print(stepperArray[3].speed ());
Serial.print("#");
Serial.print(stepperArray[4].speed ());
Serial.print("#");
Serial.print(stepperArray[5].speed ());
Serial.print("#");
Serial.println(StepperLead);
}



//MENENTUKAN MASTER MOTOR==================================================
void MasterMotor(){
  MaxStep=0;
ActPos();
for (int x = 0; x < NumberOfSteppers; x++)  {
    if (Dev[x]!=0 && abs(Dev[x])<MaxStep) {
MaxStep = abs(Dev[x]);
StepperLead = x;
    }
  }
//MENENTUKAN TARGET MOTOR========================================
for (int x = 0; x < NumberOfSteppers; x++)  {
  if ((Dev[x]<=abs(RangeTarget)) && (Dev[x]>=RangeTarget)){
  Target1[x]= stepperArray[x].currentPosition ();
  }
  else {
  Target1[x]=Dev[x]*konversi[x];
  }
  }
  Stage = 1;
}