#include <Keypad.h>
#include <LiquidCrystal_I2C.h>
#include <AccelStepper.h>
#include <math.h>
#include <stdlib.h>

//Define LCD parameters (I2C uses SCL and SDA pins)
#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

//Define LCD object
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

// Define pins for stepper motor control
#define DIR_PIN_MotA         2
#define STEP_PIN_MotA        3
#define DIR_PIN_MotB         6
#define STEP_PIN_MotB        7

//Define motor objects
AccelStepper myStepperA(AccelStepper::DRIVER, STEP_PIN_MotA, DIR_PIN_MotA);
AccelStepper myStepperB(AccelStepper::DRIVER, STEP_PIN_MotB, DIR_PIN_MotB);

//Define keypad mapping
const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};

//Define keypad pins
uint8_t colPins[COLS] = { 31, 33, 35, 37 }; // Pins connected to C1, C2, C3, C4
uint8_t rowPins[ROWS] = { 32, 34, 36, 38 }; // Pins connected to R1, R2, R3, R4

//Define keypad object
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

//Define global variables
//variables that may need to be updated based on motors/experimental setup:
//  MaxMotorSpeed; MaxMotorAcc; StepsPerMM
String StringVelA = ""; //Velocity input string for motor A (mm/s)
String StringVelB = ""; //Velocity input string for motor B (mm/s)
float VelA = 0; //Velocity value for motor A (mm/s)
float VelB = 0; //Velocity value for motor B (mm/s)
String StringDispA = ""; //Displacement input string for motor A (mm)
String StringDispB = ""; //Displacement input string for motor B (mm)
float DispA = 0; //Displacement value for motor A (mm)
float DispB = 0; //Displacement value for motor B (mm)
int Flag = 0; //Flag for which input is being entered
int RunFlag = 0; //Flag for if motors should be running
float StepsPerMM = 22.05;  // Number of steps per mm of axis travel (steps/mm)
float mmPerStep = 1 / StepsPerMM; // Number of mm of axis travel per step (mm/step)
float MotorBIncrDisplacement = 60; //Distance that motor B should move each time (mm)
float StepsPerSecA = 0; //Velocity is terms of steps per second for motor A (steps/s)
float StepsPerSecB = 0; //Velocity is terms of steps per second for motor B (steps/s)
float StepsA = 0; //Displacement is terms of steps for motor A (steps)
float StepsB = 0; //Displacement is terms of steps for motor B (steps)
int StarCount = 0; //Count variable for how many times '*' has been pressed
int OriginFlag = 0; //Flag to trigger conditionals to return motors to origin point
int MaxMotorSpeed = 1000; //Max allowed motor speed (steps/s?)
int MaxMotorAcc = 50; //Max allowed acceleration (steps/s^2?)
int StepperBIncCount = 0; //How many times the B motor has moved
float StepperBIncCountMax; //Max number of times the B motor can move

void(* resetFunc) (void) = 0; //declare reset function @ address 0

void setup() {
  //Put your setup code here, to run once:
  //Keypad
  Serial.begin(9600);
  StringVelA.reserve(10); // maximum number of digit for a number is 10, change if needed
  StringVelB.reserve(10); // maximum number of digit for a number is 10, change if needed
  StringDispA.reserve(10); // maximum number of digit for a number is 10, change if needed
  StringDispB.reserve(10); // maximum number of digit for a number is 10, change if needed

  //Set initial key values
  StringVelA = "0";
  StringVelB = "0";
  StringDispA = "0";
  StringDispB = "0";

  //Turn on LCD
  lcd.init();
  lcd.backlight();

  //Print initial values to LCD
  lcd.begin(LCD_COLUMNS, LCD_LINES); //Begin LCD
  lcd.setCursor(0, 0); //Set cursor positino on LCD
  //Print to LCD
  //Print commands broken up because lcd.print can only handle 1 data type at a time
  lcd.print("MotA Vel: ");
  lcd.print(strtod(StringVelA.c_str(), NULL) / 10, 1);
  lcd.print(" mm/s");
  lcd.setCursor(4, 1);
  lcd.print("Disp: ");
  lcd.print(strtod(StringDispA.c_str(), NULL) / 10, 1);
  lcd.print(" mm");
  lcd.setCursor(0, 2);
  lcd.print("MotB Vel: ");
  lcd.print(strtod(StringVelB.c_str(), NULL) / 10, 1);
  lcd.print(" mm/s");
  lcd.setCursor(4, 3);
  lcd.print("Disp: ");
  lcd.print(strtod(StringDispB.c_str(), NULL) / 10, 1);
  lcd.print(" mm");

  //Set max allowed motor speed
  myStepperA.setMaxSpeed(MaxMotorSpeed);
  myStepperB.setMaxSpeed(MaxMotorSpeed);

  //Set max acceleration (Controls move or moveto acceleration/speed)
  myStepperA.setAcceleration(MaxMotorAcc);
  myStepperB.setAcceleration(MaxMotorAcc);

  // Set pins for motor control as output
  pinMode(DIR_PIN_MotA, OUTPUT);
  pinMode(STEP_PIN_MotA, OUTPUT);
  pinMode(DIR_PIN_MotB, OUTPUT);
  pinMode(STEP_PIN_MotB, OUTPUT);
}

void loop() {
  //Put your main code here, to run repeatedly:
  //Get key from keypad
  char key = keypad.getKey();

  //Set flags based on A, B, C, and D keys for property being entered, reset counts, and update LCD screen
  if (key == 'A') {
    Flag = 0; //Set flag
    StarCount = 0; //Reset '*' count

    //Reset vars to allow to run again (can be used after returning to origin to keep settings)
    StepperBIncCount = 0;
    OriginFlag = 0;

    //Update LCD Display
    lcd.setCursor(0, 0);
    lcd.setCursor(0, 0);
    lcd.print("MotA Vel: ");
    lcd.print(strtod(StringVelA.c_str(), NULL) / 10, 1);
    lcd.print(" mm/s");
    lcd.setCursor(4, 1);
    lcd.print("Disp: ");
    lcd.print(strtod(StringDispA.c_str(), NULL) / 10, 1);
    lcd.print(" mm");
    lcd.setCursor(0, 2);
    lcd.print("MotB Vel: ");
    lcd.print(strtod(StringVelB.c_str(), NULL) / 10, 1);
    lcd.print(" mm/s");
    lcd.setCursor(4, 3);
    lcd.print("Disp: ");
    lcd.print(strtod(StringDispB.c_str(), NULL) / 10, 1);
    lcd.print(" mm");
  }

  if (key == 'B') {
    Flag = 1;
    StarCount = 0;

    //Reset vars to allow to run again (can be used after returning to origin to keep settings)
    StepperBIncCount = 0;
    OriginFlag = 0;

    //Update LCD Display
    lcd.setCursor(0, 0);
    lcd.setCursor(0, 0);
    lcd.print("MotA Vel: ");
    lcd.print(strtod(StringVelA.c_str(), NULL) / 10, 1);
    lcd.print(" mm/s");
    lcd.setCursor(4, 1);
    lcd.print("Disp: ");
    lcd.print(strtod(StringDispA.c_str(), NULL) / 10, 1);
    lcd.print(" mm");
    lcd.setCursor(0, 2);
    lcd.print("MotB Vel: ");
    lcd.print(strtod(StringVelB.c_str(), NULL) / 10, 1);
    lcd.print(" mm/s");
    lcd.setCursor(4, 3);
    lcd.print("Disp: ");
    lcd.print(strtod(StringDispB.c_str(), NULL) / 10, 1);
    lcd.print(" mm");
  }

  if (key == 'C') {
    Flag = 2;
    StarCount = 0;

    //Reset vars to allow to run again (can be used after returning to origin to keep settings)
    StepperBIncCount = 0;
    OriginFlag = 0;

    //Update LCD Display
    lcd.setCursor(0, 0);
    lcd.setCursor(0, 0);
    lcd.print("MotA Vel: ");
    lcd.print(strtod(StringVelA.c_str(), NULL) / 10, 1);
    lcd.print(" mm/s");
    lcd.setCursor(4, 1);
    lcd.print("Disp: ");
    lcd.print(strtod(StringDispA.c_str(), NULL) / 10, 1);
    lcd.print(" mm");
    lcd.setCursor(0, 2);
    lcd.print("MotB Vel: ");
    lcd.print(strtod(StringVelB.c_str(), NULL) / 10, 1);
    lcd.print(" mm/s");
    lcd.setCursor(4, 3);
    lcd.print("Disp: ");
    lcd.print(strtod(StringDispB.c_str(), NULL) / 10, 1);
    lcd.print(" mm");
  }

  if (key == 'D') {
    Flag = 3;
    StarCount = 0;

    //Reset vars to allow to run again (can be used after returning to origin to keep settings)
    StepperBIncCount = 0;
    OriginFlag = 0;

    //Update LCD Display
    lcd.setCursor(0, 0);
    lcd.setCursor(0, 0);
    lcd.print("MotA Vel: ");
    lcd.print(strtod(StringVelA.c_str(), NULL) / 10, 1);
    lcd.print(" mm/s");
    lcd.setCursor(4, 1);
    lcd.print("Disp: ");
    lcd.print(strtod(StringDispA.c_str(), NULL) / 10, 1);
    lcd.print(" mm");
    lcd.setCursor(0, 2);
    lcd.print("MotB Vel: ");
    lcd.print(strtod(StringVelB.c_str(), NULL) / 10, 1);
    lcd.print(" mm/s");
    lcd.setCursor(4, 3);
    lcd.print("Disp: ");
    lcd.print(strtod(StringDispB.c_str(), NULL) / 10, 1);
    lcd.print(" mm");
  }

  //Take in keypad numberic values for properties
  if (key && Flag == 0) {
    if (key >= '0' && key <= '9') {     // only act on numeric keys
      if (StringVelA  == "0") {
        StringVelA = key; // Overwrite current value
      } else {
        StringVelA += key; // Append new character to input string
      }
      //Update LCD Display
      lcd.setCursor(0, 0);
      lcd.setCursor(0, 0);
      lcd.print("MotA Vel: ");
      lcd.print(strtod(StringVelA.c_str(), NULL) / 10, 1);
      lcd.print(" mm/s");
      lcd.setCursor(4, 1);
      lcd.print("Disp: ");
      lcd.print(strtod(StringDispA.c_str(), NULL) / 10, 1);
      lcd.print(" mm");
      lcd.setCursor(0, 2);
      lcd.print("MotB Vel: ");
      lcd.print(strtod(StringVelB.c_str(), NULL) / 10, 1);
      lcd.print(" mm/s");
      lcd.setCursor(4, 3);
      lcd.print("Disp: ");
      lcd.print(strtod(StringDispB.c_str(), NULL) / 10, 1);
      lcd.print(" mm");
    } else if (key == '*') { //Clear input if '*' is pressed
      StringVelA = "";                 // clear input
    }
  }

  if (key && Flag == 1) {
    if (key >= '0' && key <= '9') {     // only act on numeric keys
      if (StringDispA  == "0") {
        StringDispA = key; // Overwrite current value
      } else {
        StringDispA += key; // Append new character to input string
      }
      //Update LCD Display
      lcd.setCursor(0, 0);
      lcd.print("MotA Vel: ");
      lcd.print(strtod(StringVelA.c_str(), NULL) / 10, 1);
      lcd.print(" mm/s");
      lcd.setCursor(4, 1);
      lcd.print("Disp: ");
      lcd.print(strtod(StringDispA.c_str(), NULL) / 10, 1);
      lcd.print(" mm");
      lcd.setCursor(0, 2);
      lcd.print("MotB Vel: ");
      lcd.print(strtod(StringVelB.c_str(), NULL) / 10, 1);
      lcd.print(" mm/s");
      lcd.setCursor(4, 3);
      lcd.print("Disp: ");
      lcd.print(strtod(StringDispB.c_str(), NULL) / 10, 1);
      lcd.print(" mm");
    } else if (key == '*') {
      StringDispA = "";                 // clear input
    }
  }

  if (key && Flag == 2) {
    if (key >= '0' && key <= '9') {     // only act on numeric keys
      if (StringVelB  == "0") {
        StringVelB = key; // Overwrite current value
      } else {
        StringVelB += key; // Append new character to input string
      }
      //Update LCD Display
      lcd.setCursor(0, 0);
      lcd.print("MotA Vel: ");
      lcd.print(strtod(StringVelA.c_str(), NULL) / 10, 1);
      lcd.print(" mm/s");
      lcd.setCursor(4, 1);
      lcd.print("Disp: ");
      lcd.print(strtod(StringDispA.c_str(), NULL) / 10, 1);
      lcd.print(" mm");
      lcd.setCursor(0, 2);
      lcd.print("MotB Vel: ");
      lcd.print(strtod(StringVelB.c_str(), NULL) / 10, 1);
      lcd.print(" mm/s");
      lcd.setCursor(4, 3);
      lcd.print("Disp: ");
      lcd.print(strtod(StringDispB.c_str(), NULL) / 10, 1);
      lcd.print(" mm");
    } else if (key == '*') {
      StringVelB = "";                 // clear input
    }
  }

  if (key && Flag == 3) {
    if (key >= '0' && key <= '9') {     // only act on numeric keys
      if (StringDispB  == "0") {
        StringDispB = key; // Overwrite current value
      } else {
        StringDispB += key; // Append new character to input string
      }
      //Update LCD Display
      lcd.setCursor(0, 0);
      lcd.print("MotA Vel: ");
      lcd.print(strtod(StringVelA.c_str(), NULL) / 10, 1);
      lcd.print(" mm/s");
      lcd.setCursor(4, 1);
      lcd.print("Disp: ");
      lcd.print(strtod(StringDispA.c_str(), NULL) / 10, 1);
      lcd.print(" mm");
      lcd.setCursor(0, 2);
      lcd.print("MotB Vel: ");
      lcd.print(strtod(StringVelB.c_str(), NULL) / 10, 1);
      lcd.print(" mm/s");
      lcd.setCursor(4, 3);
      lcd.print("Disp: ");
      lcd.print(strtod(StringDispB.c_str(), NULL) / 10, 1);
      lcd.print(" mm");
    } else if (key == '*') {
      StringDispB = "";                 // clear input
    }
  }

  //Perform defined actions if '*' is pressed repeatedly
  if (key == '*') {
    StarCount += 1; //Increment '*' count

    //Set origin flag to 1 if '*' is pressed 3 times and motors have been moved from origin
    if (StarCount == 3 && Flag == 4) {
      OriginFlag = 1;
    }

    //Reset Arduino if '*' is pressed 4 times without reset (reset by pressing A, B, C, or D)
    if (StarCount == 4) {
      lcd.clear(); //Clear LCD of current text
      //Print reset message
      lcd.setCursor(5, 1);
      lcd.print("Resetting");
      lcd.setCursor(6, 2);
      lcd.print("Arduino");
      delay(1000);
      resetFunc(); //Reset Arduino
    }
  }

  //Set RunFlag to run motors
  if (key == '#') {
    if (RunFlag == 0) {
      RunFlag = 1;
    }

    Flag = 4;

    //Turn strings into numeric
    VelA = StringVelA.toInt(); // YOU GOT AN INTEGER NUMBER
    VelB = StringVelB.toInt(); // YOU GOT AN INTEGER NUMBER
    DispA = StringDispA.toInt(); // YOU GOT AN INTEGER NUMBER
    DispB = StringDispB.toInt(); // YOU GOT AN INTEGER NUMBER

    //Divide by 10 to give 1 decimal place
    VelA = -VelA / 10;
    VelB = VelB / 10;
    DispA = DispA / 10;
    DispB = DispB / 10;
  }

  if (RunFlag == 1) {

    //Redefine inputs in terms of steps
    StepsPerSecA = VelA * StepsPerMM;
    StepsPerSecB = VelB * StepsPerMM;
    StepsA = DispA * StepsPerMM;
    //StepsB = DispB * StepsPerMM;

    StepperBIncCountMax = DispB / MotorBIncrDisplacement;

    //Run MotorA if there is a defined velocity and diplacement (first loop only)
    if (StepperBIncCount == 0 && VelA > 0) {
      myStepperA.move(StepsA);
      while (myStepperA.distanceToGo() > 0) {
        myStepperA.run();
        myStepperA.setSpeed(StepsPerSecA);
      }
    }

    //Swap direction of A
    VelA = -VelA;

    //Tell Motor B how much to move
    myStepperB.move(MotorBIncrDisplacement * StepsPerMM);

    //Stop Motor A
    myStepperA.stop();

    //Run Motor B if it has a defined velocity and dispacement (cycles moving MotorB and MotorA)
    if (VelB > 0 && StepperBIncCountMax > 0) {
      while (myStepperB.distanceToGo() > 0) {
        myStepperB.run();
        myStepperB.setSpeed(StepsPerSecB);
      }

      //Stop MotorB
      myStepperB.stop();

      //Run MotorA again after MotorB has moved (dirction dependent on VelA)
      if (VelA > 0) {
        myStepperA.move(-StepsA);
        while (myStepperA.currentPosition() < StepsA) {
          myStepperA.run();
          myStepperA.setSpeed(-StepsPerSecA);
        }
      }

      if (VelA < 0) {
        myStepperA.move(-StepsA);
        while (myStepperA.currentPosition() > 0) {
          myStepperA.run();
          myStepperA.setSpeed(-StepsPerSecA);
        }
      }
    }

    //Increment MotorB ccount
    StepperBIncCount += 1;

    //Stop both motors after meeting movement goals
    if (StepperBIncCount > StepperBIncCountMax) {
      myStepperA.stop();
      myStepperB.stop();
      RunFlag = 0;
    }
  }

  //Redefine movement targets to return motors to origin
  if (OriginFlag == 1) {
    // myStepperA.move(-myStepperA.currentPosition());
    // myStepperB.move(-myStepperB.currentPosition());
    myStepperA.moveTo(0);
    myStepperB.moveTo(0);
    OriginFlag = 2;
    lcd.clear();
    lcd.setCursor(4, 1);
    lcd.print("Returning to");
    lcd.setCursor(7, 2);
    lcd.print("Origin");
  }

  //Run motors until reach origin
  if (OriginFlag == 2) {
    if (VelA != 0 && DispA != 0) { //Only run if velocity and displacement defined for MotorA
      myStepperA.run();
    }

    if (VelB != 0 && DispB != 0) { //Only run if velocity and displacement defined for MotorB
      myStepperB.run();
    }
  }
}