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

//old states
byte oldstartstopState = LOW;
byte oldresetState = LOW;

byte start = true;
byte lastlimithit = LHS;

int speedControl = 0;

//Data
int limits = -1;
float distance = 0;
float 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() {
  digitalWrite(driverReset, HIGH);
  digitalWrite(startstopLED, LOW);
  stepper.spin(0);
  startstopState = digitalRead(startstopPin);
  shutdownState = digitalRead(shutdownPin);
  resetState = digitalRead(resetPin);

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

  if(start == true){
    if(lastlimithit == LHS){
      LHShit();
    }
    else{
      RHShit();
    }
  }

  if(shutdownState == LOW){
    SHUTDOWN();
  }

  if(resetState == LOW){
    RESET();
  }
}

void LHShit()  {
    speedControl = analogRead(speedPin);
    speedControl = map(speedControl, 0, 1023, 0, 10000);
    stepper.spin(speedControl);
    startstopState = digitalRead(startstopPin);
    RHSdirState = digitalRead(RHSdirPin);
    shutdownState = digitalRead(shutdownPin);

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

  if(RHSdirState == LOW){
    limits = limits + 1;
    RHShit();
  }
  if(shutdownState == LOW){
    SHUTDOWN();
  }
}

void LHShit()  {
    speedControl = analogRead(speedPin);
    speedControl = map(speedControl, 0, 1023, 0, 10000);
    stepper.spin(-speedControl);
    startstopState = digitalRead(startstopPin);
    RHSdirState = digitalRead(RHSdirPin);
    shutdownState = digitalRead(shutdownPin);

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

  if(LHSdirState == LOW){
    limits = limits + 1;
    LHShit();
  }
  if(shutdownState == LOW){
    SHUTDOWN();
  }
}

void SHUTDOWN() {
  restState = digitalRead(resetPin);

  if(resetState != oldresetState){
  oldresetState = resetState;
    if(resetState == LOW){
      loop();
    }
  }

  digitalWrite(driverReset, LOW);
  digitalWrite(startstopLED, LOW);
  digitalWrite(resetLED, HIGH);
  digitalWrite(dataLED, HIGH);
  delay(500);
  digitalWrite(resetLED, LOW);
  digitalWrite(dataLED, LOW);
}

void RESET()  {
  limits = -1;
  speed = 0;
  distance = 0;
}