#include <LiquidCrystal_I2C.h>
#include <ContinuousStepper.h>

ContinuousStepper<StepperDriver> stepper;

//initialize lcd
LiquidCrystal_I2C lcd(0x27,16,2);

byte dirPin = 12;
byte stepPin = 13;
byte startstopPin = 17;
byte changedataPin = 15;
byte resetPin = 19;
byte onoffPin = 21;
byte speedPin = A5;
byte driverReset = 11;

//LED's
byte startstopLED = 18;
byte resetLED = 20;
byte dataLED = 16;

byte shutdownPin = 4;
byte RHSdirPin = 5;
byte LHSdirPin = 6;

//States
byte shutdownState = LOW;
byte startstopState = LOW;
byte RHSdirState = LOW;
byte LHSdirState = LOW;
byte resetState = LOW;

byte moveRight = true;

int speedControl = 0;

//Data
int limits = -1;
int distance = 0;
int speed = 0;

void setup() {
pinMode(dirPin, OUTPUT);
pinMode(stepPin, OUTPUT);
pinMode(startstopPin, INPUT_PULLUP);
pinMode(changedataPin, INPUT_PULLUP);
pinMode(resetPin, INPUT_PULLUP);
pinMode(onoffPin, INPUT);
pinMode(speedPin, INPUT);
pinMode(driverReset, OUTPUT);

//LED's
pinMode(startstopLED, OUTPUT);
pinMode(resetLED, OUTPUT);
pinMode(dataLED, OUTPUT);

pinMode(shutdownPin, INPUT_PULLUP);
pinMode(RHSdirPin, INPUT_PULLUP);
pinMode(LHSdirPin, INPUT_PULLUP);

digitalWrite(driverReset, HIGH);

stepper.begin(stepPin, dirPin);

lcd.begin(16, 2); // Configura lcd numero columnas y filas
lcd.clear();
lcd.setCursor (0,0);
lcd.print("  FYP Wear Rig  ");
lcd.setCursor (0,1);
lcd.print("      Data      ");
delay(2000);
lcd.clear();
}

void startTest()  {
digitalWrite(startstopLED, HIGH);
if(moveRight == true){
  limits = limits + 1;
  travelRight();
}
else{
  limits = limits + 1;
  travelLeft();
}
}

void travelRight()  {
  speedControl = analogRead(speedPin);
  speedControl = map(speedControl, 0, 1023, 0, 10000);
  stepper.spin(speedControl);
  startstopState = digitalRead(startstopPin);
  RHSdirState = digitalRead(RHSdirPin);
  shutdownState = digitalRead(shutdownPin);
  if(startstopState == LOW){
  loop();
  if(RHSdirState == LOW){
    limits = limits + 1;
    travelLeft();
  }
  if(shutdownState == LOW){
    SHUTDOWN();
  }
}
}

void travelLeft()  {
  speedControl = analogRead(speedPin);
  speedControl = map(speedControl, 0, 1023, 0, 10000);
  stepper.spin(-speedControl);
  startstopState = digitalRead(startstopPin);
  LHSdirState = digitalRead(LHSdirPin);
  shutdownState = digitalRead(shutdownPin);
  if(startstopState == LOW){
    loop();
  if(LHSdirState == LOW){
    limits = limits + 1;
    travelRight();
  }
  if(shutdownState == LOW){
    SHUTDOWN();
  }
}
}

void SHUTDOWN() {
  resetState = digitalRead(shutdownPin);
  if(resetState = LOW){
    digitalWrite(resetLED, LOW);
    digitalWrite(dataLED, LOW);
    loop();
  }
  digitalWrite(driverReset, LOW);
  digitalWrite(startstopLED, LOW);
  digitalWrite(resetLED, HIGH);
  digitalWrite(dataLED, HIGH);
  delay(500);
  digitalWrite(resetLED, LOW);
  digitalWrite(dataLED, LOW);
}

void loop() {
digitalWrite(startstopLED, LOW);
stepper.spin(0);
startstopState = digitalRead(startstopPin);
if(startstopState == LOW){
  startTest();
}
}