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

ContinuousStepper<StepperDriver> stepper;

//initialize lcd
LiquidCrystal_I2C lcd1(0x27,16,2);  //data
LiquidCrystal_I2C lcd2(0x26,16,2);  //timer

byte dirPin = 12;
byte stepPin = 13;
byte startstopPin = 33;
byte changedataPin = 23;
byte resetPin = 49;
byte onoffPin = 53;
byte speedPin = A5;
byte driverReset = 11;

//LED's
byte startstopLED = 41;
byte resetLED = 51;
byte 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 = true;
byte lastlimithit = 0;    //0 = LHS   1 = RHS

int speedControl = 0;

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

//timer
int hh=0, mm=0, ss=0, ms=0;

void 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);

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

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

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

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

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

  RUNNING();
}

void SHUTDOWN() {
  resetState = digitalRead(resetPin);
  start = false;

  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 RUNNING()  {
  startstopState = digitalRead(startstopPin);
  LHSdirState = digitalRead(LHSdirPin);
  RHSdirState = digitalRead(RHSdirPin);
  shutdownState = digitalRead(shutdownPin);

  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 != changedataState){
    oldchangedataState = changedataState;
    if(changedataState == LOW){
      datamode = datamode + 1;
      if(datamode = 4){
        datamode = 0;
      }
    }
  }

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

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

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

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

void RESET()  {
  limits = 0;
  speed = 0;
  distance = 0;
  ms=0, ss=0, mm=0, hh=0;
}

ISR(TIMER1_COMPA_vect){   
  if(start == true){
    ms=ms+1;
  if(ms>999){ms=0;ss=ss+1;
  if(ss>59){ss=0; mm=mm+1;}
  if(mm>59){mm=0; hh=hh+1;}
    }  
  }
}

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

  lcd2.init();
  lcd2.backlight();
  lcd2.clear();
  lcd2.setCursor (0,0);
  lcd2.print("  FYP Wear Rig  ");
  lcd2.setCursor (0,1);
  lcd2.print("   Stopwatch   ");

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

  lcd1.clear();
  lcd2.clear();

  noInterrupts();         // disable all interrupts
  TCCR1A = 0;             // set entire TCCR1A register to 0  //set timer1 interrupt at 1kHz  // 1 ms
  TCCR1B = 0;             // same for TCCR1B
  TCNT1  = 0;             // set timer count for 1khz increments
  OCR1A = 1999;           // = (16*10^6) / (1000*8) - 1
  //had to use 16 bit timer1 for this bc 1999>255, but could switch to timers 0 or 2 with larger prescaler
  // turn on CTC mode
  TCCR1B |= (1 << WGM12); // Set CS11 bit for 8 prescaler
  TCCR1B |= (1 << CS11);  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);
  interrupts();           // enable
}

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 = true;
    }
  }

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

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

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