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

ContinuousStepper<StepperDriver> stepper;

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

#define dirPin  12
#define stepPin  13
#define startstopPin  33
#define changedataPin  23
#define resetPin  49
#define onoffPin  53
#define speedPin  A5
#define driverReset  11

//LED's
#define startstopLED  41
#define resetLED  51
#define dataLED  25

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 changedataState = LOW;

//old states
byte oldstartstopState = LOW;
byte oldRHSdirState = LOW;
byte oldLHSdirState = LOW;
byte oldresetState = LOW;
byte oldchangedataState = LOW;

byte start = false;
int lastlimithit = 0;    //0 = LHS   1 = RHS

int speedControl = 0;

//Data
int datamode = 0;
int limits = 0;
float distance = 0;
double speed = 0;

//speed calcs
double startsec = 0;
double endsec = 0;
double speedtime = 0;

void CHANGEDATA()  {
  Serial.print("changedata");
  if(datamode == 4){
    datamode = 0;
  }

  if(datamode == 0){
    lcd.clear();
    lcd.setCursor (0,0);
    lcd.print("  FYP Wear Rig  ");
    lcd.setCursor (0,1);
    lcd.print("      Data      ");
  }

  if(datamode == 1){
    lcd.clear();
    lcd.setCursor (0,0);
    lcd.print("Limits Hit");
    lcd.setCursor (0,1);
    lcd.print(limits);
  }

  if(datamode == 2){
    lcd.clear();
    lcd.setCursor (0,0);
    lcd.print("Speed (m/s)");
    lcd.setCursor (0,1);
    lcd.print(speed);
  }

  if(datamode == 3){
    lcd.clear();
    lcd.setCursor (0,0);
    lcd.print("Distance (m)");
    lcd.setCursor (0,1);
    lcd.print(distance);
  }
  return;
}

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

  endsec = millis()/1000;
  speedtime = endsec - startsec;
  speed = 1.8/speedtime;
  startsec = endsec;

  if(startstopState != oldstartstopState){
    oldstartstopState = startstopState;
    if(startstopState == LOW){
      start = false;
      loop();
    }
  }

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

void RHShit()  {
  Serial.print("RHShit");
  speedControl = analogRead(speedPin);
  speedControl = map(speedControl, 0, 1023, 0, 10000);
  stepper.spin(-speedControl);
  startstopState = digitalRead(startstopPin);
  LHSdirState = digitalRead(LHSdirPin);
  shutdownState = digitalRead(shutdownPin);

  endsec = millis()/1000;
  speedtime = endsec - startsec;
  speed = 1.8/speedtime;
  startsec = endsec;

  if(startstopState != oldstartstopState){
    oldstartstopState = startstopState;
    if(startstopState == LOW){
      start = false;
      loop();
    }
  }

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

void SHUTDOWN() {
  Serial.print("shutdown");
  resetState = digitalRead(resetPin);
  start = false;
  digitalWrite(driverReset, LOW);

  lcd.clear();
  lcd.setCursor (0,0);
  lcd.print("    WARNING     ");
  lcd.setCursor (0,1);
  lcd.print("    SHUTDOWN    ");

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

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

void RUNNING()  {
  Serial.print("running");
  Serial.print(speedtime);
  startstopState = digitalRead(startstopPin);
  LHSdirState = digitalRead(LHSdirPin);
  RHSdirState = digitalRead(RHSdirPin);
  shutdownState = digitalRead(shutdownPin);
  changedataState = digitalRead(changedataPin);

  distance = limits*1.8;

  if(startstopState != oldstartstopState){
    oldstartstopState = startstopState;
    if(startstopState == LOW){
      start = false;
      loop();
    }
  }

  if(RHSdirState != oldRHSdirState){
    oldRHSdirState = RHSdirState;
    if(RHSdirState == LOW){
      limits = limits + 1;
      lastlimithit = 1;
      RHShit();
    }
  }

  if(LHSdirState != oldLHSdirState){
    oldLHSdirState = LHSdirState;
    if(LHSdirState == LOW){
      limits = limits + 1;
      lastlimithit = 0;
      LHShit();
    }
  }

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

  if(changedataState != oldchangedataState){
    oldchangedataState = changedataState;
    if(changedataState == LOW){
      datamode = datamode+1;
      CHANGEDATA();
    }
  }

  RUNNING();
}

void RESET()  {
  Serial.print("reset");
  limits = 0;
  speed = 0;
  distance = 0;
  loop();
}

void setup() {
  Serial.print("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);

  Serial.begin(9600);

  //lcd

  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor (0,0);
  lcd.print("  FYP Wear Rig  ");
  lcd.setCursor (0,1);
  lcd.print("      Data      ");
  delay(2000);
}

void loop() {
  Serial.print("loop");
  digitalWrite(driverReset, HIGH);
  digitalWrite(startstopLED, LOW);
  stepper.spin(0);
  startstopState = digitalRead(startstopPin);
  shutdownState = digitalRead(shutdownPin);
  resetState = digitalRead(resetPin);
  changedataState = digitalRead(changedataPin);

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

  if(start == true){
    digitalWrite(startstopLED, HIGH);
    if(lastlimithit == 0){
      LHShit();
    }
    else{
      RHShit();
    }
  }

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

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

  if(changedataState != oldchangedataState){
    oldchangedataState = changedataState;
    if(changedataState == LOW){
      datamode = datamode+1;
      CHANGEDATA();
    }
  }

  loop();
}