/*
   Project:       Stepper Angle Control
   Description:   Moves a stepper motor to the angle entered into
                  Serial via the shortest path.
                  Wokwi allows 1/8 step but only simulates 1/2 step.
                  This is wired for 1/8 step.
                  https://docs.wokwi.com/parts/wokwi-a4988
   Creation date: 2/24/24
   Author:        AnonEngineering
   Inspired by:   Cringy Mikolo, Discord | Arduino | coding-help, 2/13/24 at 7:31 PM
   License:       https://en.wikipedia.org/wiki/Beerware
*/

#include <AccelStepper.h>

#define motorInterfaceType 1

const int DIR_PIN = 8;
const int STP_PIN = 9;
const int SPEED = 200;
const int ACCEL =  50;
const int STEPS_REVOLUTION = 1600;  // change to match
const byte NUM_CHARS = 8;

boolean newData = false;
char rxChars[NUM_CHARS];

AccelStepper stepper(motorInterfaceType, STP_PIN, DIR_PIN);

int calcNewPosition(int targetAngle) {
  static int endAngle = 0;
  int startAngle = endAngle;
  //Serial.print("Curr Position: ");
  //Serial.println(stepper.currentPosition());
  Serial.print("Start angle : ");
  Serial.println(startAngle);
  Serial.print("Target angle: ");
  Serial.println(targetAngle);
  int deltaTheta = calculateShortestArc(startAngle, targetAngle);
  Serial.print("Degree difference: ");
  Serial.println(deltaTheta);
  long deltaSteps = (int) (round(deltaTheta * (STEPS_REVOLUTION / 360.0)));
  Serial.print("Steps difference : ");
  Serial.println(deltaSteps);
  Serial.println();
  endAngle = targetAngle;
  return deltaSteps;
}

int calculateShortestArc(int theta_A, int theta_B) {
  // Calculate angular distance in both directions
  int ccwDistance = ((theta_B - theta_A + 360) % 360);
  int cwDistance = ((theta_A - theta_B + 360) % 360);
  // Determine direction (CW or CCW) based on the shorter distance
  int direction = (ccwDistance < cwDistance) ? 1 : -1;
  // Calculate shortest arc
  int shortestArc = direction * min(ccwDistance, cwDistance);

  return shortestArc;
}

void checkNewData() {
  static byte index = 0;
  char rxChar;
  char endMarker = '\n';
  while (Serial.available() > 0 && newData == false) {
    rxChar = Serial.read();
    if (rxChar != endMarker) {
      rxChars[index] = rxChar;
      index++;
      if (index >= NUM_CHARS) {
        index = NUM_CHARS - 1;
      }
    } else {
      rxChars[index] = '\0'; // terminate the string
      index = 0;
      newData = true;
    }
  }
}

void setup() {
  Serial.begin(9600);
  stepper.setSpeed(SPEED);
  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(ACCEL);
  // needed to start at 0!?!
  //stepper.moveTo(16);
  Serial.println("Enter target angle in degrees:\n");
}

void loop() {
  checkNewData();
  if (newData == true)  {
    long stepsDiff = calcNewPosition(atoi(rxChars));
    stepper.move(stepsDiff);
    newData = false;
  }
  stepper.runToPosition();  // blocking
}