#include <Servo.h> // Header for Servo operation
Servo myservo; // Define Object
//=========================================================================================================
//Motor Pins
int smDirectionPin1 = 55; //pin number for stepper motor 1 direction
int smStepPin1 = 54; //pin number for motor 1 steps
int smDirectionPin2 = 61; //pin number for stepper motor 2 direction
int smStepPin2 = 60; //pin number for motor 2 steps
int smEnable1 = 38; //stepper driver enable pin
int smEnable2 = 56; //stepper driver enable pin
//---------------------------------------------------------------------------------------------------------
float rotationcoeff = 2.5; // number of rotation of wheel to rotate car to 360 angle
float MotorStepPerRev = 800; //number of step to rotate wheel to 360 angle
long num1, num2, num3, num4 = 0; // variable needed
String r1_data, r2_data, r3_data; // receive data storing variable
//----------------------------------------------------------------------------------------------------------
void wheels(int stp, int spd, int wd1, int wd2)
{
  digitalWrite(smDirectionPin1, wd1);
  digitalWrite(smDirectionPin2, wd2);
  int x = 0;
  for ( x = stp; x > 0; x--)
  {
    digitalWrite(smStepPin1, HIGH);
    digitalWrite(smStepPin2, HIGH);
    delayMicroseconds(spd);
    digitalWrite(smStepPin1, LOW);
    digitalWrite(smStepPin2, LOW);
    delayMicroseconds(spd);
  }
}

void stp(float x)
{
  num2 = long(x);
  while (num1 < num2) {
    wheels(1, 1000, 1, 1);
    num1++;
  }
  while (num1 > num2) {
    wheels(1, 1000, 0, 0);
    num1--;
  }
  num1 = long(x);
}

void rev(float y)
{
  num4 = long(y);
  if (num3 < num4) {
    wheels(int(((MotorStepPerRev * rotationcoeff) / 360)*y), 1000, 1, 0);
    //num3++;
  }
  else if (num3 > num4) {
    wheels(int(((MotorStepPerRev * rotationcoeff) / 360)*y), 1000, 0, 1);
    //num3--;
  }
  num3 = long(y);
}

void zaxis(int z) {
  if ( z < 0)
  {
    myservo.write(0);
  }
  else if (z > 180)
  {
    myservo.write(180);
  }
  else
  {
    myservo.write(z);
  }
}

void setup() {
  myservo.attach(6);
  pinMode(smDirectionPin1, OUTPUT);
  pinMode(smStepPin1, OUTPUT);
  pinMode(smDirectionPin2, OUTPUT);
  pinMode(smStepPin2, OUTPUT);
  pinMode(smEnable1, OUTPUT);
  pinMode(smEnable2, OUTPUT);
  digitalWrite(smEnable1, LOW);
  digitalWrite(smEnable2, LOW);
  digitalWrite(smDirectionPin1, HIGH);
  digitalWrite(smDirectionPin2, HIGH);
  Serial.begin(9600);
}

void loop() {
  if (Serial.available() > 0)
  {
    r1_data = Serial.readString();
    if ( r1_data.toFloat() == 123.321)
    {
      Serial.println("start");
      while (!Serial.available());
      r1_data = Serial.readString();
      Serial.println("dones");
      while (!Serial.available());
      r2_data = Serial.readString();
      Serial.println("doner");
      while (!Serial.available());
      r3_data = Serial.readString();
      Serial.println("donez");
      zaxis(r3_data.toInt());
      stp(r1_data.toFloat());
      rev(r2_data.toFloat());
      Serial.println("done");
    }
  }
}