#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;

byte start = false;

//old states
byte oldstartstopState = LOW;
byte oldRHSdirState = LOW;
byte oldLHSdirState = LOW;
byte oldshutdownState = LOW;

//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 loop() {
startstopState = digitalRead(startstopPin);
shutdownState = digitalRead(shutdownPin);
speedControl = analogRead(speedPin);
speedControl = map(speedControl, 0, 1023, 0, 10000);
resetState = digitalRead(resetPin);
RHSdirState = digitalRead(RHSdirPin);
LHSdirState = digitalRead(LHSdirPin);

if(startstopState != oldstartstopState){
  oldstartstopState = startstopState;
  if(startstopState == LOW){
    start = (start == true) ? false: true;
  }
}

if(start == true){
  digitalWrite(startstopLED, HIGH);
  if(RHSdirState != oldRHSdirState){
    oldRHSdirState = RHSdirState;
    if(RHSdirState == LOW){
      moveRight = false;
      limits = limits+1;  
    }
  }

  if(LHSdirState != oldLHSdirState){
    oldLHSdirState = LHSdirState;
    if(LHSdirState == LOW){
      moveRight = true;
      limits = limits+1;  
    }
  }

  if(moveRight == true){
    stepper.spin(speedControl);
    }

  if(moveRight == false){
    stepper.spin(-speedControl);
    }
  }

if(start == false){
  digitalWrite(startstopLED, LOW);
}

delay(100);
}