// Example sketch to control a stepper motor with A4988 stepper motor driver
// and Arduino without a library.
// More info: https://www.makerguides.com
#include "Stepper.h"
#include "ServoSet.h"
#include <Servo.h>

// Define stepper motor connections and steps per revolution:
#define dirPin 2
#define stepPin 3
#define stepsPerRevolution 200
#define servo1Pin 13
#define servo2Pin 12
#define servo3Pin 45
#define servo4Pin 44
#define gripperPin 11
ServoSet shoulder, elbow;
Servo gripper;
Stepper stepper;
bool servoPos = 0;

int lastValue = 100;
int lastRot = 100;
unsigned long lastPress;

void setup() {

  Serial.begin(115200);

  // Declare pins as output:
  stepper = Stepper(dirPin, stepPin, stepsPerRevolution);

  gripper.attach(gripperPin);
  shoulder = ServoSet(servo1Pin, servo2Pin);
  elbow = ServoSet(servo3Pin, servo4Pin);
}

void loop() {

  int speed = -1;
  double rotDegrees;
  bool dir = 1;
  Serial.println("Enter rotation speed 0-20");

  while (speed < 0) {

    if (Serial.available()) {
      String input = Serial.readString();
      speed = input.toInt();
      speed = (speed > 20) ? 20 : speed;
      if (speed < 0)
        Serial.println("Enter rotation speed 0-20");
    }
  }

  Serial.println("Enter rotation degrees (- = CCW, + = CW): ");

  while (rotDegrees == 0.00) {

    if (Serial.available()) {
      String input = Serial.readString();
      rotDegrees = input.toDouble();
      if (rotDegrees < 0) {
        dir = 0;
        rotDegrees = rotDegrees * -1.0;
      }
      else
        dir = 1;

      if (rotDegrees == 0.00)
        Serial.println("Enter rotation degrees (- = CCW, + = CW): ");

    }
  }
  if (speed != lastValue) {
    Serial.print("Speed: ");
    Serial.println(speed);
    lastValue = speed;
  }

  if (rotDegrees != lastRot) {
    Serial.print("Rotation Degrees: ");
    Serial.println(rotDegrees);
    lastRot = rotDegrees;
  }

  stepper.rotate(dir, speed, rotDegrees);

  switch (dir) {
    case 0:
      shoulder.setPosition(0);
      elbow.setPosition(90);
      break;
    case 1:
      shoulder.setPosition(180);
      elbow.setPosition(45);
  }

  servoPos = !servoPos;
  int pos = servoPos * 180;
  gripper.write(pos);


}

void compare(int val1, int val2) {
  Serial.print(val1);
  Serial.print("\t|\t");
  Serial.println(val2);
}