// Add library
#include <Keypad.h>
#include <LiquidCrystal.h>
#include <AccelStepper.h>
#include <EEPROM.h>
#include "HX711_ADC.h"
#include <SD.h>

// Define digital pins for motors
#define dirPin 19
#define stepPin 18
#define dirPin2 24
#define stepPin2 22
#define stepsPerRevolution 200
#define motorInterfaceType 1
#define PIN_SPI_CS 53

File myFile;

// define motors parameters
AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);
AccelStepper stepper2 = AccelStepper(motorInterfaceType, stepPin2, dirPin2);
//const int RS = 13, EN = 12, D4 = 14, D5 = 15, D6 = 16, D7 = 17;

HX711_ADC LoadCell_1(A0, A1);
HX711_ADC LoadCell_2(A2, A3);
unsigned long t = 0;

// define LCD digital pins
LiquidCrystal lcd(13, 12, 14, 15, 16, 17);
LiquidCrystal lcd2(3, 2, 25, 27, 29, 31);

// define keypad digital pins
const byte ROWS = 4;       //four rows
const byte COLS = 4;       //four columns
char keys[ROWS][COLS] = {  //Defining keypad characters
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};
byte rowPins[ROWS] = { 11, 10, 9, 8 };  //digital connect to the row pins of the keypad
byte colPins[COLS] = { 7, 6, 5, 4 };  //digital connect to the column pints of the keypad

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);  //mapping digital with keypad

// define common variables
String displacementValue = "";    // User defined max displacement
double displacement;

int pressedA = 0;                 //Initialize key A state
int pressedB = 0;                 //Initialize key B state
int next = 0;                     //define motors operation
double steps = 0;                 //displacement translated as motor steps
int numCheck = 0;
double validMM = 100;             //Valid mm range
double validIN = 100;             //Valid in range
unsigned long int t1 = 0;         //time region
unsigned long int t2 = 0;
int i = 0;
int dir = 0;                      //motors direction
int mode2 = 0;                    //motors mode\

//holding variable
char key3;
char holdKey;
unsigned long t_hold;

// Initial function setup
void setup() {
  //scale.begin(A0, A1);

  // Serial and LCD initial
  Serial.begin(9600);

  if (!SD.begin(PIN_SPI_CS)) {
    Serial.println(F("SD CARD FAILED, OR NOT PRESENT!"));
    while (1); // don't do anything more:
  }
  Serial.println(F("SD CARD INITIALIZED."));
  SD.remove("arduino.txt"); // delete the file if existed

  // create new file by opening file for writing
  myFile = SD.open("arduino.txt", FILE_WRITE);

  if (myFile) {
    Serial.println("SD is recording"); 
  } else {
    Serial.print(F("SD Card: error on opening file arduino.txt"));
  }

  Serial.println("HX710B Demo with HX711 Library");
  Serial.println("Initializing the scale");

  LoadCell_1.begin();
  LoadCell_2.begin();
  //LoadCell.setReverseOutput(); //uncomment to turn a negative output value to positive
  float calibrationValue_1; // calibration value (see example file "Calibration.ino")
  float calibrationValue_2;
  calibrationValue_1 = 0.42; // uncomment this if you want to set the calibration value in the sketch
  calibrationValue_2 = 0.42;
  
  unsigned long stabilizingtime = 2000; // preciscion right after power-up can be improved by adding a few seconds of stabilizing time
  boolean _tare = true; //set this to false if you don't want tare to be performed in the next step
  byte loadcell_1_rdy = 0;
  byte loadcell_2_rdy = 0;
  while ((loadcell_1_rdy + loadcell_2_rdy) < 2) { //run startup, stabilization and tare, both modules simultaniously
    if (!loadcell_1_rdy) loadcell_1_rdy = LoadCell_1.startMultiple(stabilizingtime, _tare);
    if (!loadcell_2_rdy) loadcell_2_rdy = LoadCell_2.startMultiple(stabilizingtime, _tare);
  }
  if (LoadCell_1.getTareTimeoutFlag()) {
    Serial.println("Timeout, check MCU>HX711 no.1 wiring and pin designations");
  }
  if (LoadCell_2.getTareTimeoutFlag()) {
    Serial.println("Timeout, check MCU>HX711 no.2 wiring and pin designations");
  }
  LoadCell_1.setCalFactor(calibrationValue_1); // user set calibration value (float)
  LoadCell_2.setCalFactor(calibrationValue_2); // user set calibration value (float)
  Serial.println("Startup is complete");


/*
  Serial.println("Initializing the scale");
  Serial.println("Before setting up the scale:");
  Serial.print("read: \t\t");
  Serial.println(scale.read());                  // print a raw reading from the ADC
  Serial.print("read average: \t\t");
  Serial.println(scale.read_average(20));        // print the average of 20 readings from the ADC
  Serial.print("get value: \t\t");
  Serial.println(scale.get_value(5));            // print the average of 5 readings from the ADC minus the tare weight (not set yet)
  Serial.print("get units: \t\t");
  Serial.println(scale.get_units(5), 1);         // print the average of 5 readings from the ADC minus tare weight (not set) divided

  // by the SCALE parameter (not set yet)
  scale.set_scale(0.42);                       // this value is obtained by calibrating the scale with known weights; see the README for details
  scale.tare();                                  // reset the scale to 0

  Serial.println("After setting up the scale:");
  Serial.print("read: \t\t");
  Serial.println(scale.read());                  // print a raw reading from the ADC
  Serial.print("read average: \t\t");
  Serial.println(scale.read_average(20));        // print the average of 20 readings from the ADC
  Serial.print("get value: \t\t");
  Serial.println(scale.get_value(5));            // print the average of 5 readings from the ADC minus the tare weight, set with tare()
  Serial.print("get units: \t\t");
  Serial.println(scale.get_units(5), 1);         // print the average of 5 readings from the ADC minus tare weight, divided

  // by the SCALE parameter set with set_scale
  Serial.println("Readings:");
*/
  lcd.begin(20, 4);
  lcd.setCursor(0, 0);
  lcd.print("  Press A to Start");
  lcd2.begin(20, 4);
  lcd2.setCursor(0, 0);
  lcd2.print("  Press A to Start");

  //digital pin for output
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  stepper.setMaxSpeed(1000);
  stepper2.setMaxSpeed(1000);
  digitalWrite(dirPin, HIGH);
  digitalWrite(dirPin2, HIGH);
}

// loop function for the system
void loop() {
  // 8mm per rev; 0.31496063 in per rev;
  // 200 steps per rev; 0.125 rev per mm; 1/0.31496063 rev per in;
  // steps = 0.125*displacement*200
  // steps = 1/0.31496063*displacement*200

  // translate User defined max displacement to step
  displacement = displacementValue.toDouble();
  if (pressedA == 1) {
    steps = 1 / 0.31496063 * displacement * 200;
  }
  if (pressedB == 1) {
    steps = 0.125 * displacement * 200;
  }

  // Enter mode 1 of the system
  if (next == 1) {
    //       if ( (i % 2) == 0) {
    //       digitalWrite(dirPin, HIGH);
    //       digitalWrite(dirPin2, HIGH);
    //       }
    int speed = 100;                    // motor initial speed

    // Enter mode 2 of the system
    if (mode2 == 1) {
      speed = 0;                            // motor inital speed
    }

    while (next == 1)
    {
      lcd.setCursor(0, 0);
      if (mode2 == 0) {
        lcd.print("Mode1:Extend/Retract");
      }
      if (mode2 == 1) {
        lcd.print("Mode2:Manual Control");
      }
      // display data using imperial
      if (pressedA == 1) {
        //lcd.clear();
        lcd.setCursor(0, 1);
        lcd.print("X = ");
        lcd.print(stepper.currentPosition() / (1 / 0.31496063) / 200, 4);
        lcd.print(" in");
        lcd.setCursor(0, 2);
        lcd.print("Y = ");
        lcd.print(stepper2.currentPosition() / (1 / 0.31496063) / 200, 4);
        lcd.print(" in");
        Serial.print("X = ");
        Serial.print(stepper.currentPosition() / (1 / 0.31496063) / 200, 4);
        Serial.print(" in  Y = ");
        Serial.print(stepper2.currentPosition() / (1 / 0.31496063) / 200, 4);
        Serial.print(" in  ");
        Loadcell();

        myFile.print("X = ");
        myFile.print(stepper.currentPosition() / (1 / 0.31496063) / 200, 4);
        myFile.print(" in  ");
        myFile.print("Y = ");
        myFile.print(stepper2.currentPosition() / (1 / 0.31496063) / 200, 4);
        myFile.print(" in");

        float a = LoadCell_1.getData();
        float b = LoadCell_2.getData();
        String c = "g";
        lcd2.setCursor(0,0);
        lcd2.print("X = ");
        if (pressedA == 1){
          a = a*0.00220462;
          b = b*0.00220462;
          c = "lb";
        }

        myFile.print("  X = ");
        myFile.print(a, 4);
        myFile.print(" ");
        myFile.print(c);
        myFile.print("  Y = ");
        myFile.print(b, 4);
        myFile.print(" ");
        myFile.print(c);
        
        /*
        lcd2.setCursor(0,1);
        lcd2.print("one reading:\t");
        lcd2.print(scale.get_units(), 1);
        lcd2.print("\t| average:\t");
        lcd2.print(scale.get_units(10), 1);
        */

        /*
        Serial.print("one reading:\t");
        Serial.print(scale.get_units(), 1);
        Serial.print("\t| average:\t");
        Serial.print(scale.get_units(10), 1);*/
        Serial.print("  t = ");
      }
      // display data using metrics
      if (pressedB == 1) {
        //lcd.clear();
        lcd.setCursor(0, 1);
        lcd.print("X = ");
        lcd.print(stepper.currentPosition() / 0.125 / 200, 4);
        lcd.print(" mm");
        lcd.setCursor(0, 2);
        lcd.print("Y = ");
        lcd.print(stepper2.currentPosition() / 0.125 / 200, 4);
        lcd.print(" mm");
        Serial.print("X = ");
        Serial.print(stepper.currentPosition() / 0.125 / 200, 4);
        Serial.print(" mm  Y = ");
        Serial.print(stepper2.currentPosition() / 0.125 / 200, 4);
        Serial.print(" mm  ");

        Loadcell();

        myFile.print("X = ");
        myFile.print(stepper.currentPosition() / 0.125 / 200, 4);
        myFile.print(" mm  ");
        myFile.print("Y = ");
        myFile.print(stepper2.currentPosition() / 0.125 / 200, 4);
        myFile.print(" mm");

        float a = LoadCell_1.getData();
        float b = LoadCell_2.getData();
        String c = "g";
        lcd2.setCursor(0,0);
        lcd2.print("X = ");
        if (pressedA == 1){
          a = a*0.00220462;
          b = b*0.00220462;
          c = "lb";
        }

        myFile.print("  X = ");
        myFile.print(a, 4);
        myFile.print(" ");
        myFile.print(c);
        myFile.print("  Y = ");
        myFile.print(b, 4);
        myFile.print(" ");
        myFile.print(c);
        /*
        lcd2.setCursor(0,1);
        lcd2.print("one reading:\t");
        lcd2.print(scale.get_units(), 1);
        lcd2.print("\t| average:\t");
        lcd2.print(scale.get_units(10), 1);
        */
        /*
        Serial.print("one reading:\t");
        Serial.print(scale.get_units(), 1);
        Serial.print("\t| average:\t");
        Serial.print(scale.get_units(10), 1);
        */
        Serial.print("  t = ");
      }
      // display time data
      t2 = millis();
      Serial.print(t2 - t1);
      Serial.println(" ms");
      lcd.setCursor(0, 3);
      lcd.print("t = ");
      lcd.print(t2 - t1);
      lcd.print(" ms");

      myFile.print("  t = ");
      myFile.print(t2 - t1);
      myFile.println(" ms  ");

      // enter mode 2 operation
      if (mode2 == 1) {

        if (key3) {
          holdKey = key3;
        }
        // Enter if key is pressed
        if (keypad.getState() == PRESSED) {
          if (holdKey == 'B' && stepper.currentPosition() < steps) {
            speed = 100;
          }
          else if (holdKey == 'C' && stepper.currentPosition() > 0) {
            speed = -100;
          }
          else {
            speed = 0;
          }
        }
        // Enter if key is held
        else if (keypad.getState() == HOLD) {
          if ((millis() - t_hold) > 0 ) {
            if (holdKey == 'B' && stepper.currentPosition() < steps) {
              speed = 100;
            }
            else if (holdKey == 'C' && stepper.currentPosition() > 0) {
              speed = -100;
            }
            else {
              speed = 0;
            }
            t_hold = millis();
          }
        }
        // Enter if idle
        else {
          speed = 0;
        }
        /*
          if (key3 == 'B' && stepper.currentPosition() < steps){
          KeyState state = keypad.getState();
          if(state == PRESSED){
            speed = 100;
          }
          }
          else if (key3 == 'C'&& stepper.currentPosition() > 0){
          KeyState state = keypad.getState();
          if(state == PRESSED){
          speed = -100;
          }
          }
          else{
            speed = 0;
          }
        */
        //motor control
        stepper.setSpeed(speed);
        stepper.runSpeed();

        stepper2.setSpeed(speed);
        stepper2.runSpeed();
      }

      // Enter mode 1 operation if operation 2 didnt run
      // Motor extension
      else if (stepper.currentPosition() <= steps && dir == 0) {
        stepper.setSpeed(speed);
        stepper.runSpeed();

        stepper2.setSpeed(speed);
        stepper2.runSpeed();
        //t2 = millis();
        //Serial.print(t2-t1);
        //Serial.println(" ms");
        // change direction
        if (stepper.currentPosition() > steps) {
          dir = 1;
          delay(1000);
        }

      }
      // Motor retraction
      else if (stepper.currentPosition() >= 0 && dir == 1) {
        stepper.setSpeed(-speed);
        stepper.runSpeed();

        stepper2.setSpeed(-speed);
        stepper2.runSpeed();
        //t2 = millis();
        //Serial.print(t2-t1);
        //Serial.println(" ms");

        // change direction
        if (stepper.currentPosition() < 0) {
          dir = 0;
          delay(1000);
        }
      }

      // stop motors
      key3 = keypad.getKey();
      if (key3 == 'A') {
        speed = 0;
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Motors Stopped");
        lcd.setCursor(0, 2);
        lcd.print("Press A to Continue");
        next = 0;
        validMM = 100;
        validIN = 100;
        myFile.close();
        Serial.println("------Reading Data from SD------");
        myFile = SD.open("arduino.txt", FILE_READ);
        while (myFile.available()) {
          char ch = myFile.read(); // read characters one by one from Micro SD Card
          Serial.print(ch); // print the character to Serial Monitor
        }
        myFile.close();
        
      }
      i++;
    }
  }

  // User interaction
  while (validIN > 1 && validMM > 25.4) {
    char key = keypad.getKey();               //key is input
    int dot = 1;
    if (key != NO_KEY) {                      // When pressed

      // Request max displacement
      if (key == 'A' & next == 0) {                           //Continue after A is pressed
         myFile = SD.open("arduino.txt", FILE_WRITE);
        lcd.clear();
        lcd2.clear();
        lcd.setCursor(0, 0);
        lcd.print("Enter Max");
        lcd.setCursor(0, 1);
        lcd.print("Displacement: in");
        displacementValue = "";
        numCheck = 0;
        pressedA = 1;
        pressedB = 0;
      }
      // Change to metrics
      if (key == 'B' & next == 0 & pressedA == 1) {            //Continue after B is pressed
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Enter Max");
        lcd.setCursor(0, 1);
        lcd.print("Displacement: mm");
        displacementValue = "";
        numCheck = 0;
        pressedA = 0;
        pressedB = 1;
      }

      // Enter numerical values
      if (key != 'A' && key != 'B' && key != 'C' && key != 'D' && key != '*' && key != '#' && (pressedA == 1 || pressedB == 1)) {
        displacementValue = displacementValue + key;  //Reset value
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print(displacementValue);        //display key input
        numCheck = 1;

      }
      // Allow decimal values
      if (key == '*' && dot == 1 && (pressedA == 1 || pressedB == 1)) {
        displacementValue = displacementValue + '.';  //Reset value
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print(displacementValue);        //display key input
        dot = 0;
      }

      // Key evaluation
      if (key == 'D' && numCheck == 1) {  //if key D is pressed
        if (pressedA == 1) {
          validIN = displacementValue.toDouble();
        }
        if (pressedB == 1) {
          validMM = displacementValue.toDouble();
        }
        // Display input in imperial (Incorrect ver.)
        if (validIN > 1 && pressedA == 1) {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Max Displacement");
          lcd.setCursor(0, 1);
          lcd.print("is too large");
          delay (2000);
          lcd.setCursor(0, 0);
          lcd.print("Enter Max");
          lcd.setCursor(0, 1);
          lcd.print("Displacement: in");
          displacementValue = "";
        }
        // Display input in metrics (Incorrect ver.)
        else if (validMM > 25.4 && pressedB == 1) {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Max Displacement");
          lcd.setCursor(0, 1);
          lcd.print("is too large");
          delay (2000);
          lcd.setCursor(0, 0);
          lcd.print("Enter Max");
          lcd.setCursor(0, 1);
          lcd.print("Displacement: mm");
          displacementValue = "";
        }
        // Display inputs
        else {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Displacement: ");
          lcd.setCursor(0, 1);
          lcd.print(displacementValue);        //display key input
          if (pressedA == 1) {
            lcd.println(" in");
          }
          else {
            lcd.println(" mm");
          }

          delay(2000);

          // display motors mode option
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Mode1:Extend/Retract");
          lcd.setCursor(0, 1);
          lcd.print("Mode2:Manual Control");

          // Leading to mode 1 or mode 2
          char key2 = keypad.getKey();
          while (next == 0) {
            key2 = keypad.getKey();
            if (key2 == '1') {
              lcd.clear();
              lcd.setCursor(0, 0);
              lcd.print("Mode1:Extend/Retract");
              next = 1;
              mode2 = 0;                  // enter mode 1
            }
            if (key2 == '2') {
              lcd.clear();
              lcd.setCursor(0, 0);
              lcd.print("Mode2:Manual Control");
              next = 1;
              mode2 = 1;                  // enter mode 2

            }
          }
          delay(1000);
          t1 = millis();                  // record time at then end of loop
        }
      }
    }
  }
}
void Loadcell() {

  static boolean newDataReady = 0;
  const int serialPrintInterval = 0; //increase value to slow down serial print activity

  // check for new data/start next conversion:
  if (LoadCell_1.update()) newDataReady = true;
  LoadCell_2.update();

  //get smoothed value from data set
  if ((newDataReady)) {
    if (millis() > t + serialPrintInterval) {
      float a = LoadCell_1.getData();
      float b = LoadCell_2.getData();
      String c = "g";
      lcd2.setCursor(0,0);
      lcd2.print("X = ");
      if (pressedA == 1){
        a = a*0.00220462;
        b = b*0.00220462;
        c = "lb";
      }
      lcd2.print(a,4);
      lcd2.setCursor(18,0);
      lcd2.print(c);

      lcd2.setCursor(0,1);
      lcd2.print("Y = ");
      lcd2.print(b,4);
      lcd2.setCursor(18,1);
      lcd2.print(c);

      newDataReady = 0;
      t = millis();
    }
  }

  // receive command from serial terminal, send 't' to initiate tare operation:
  if (Serial.available() > 0) {
    char inByte = Serial.read();
    if (inByte == 't') {
      LoadCell_1.tareNoDelay();
      LoadCell_2.tareNoDelay();
    }
  }

  //check if last tare operation is complete
  if (LoadCell_1.getTareStatus() == true) {
    lcd2.setCursor(0,2);
    lcd2.print("Tare load cell 1 complete");
  }
  if (LoadCell_2.getTareStatus() == true) {
    lcd2.setCursor(0,3);
    lcd2.print("Tare load cell 2 complete");
  }
}