#include <LiquidCrystal_I2C.h>
#include <ErriezSerialTerminal.h>
char newlineChar = '\n';
// Separator character between commands and arguments
char delimiterChar = '_';

// Create serial terminal object
SerialTerminal term(newlineChar, delimiterChar);
LiquidCrystal_I2C lcd(0x27, 20, 4);  

int enablePins[4] = {22,23,24,25};
int directionPins[4] = {26,27,28,29};
int stepperPins[4] = {30,31,32,33};

#define swClk 2
#define swDir 3
#define swBtn 4

int xSteps;
int ySteps;
int zSteps;
int eSteps;

void updateLcd()
{
  char tmp[20*4];
  lcd.clear();
  lcd.setCursor(0, 0);
  sprintf(tmp, "X:%d,Y:%d,Z:%d,E:%d",xSteps,ySteps,zSteps,eSteps);
  lcd.print(tmp);
}

void testStepper(int stepper, int direction, int steps, int stepDelay)
{
  digitalWrite(directionPins[stepper], direction);
  int infoMathType=0;
  if(direction==0)
  {
    infoMathType=1;
  }
  else {
    infoMathType=-1;
  }
  for(int step=0; step<steps; step++)
  {
    switch(stepper)
    {
      case 0:
        xSteps+=infoMathType;
        break;
      case 1:
        ySteps+=infoMathType;
        break;
      case 2:
        zSteps+=infoMathType;
        break;
      case 3:
        eSteps+=infoMathType;
        break;
    }
    digitalWrite(enablePins[stepper], HIGH);
    digitalWrite(stepperPins[stepper], HIGH);
    delayMicroseconds(stepDelay/2);
    digitalWrite(stepperPins[stepper], LOW);
    delayMicroseconds(stepDelay/2);
    digitalWrite(enablePins[stepper], LOW);
  }
}
void message()
{
  char *arg;
  int x,y;

  arg = term.getNext();
  x=atoi(arg);
  arg = term.getNext();
  y=atoi(arg);
  arg = term.getNext();
  lcd.setCursor(x,y);
  lcd.print(arg);
}
void moveMotor()
{
  char *arg;
  int mot = 0;
  int dir = 0;
  int steps = 0;
  int del = 0;
  // Get first argument
  arg = term.getNext();
  mot=atoi(arg);

  arg = term.getNext();
  dir=atoi(arg);

  arg = term.getNext();
  steps=atoi(arg);

  arg = term.getNext();
  del=atoi(arg);

  testStepper(mot, dir, steps, del);
  char tmp[1024];
  sprintf(tmp, "M%d_D%d_S%d_W%d", mot, dir, steps, del);
  Serial.println(tmp);
}

void setup() {
  lcd.init();
  Serial.begin(115200);
  Serial.println(F("COMMAND_LINE:"));
  lcd.backlight();
  lcd.setCursor(4, 0);
  lcd.print("Hello World!");
  term.addCommand("step", moveMotor);
  term.addCommand("msg", message);
  pinMode(swClk, INPUT);
  pinMode(swDir, INPUT);
  pinMode(swBtn, INPUT);
  
  // put your setup code here, to run once:
  for(int pin=0; pin<4; pin++)
  {
    pinMode(enablePins[pin], OUTPUT);
  }
  for(int pin=0; pin<4; pin++)
  {
    pinMode(directionPins[pin], OUTPUT);
  }
  for(int pin=0; pin<4; pin++)
  {
    pinMode(stepperPins[pin], OUTPUT);
  }
  //testStepper(0, 1, 100, 10);
}

void loop() {
  // put your main code here, to run repeatedly:
  if(digitalRead(swClk) == LOW)
  {
    testStepper(0, digitalRead(swDir), 1, 1);
    updateLcd();
  }
  term.readSerial();
}