#include <AccelStepper.h>
#include <MultiStepper.h>
//help: https://www.airspayce.com/mikem/arduino/AccelStepper/MultiStepper_8pde-example.html

#define dirPin 2
#define stepPin 3
#define motorInterfaceType 1

int ledX=13;
int ledY=12;
int xmm_obrat=15;
int ymm_obrat=15;
int motorSpeed = 100; 
int motorAccel = 10;
String cmd="";   // komanda iz serijskega porta
long cnt=0;

// Create a new instance of the AccelStepper class:
AccelStepper sX = AccelStepper(motorInterfaceType, stepPin, dirPin);
AccelStepper sY = AccelStepper(motorInterfaceType, 5, 4);
MultiStepper steppers;
long positions[2]; // Array of desired stepper positions
void setup()
{
  pinMode(ledX, OUTPUT);
  pinMode(ledY, OUTPUT);
  Serial.begin(9600);
  Serial.setTimeout(200);
  sX.setMaxSpeed(motorSpeed);
  sX.setSpeed(motorSpeed);
  sX.setAcceleration(motorAccel);

  sY.setMaxSpeed(motorSpeed);
  sY.setSpeed(motorSpeed);
  sY.setAcceleration(motorAccel);
  sY.moveTo(2);

  steppers.addStepper(sX);
  steppers.addStepper(sY);

  positions[0] = 100;
  positions[1] = 50;
  steppers.moveTo(positions);
  steppers.runSpeedToPosition(); // Blocks until all are in position
  
  // Move to a different coordinate
  positions[0] = 0;
  positions[1] = 0;
  steppers.moveTo(positions);
  steppers.runSpeedToPosition(); // Blocks until all are in position
  delay(1000);
  Serial.println("vnos komande za pozicijo x,y npr: x035y210");
  delay(2000);
}

void loop()
{
  //komunikacija s serijskim portom
  if (Serial.available() > 0) 
  {
    cmd = Serial.readString();
    cmd.replace("\n","");
    //3 znaki za X in 3 znaki za Y
    //vnos komande npr: x035y210
    String tmp=cmd.substring(1,4);
    int stpX=tmp.toInt();
    int mmX = map(stpX,0,xmm_obrat,0,200)+1;
    String tmp1=cmd.substring(5,8);
    int stpY=tmp1.toInt();
    int mmY = map(stpY,0,ymm_obrat,0,200)+1;
    Serial.println("x:"+String(mmX)+"  y:"+String(mmY));
    delay(3000);
    if (cmd=="s") //vrni se v začetno pozicijo
    {
      positions[0] = 0;
      positions[1] = 0;
      steppers.moveTo(positions);
    }
    //v primeru, da številke ne more pretvoriti, v mm zapiše 0. torej če je komanda x0 ali y0, pojdi v 0
    if(mmX!=0 && mmY!=0)
    {
      positions[0] = mmX;
      positions[1] = mmY;
      steppers.moveTo(positions);
    }
  }
  
  //se mora izvajati, da steper izvaja premikanje
  
  //delay(300);
  //detekcija, ko steper doseže končni položaj
  bool xposReach=!sX.isRunning();
  digitalWrite(ledX,xposReach);
  bool yposReach=!sY.isRunning();
  digitalWrite(ledY,yposReach);
  steppers.runSpeedToPosition(); // Blocks until all are in position
  //vsak 100 klic izpišemo pozicijo, da ne preobremenimo procesorja z izpisovanjem
  if(cnt%1000)
  {
    int currX=sX.currentPosition();
    int currY=sY.currentPosition();
    //pretvorba iz stepov v stopinje
    //int mm = map(value, fromLow, fromHigh, toLow, toHigh)
    int mmX=map(currX,0,200,0,xmm_obrat);
    int mmY=map(currY,0,200,0,ymm_obrat);
    Serial.println("X:"+String(mmX)+"mm  -  Y:"+String(mmY)+"mm");
  }
  cnt++;
  if(cnt>100000)
  {
    cnt=0;
  }
}