#include <AccelStepper.h>


#include <Wire.h>
#include <SoftwareSerial.h>
#define dirPin 4
#define stepPin 3
#define motorInterfaceType 1

AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);



//const float stepsperul = 230; //resolution of 1ml disposable syringes
//const float stepsperul = 131.53; //resolution of 1ml disposable syringes
//const float stepsperul = 272.12; //resolution of 250ul glass syringe


int enablePin = 5;
int estepsPin = 3;
int directionPin = 4;
int elimitPin = A1; 
//int esteps = 1000;
//int esteprate = 500;
//int estepsincrement = 0;
int estepcount = 0;

//float stepsperul = 23; //resolution of 1ml disposable syringes
float stepsperul = 131.43;

int acc = 30;
String command;
long int currpos;
long int pos;

void setup() {
  // put your setup code here, to run once:
  stepper.setMaxSpeed(1000*16);
  Serial.begin(115200);
  pinMode(enablePin, OUTPUT);
  pinMode(directionPin, OUTPUT);
  pinMode(estepsPin, OUTPUT);
  pinMode(elimitPin, INPUT);
  digitalWrite(elimitPin, HIGH); 
  digitalWrite(enablePin,LOW);
  currpos = 0;
  stepper.setAcceleration(acc);
}

void loop() {
  // put your main code here, to run repeatedly:
 if(Serial.available())
 {
    char c = Serial.read();
    if (c== '\n')
    {
      currpos = parseCommand(command, currpos);
      command = "";
    }
    else 
    {
      command +=c;
    }
 }
 delay(30);
}

long int parseCommand(String com, long int currpos)
{
  //Serial.print("Your command: ");
  //Serial.println(com);

  //"G1E10F200";
  String part1 = com.substring(0,com.indexOf("e"));
  if (part1.equalsIgnoreCase("g1")){
    String part2 = com.substring(com.indexOf("e")+1,com.indexOf("s"));
    String part3 = com.substring(com.indexOf("s")+1,com.indexOf("a"));
    String part4 = com.substring(com.indexOf("a")+1);
    int vpos = part2.toInt();
    pos = vpos * stepsperul; 
    int vfeed = part3.toInt();
    int feed = vfeed * stepsperul;
    //int acc = part4.toInt();
    //int feed = vfeed;
    int acc = feed/3;
    if ((pos-currpos) < 0) {
      feed = feed * -1;
    }
    moveacc((pos-currpos), feed, acc);
    currpos = pos;
  }
  else if(com.equalsIgnoreCase("m114")){
   Serial.print("Step Position: ");
   Serial.print(pos);
   Serial.print(" volume Position: ");
   Serial.println((pos/stepsperul),3);
  } 
  else if(com.equalsIgnoreCase("g28e0")){
   currpos = homing(estepsPin,directionPin,elimitPin);
   currpos = 0;
   pos = 0;
  } 
  else if(com.equalsIgnoreCase("readinput")){
   Serial.print("Input ");
   Serial.println(digitalRead(elimitPin));
  } 
  
  else if(com.equalsIgnoreCase("info")){
    Serial.println("multistepper");
  }
  else {
   Serial.print("Did not recognize ");
   Serial.println(com);
  }
   return currpos;
}


void moveacc(long int inc, int spd, int acc){
 //speed is in steps per second
 stepper.setCurrentPosition(0);
 stepper.setAcceleration(acc);
 while(stepper.currentPosition() != inc){
    stepper.setSpeed(spd);
    stepper.runSpeed();
 }
}



int homing(int stepsPin, int directionPin, int limitPin){
  digitalWrite(directionPin,LOW); // Set Dir high
  int checker = 1;
  int cnter = 0;
  int stpper = 1;
   while(stpper > 0){
    checker = digitalRead(limitPin);
    if (checker == HIGH){
      cnter = cnter + 1;
    }
    else {
      cnter = 0;
    }
    if (cnter > 4){
      stpper = 0;
    }
    digitalWrite(stepsPin,HIGH); // Output high
    delayMicroseconds(200); // Wait 1/2 a ms
    digitalWrite(stepsPin,LOW); // Output low
    delayMicroseconds(200); // Wait 1/2 a ms
   }
   digitalWrite(directionPin,HIGH); // Set Dir high
   for(int x = 0; x < 18; x++){ // Loop 200 times
    digitalWrite(stepsPin,HIGH); // Output high
    delayMicroseconds(1000); // Wait 1/2 a ms
    digitalWrite(stepsPin,LOW); // Output low
    delayMicroseconds(1000); // Wait 1/2 a ms
   }
    int stepcount = 0;
    return stepcount;
}