/* Stepper motor demonstration program written by Arthur Jones,
   4 November 2018.  Implements a simplistic and ineffective ramping 
   algorithm but provides framework for implementation of LeibRamp 
   algorithm described by Aryeh Eiderman, http://hwml.com/LeibRamp.pdf

   Makes use of background work and some aspects of code developed
   by Choaran Wang, 2017-18. This in turn incorporates some ideas
   used in the AccelStepper library:
   https://www.airspayce.com/mikem/arduino/AccelStepper/

   Serial input aspects are based closely upon:
   http://forum.arduino.cc/index.php?topic=396450
   Example 4 - Receive a number as text and convert it to an int
   Modified to read a long */

const int stepPin = 13;
const int dirPin = 9;
const bool FWDS = true;
const bool BWDS = false;

const long ticksPerSec = 1000000; // microseconds in this case


/* Define permissible parameters for motor */
// For testing by watching LED: try movements in order of 100 steps
//float accelSteps=20; /* leave this as a variable as we may over-write it */
//const float minSpeed = 2.0;
//const float maxPermissSpeed = 20.0;
//const float maxAccel = 10.0;
//const long stepLengthMus = 10000;
// For lab testing with real motor: try movements in the order of 3000 steps
 float accelSteps=1000; /* leave this as a variable as we may over-write it */
 const float minSpeed=10.0;
 const float maxPermissSpeed=100000000.0;
 const float maxAccel=500.0;
 const long stepLengthMus=100;

/* Intervals in milliseconds for user-defined timed loops */
const long printInterval = 1000;

/* Global variables used for loop timing */
unsigned long prevMillisPrint = 0;        /* stores last time values were printed */

/* Global variables used in serial input */
enum {numChars = 32};
char receivedChars[numChars];    /* an array to store the received data */
long dataNumber = 0;             /* Value read from serial monitor input */
boolean newData = false;

/* Global variables relating to stepper motor position counting etc. */
long stepsToGo;             /* Number of steps left to make in present movement */
long targetPosition;        /* Intended destination of motor for given movement */
volatile long currentPosition = 0;   /* Position in steps of motor relative to startup position */
double maxSpeed;            /* Maximum speed in present movement (not nec. max permitted) */
bool direction;             /* Direction of present movement: FWDS or BWDS */

volatile float p;   /* Step interval in clock ticks or microseconds */
float p1, ps;       /* Minimum and maximum step periods */
double deltaP;      /* You'll be able to get rid of this later */
double R;           /* Multiplying constant used in Eiderman's algorithm */

/* Global variable used for noting previous time of a step in timed loop */
long prevStepTime;

void setup()
{
    long stepsToGo = 0;
    currentPosition = 0;
    goToPosition(dataNumber);
    pinMode(stepPin, OUTPUT);
    pinMode(dirPin, OUTPUT);
    Serial.begin(9600);
    Serial.println("Enter target position in number of steps and hit return");
  
    prevStepTime = micros();
}

void loop()
{
    unsigned long currentMillis = millis();
    unsigned long currentMicros;
    recvWithEndMarker();
    stepsToGo = computeStepsToGo();
    if (convertNewNumber())
    {
        Serial.print("Converted number: datanumber is: ");
        Serial.println(dataNumber);
        // Only get to this stage if there was new data to convert
        if (stepsToGo <= 0)
        {
            // Only get to this stage if not busy, otherwise will have thrown away input
            goToPosition(dataNumber);
            Serial.print("Got target position: ");
            Serial.println(targetPosition);
      
      
            /* Define number of steps in acceleration phase using Equation (3) */
            accelSteps = long(( maxPermissSpeed * maxPermissSpeed) / ( 2.0 * (double)maxAccel)); // Equation 4 but need to consider initial speed
            stepsToGo = computeStepsToGo();
            maxSpeed = maxPermissSpeed;
            
            if (2 * accelSteps > stepsToGo)
            {
               // Define maximum speed in profile and number of steps in acceleration phase 
                maxSpeed = sqrt(minSpeed * minSpeed + stepsToGo * maxAccel); // Modified version of eq. 5
                accelSteps = (long)(stepsToGo / 2);
            }
            ps = ((double)ticksPerSec) / maxSpeed; // Eq 7
      
            p1 = (double)ticksPerSec / sqrt( minSpeed * minSpeed + 2 * maxAccel); // Eq 17 but need initial velocity
            p = p1;
            R = (double) maxAccel / ((double)ticksPerSec * (double)ticksPerSec); // Eq 19
        }
    }
  
    /* Timed loop for stepping */
    currentMicros = micros();
    if (currentMicros - prevStepTime >= p)
    {
        moveOneStep();
        prevStepTime = currentMicros;
        computeNewSpeed();
    }
  
    /* Timed loop for printing */
    if (currentMillis - prevMillisPrint >= printInterval)
    {
        // save the last time you printed output
        prevMillisPrint = currentMillis;
        printLoop();
    }
}

/* Move a single step, holding pulse high for delayMicroSeconds */
void moveOneStep()
{
    if (p != 0) /* p=0 is code for "don't make steps" */
    {
        digitalWrite(stepPin, HIGH);
        if (direction == FWDS)
        {
            /* Is something missing here? */
            digitalWrite(dirPin, HIGH);
            currentPosition++;
        }
        else
        {
            /* Is something missing here? */
            digitalWrite(dirPin, LOW);
            currentPosition--;
        }
        delayMicroseconds(stepLengthMus);
        digitalWrite(stepPin, LOW);
    }
}

/* Calcuate new value of step interval p based on constants defined in loop() */
void computeNewSpeed()
{
    double q;
    double m;
    stepsToGo = computeStepsToGo();

    /* ----------------------------------------------------------------- */
    /* Start of on-the-fly step calculation code, executed once per step */
    if (stepsToGo == 0)
    {
        p = 0; // Not actually a zero step interval, used to switch stepping off
        return;
    }
    else if (stepsToGo > accelSteps && (long)p > long(ps))  //Speeding up
    {
        m = -R;  // definition following equation 20
    }
    else if (stepsToGo <= accelSteps)   //  Slowing down
    {
       m = R;
    }
    else   //  Running at constant speed
    {
       m = 0;
    }
  
    /* Update to step interval based on Eiderman's algorithm, using temporary variables */
    q = m * p * p; // this is a part of optional enhancement
    p = p * ( 1 + q + 1.5 * q * q); // this is an enhanced approximation -equation [22]
    /* Need to ensure rounding error does not cause drift outside acceptable interval range: 
       replace p with relevant bound if it strays outside */
    if (p < ps)
    {
        p = ps;
    }
    if (p > p1)
    {
        p = p1;
    }
    /* End of on-the-fly step calculation code */
    /* ----------------------------------------------------------------- */
}

/* Work out how far the stepper motor still needs to move */
long computeStepsToGo()
{
    if (direction == FWDS)
    {
        return targetPosition - currentPosition;
    }
    else
    {
        return currentPosition - targetPosition;
    }
}

/* Set the target position and determine direction of intended movement */
void goToPosition(long newPosition)
{
    targetPosition = newPosition;
    if (targetPosition - currentPosition > 0)
    {
        direction = FWDS;
    }
    else
    {
       direction = BWDS;
    }
}

/* Receive data from serial port finishing with "newline" character.
   Based on http://forum.arduino.cc/index.php?topic=396450 Example 4 */
void recvWithEndMarker()
{
    static byte ndx = 0;
    char endMarker = '\n';
    char rc;
  
    if (Serial.available() > 0) 
    {
        rc = Serial.read();
    
        if (rc != endMarker) 
        {
            receivedChars[ndx] = rc;
            ndx++;
            if (ndx >= numChars) 
            {
               ndx = numChars - 1;
            }
        }
        else 
        {
            receivedChars[ndx] = '\0'; // terminate the string
            ndx = 0;
            newData = true;
        }
    }
}

bool convertNewNumber()
/* Converts character string to long integer only if there are new
       data to convert, otherwise returns false */
{
    if (newData) 
    {
        dataNumber = 0.0;             
        dataNumber = atol(receivedChars);   
        newData = false;
        Serial.println(dataNumber);
        return true;
    }
    else
    {
        return false;
    }
}

/* Print current position of stepper using timed loop */
void printLoop()
{
    /* Sample all counters one after the other to avoid delay-related offsets */
    Serial.print("Current position = ");
    Serial.print(currentPosition);
    Serial.print("\r\n");
    Serial.print("p = ");
    Serial.print(p);
    Serial.print("\r\n");
}