#include <Stepper.h>
#include <LiquidCrystal_I2C.h>

// change this to the number of steps on your motor
#define STEPS 1600
#define MM_PER_STEP 12

// encoder and stepper set 1 pins
#define STEPPER1_STEP_PIN A0
#define STEPPER1_DIR_PIN 7
#define ENCODER1_CLOCK_PIN 3
#define ENCODER1_DT_PIN 4
#define ENCODER1_SW_PIN 5

// encoder and stepper set 2 pins
#define STEPPER2_STEP_PIN 8
#define STEPPER2_DIR_PIN 6
#define ENCODER2_CLOCK_PIN 2
#define ENCODER2_DT_PIN A1
#define ENCODER2_SW_PIN A2

// LCD pins
#define DISPLAY_SCL_PIN A5
#define DISPLAY_SDA_PIN A4

// button pin
#define BTN_SWH_PIN A3

// create a variable to hold value of button switch
// btwn %spd and pos modes
int btnSwhVal = 0;

// create variables for ***STEPPER 1***, specifying
// the number of steps of the motor and the pins it is
// attached to
boolean stepper1Enabled = false;
boolean isStepper1SpeedMode = false;
int stepper1PercentSpeed = 0;
int stepper1Delay = 0;
int stepper1Position = 0;
int previousStepper1Position = 0;
int stepper1MM = 0;

// create variables for ***STEPPER 2***, specifying
// the number of steps of the motor and the pins it is
// attached to
boolean stepper2Enabled = false;
boolean isStepper2SpeedMode = false;
int stepper2PercentSpeed = 0;
int stepper2Delay = 0;
int stepper2Position = 0;
int previousStepper2Position = 0;
int stepper2MM = 0;

// create an instance of the LCD I2C class
LiquidCrystal_I2C lcd(0x27, 16, 2);

// create long variables to hold the values of
// last button press for button 1, button 2,
// and button switch
unsigned long lastButton1Press = 0;
unsigned long lastButton2Press = 0;

unsigned long lastButtonSwhPress = 0;

unsigned long lastLCD = 0;

void setup() {
  
  // set the pins connected to encoder 1 and 2 clock,
  // dt, and sw pins to inputs
  pinMode(ENCODER1_CLOCK_PIN, INPUT);
  pinMode(ENCODER1_DT_PIN, INPUT);
  pinMode(ENCODER1_SW_PIN, INPUT_PULLUP);
  pinMode(ENCODER2_CLOCK_PIN, INPUT);
  pinMode(ENCODER2_DT_PIN, INPUT);
  pinMode(ENCODER2_SW_PIN, INPUT_PULLUP);

  pinMode(STEPPER1_STEP_PIN, OUTPUT);
  pinMode(STEPPER1_DIR_PIN, OUTPUT);
  pinMode(STEPPER2_STEP_PIN, OUTPUT);
  pinMode(STEPPER2_DIR_PIN, OUTPUT);

  pinMode(BTN_SWH_PIN, INPUT_PULLUP);
  
  // create an interrupt sequence for when encoders
  // clock pins are in the falling cycle
  attachInterrupt(digitalPinToInterrupt(ENCODER1_CLOCK_PIN), readEncoder1, FALLING);
  attachInterrupt(digitalPinToInterrupt(ENCODER2_CLOCK_PIN), readEncoder2, FALLING);

  // set up Serial monitor
  Serial.begin(9600);

  // set up display monitor
  lcd.init();
  lcd.backlight();
  setUpLCD();
}

void readEncoder1() {
  int dtValue = digitalRead(ENCODER1_DT_PIN);
  if (dtValue == HIGH) {
    if (isStepper1SpeedMode){
      stepper1PercentSpeed = min(stepper1PercentSpeed + 5, 100);
    }
    else {
      previousStepper1Position = stepper1Position;
      stepper1Position++;
      stepper1MM += MM_PER_STEP;
    }
  }
  else {
    if (isStepper1SpeedMode) {
      stepper1PercentSpeed = max(stepper1PercentSpeed - 5, -100);
    }
    else {
      previousStepper1Position = stepper1Position;
      stepper1Position--;
      stepper1MM -= MM_PER_STEP;
    }
  }
}

void readEncoder2() {
  int dtValue = digitalRead(ENCODER2_DT_PIN);
  if (dtValue == HIGH) {
    if (isStepper2SpeedMode) {
      stepper2PercentSpeed = min(stepper2PercentSpeed + 5, 100);
    }
    else {
      previousStepper2Position = stepper2Position;
      stepper2Position++;
      stepper2MM += MM_PER_STEP;
    }
  }
  else {
    if (isStepper2SpeedMode) {
      stepper2PercentSpeed = max(stepper2PercentSpeed - 5, -100);
    }
    else {
      previousStepper2Position = stepper2Position;
      stepper2Position--;
      stepper2MM -= MM_PER_STEP;
    }
  }
}

void stepper1SpeedMode() {
  int desiredMaxSpeedRPM = 60;
  
  // Convert RPM to steps per second
  float desiredMaxSpeedStepsPerSec = (desiredMaxSpeedRPM * STEPS) / 60.0;
  
  // Set the step delay to control motor speed for both drivers
  // Formula: delayMicroseconds = (1 / steps_per_second) * 1,000,000
  unsigned int stepMinDelay = 1000000 / desiredMaxSpeedStepsPerSec;
  unsigned int stepMaxDelay = stepMinDelay * 10000;

  if (stepper1Enabled && stepper1PercentSpeed != 0){
    int stepDelay = (100 - abs(stepper1PercentSpeed)) / (100) * (stepMaxDelay - stepMinDelay);
    if (stepper1PercentSpeed > 0){
      digitalWrite(STEPPER1_DIR_PIN, HIGH);
    }
    else{
      digitalWrite(STEPPER1_DIR_PIN, LOW);
    }
    //for (int x = 0; x < 10; x++) {
      digitalWrite(STEPPER1_STEP_PIN, HIGH);
      delayMicroseconds((stepDelay + stepMinDelay)/2);
      digitalWrite(STEPPER1_STEP_PIN, LOW);
    //  delayMicroseconds(stepDelay + stepMinDelay);
    //}
  }
}

void stepper2SpeedMode() {
  int desiredMaxSpeedRPM = 60;
  
  // Convert RPM to steps per second
  float desiredMaxSpeedStepsPerSec = (desiredMaxSpeedRPM * STEPS) / 60.0;
  
  // Set the step delay to control motor speed for both drivers
  // Formula: delayMicroseconds = (1 / steps_per_second) * 1,000,000
  unsigned int stepMinDelay = 1000000 / desiredMaxSpeedStepsPerSec;
  unsigned int stepMaxDelay = stepMinDelay * 10000;

  if (stepper2Enabled && stepper2PercentSpeed != 0){
    int stepDelay = (100 - abs(stepper2PercentSpeed)) / (100) * (stepMaxDelay - stepMinDelay);
    if (stepper2PercentSpeed > 0){
      digitalWrite(STEPPER2_DIR_PIN, HIGH);
    }
    else{
      digitalWrite(STEPPER2_DIR_PIN, LOW);
    }
    digitalWrite(STEPPER2_STEP_PIN, HIGH);
    delayMicroseconds((stepDelay + stepMinDelay)/2);
    digitalWrite(STEPPER2_STEP_PIN, LOW);
  }
}

void stepper1PositionMode() {
  int stepsToTake = stepper1Position - previousStepper1Position;
  // sets direction to move
  if (stepsToTake > 0) { 
    digitalWrite(STEPPER1_DIR_PIN, HIGH);
  }
  else {
    digitalWrite(STEPPER1_DIR_PIN, LOW);
  }
  // moves 1/20 of a revolution if stepper1 position has changed
  for (int i = 0; i < abs(stepsToTake) * STEPS / 20; i++) {
    digitalWrite(STEPPER1_STEP_PIN, HIGH);
    delayMicroseconds(500);
    digitalWrite(STEPPER1_STEP_PIN, LOW);
  }
  previousStepper1Position = stepper1Position;
}

void stepper2PositionMode() {
  int stepsToTake = stepper2Position - previousStepper2Position;
  // sets direction to move
  if (stepsToTake > 0) { 
    digitalWrite(STEPPER2_DIR_PIN, HIGH);
  }
  else {
    digitalWrite(STEPPER2_DIR_PIN, LOW);
  }
  // moves 1/20 of a revolution if stepper2 position has changed
  for (int i = 0; i < abs(stepsToTake) * STEPS / 20; i++) {
    digitalWrite(STEPPER2_STEP_PIN, HIGH);
    delayMicroseconds(500);
    digitalWrite(STEPPER2_STEP_PIN, LOW);
  }
  previousStepper2Position = stepper2Position;
}

void setUpLCD() {
  lcd.setCursor(0,0);
  lcd.print("spd:");
  lcd.setCursor(9, 0);
  lcd.print("mm:");

  lcd.setCursor(0,1);
  lcd.print("spd:");
  lcd.setCursor(9, 1);
  lcd.print("mm:");
}

void checkBtnSwhState() {
  int btnSwhState = digitalRead(BTN_SWH_PIN);

	// if LOW signal detected, button switch is pressed
	if (btnSwhState == LOW) {
		// if 50ms have passed since last LOW pulse, it means that the
		// button has been pressed
		if (millis() - lastButtonSwhPress > 50) {
      btnSwhVal++;
      isStepper1SpeedMode = !(isStepper1SpeedMode);
      isStepper2SpeedMode = !(isStepper2SpeedMode);
      stepper1Enabled = false;
      stepper2Enabled = false;
		}

		// Remember last button press event (last LOW reading)
		lastButtonSwhPress = millis();
	}
}

void checkBtn1State() {
  int btn1State = digitalRead(ENCODER1_SW_PIN);

	// if LOW signal detected, encoder1 button is pressed
	if (btn1State == LOW) {
		// if 50ms have passed since last LOW pulse, it means that the
		// button has been pressed
		if (millis() - lastButton1Press > 50) {
      stepper1Enabled = !stepper1Enabled;
		}

		// Remember last button press event (last LOW reading)
		lastButton1Press = millis();
	}
}

void checkBtn2State() {
  int btn2State = digitalRead(ENCODER2_SW_PIN);

	// if LOW signal detected, encoder2 button is pressed
	if (btn2State == LOW) {
		// if 50ms have passed since last LOW pulse, it means that the
		// button has been pressed
		if (millis() - lastButton2Press > 50) {
      stepper2Enabled = !stepper2Enabled;
		}

		// Remember last button press event (last LOW reading)
		lastButton2Press = millis();
	}
}

void displayLCD() {
	// if 500ms have passed since last LCD update, updates screen
  if (millis() - lastLCD > 500) {
    // update first line
    lcd.setCursor(4,0);
    lcd.print((String)(abs(stepper1PercentSpeed)) + "%  ");
    lcd.setCursor(12, 0);
    lcd.print((String)(stepper1MM));
    
    // update second line
    lcd.setCursor(4,1);
    lcd.print((String)(abs(stepper2PercentSpeed)) + "%  ");
    lcd.setCursor(12, 1);
    lcd.print((String)(stepper2MM));
	}
	// Remember last button press event (last LOW reading)
	lastLCD = millis();
}

void loop() {

  if(btnSwhVal % 2 == 1){
    // SPEED
    stepper1SpeedMode();
    stepper2SpeedMode();
  }
  else {
    // POSITION
    stepper1PositionMode();
    stepper2PositionMode();
  }
  
  checkBtnSwhState();
  
  // ENCODER1 BUTTON:
  // Read the state of encoder1 button
	checkBtn1State();
  
  // ENCODER2 BUTTON:
  // Read the state of encoder2 button
	checkBtn2State();
	
	// update LCD screen with %speed and position
	displayLCD();
}