#include <AccelStepper.h>
#include <LiquidCrystal.h>
#include <Keypad.h>

// Define the motor control pins
const int stepPin1 = 8;  // TMC2208 Step Pin for Motor 1
const int dirPin1 = 9;   // TMC2208 Direction Pin for Motor 1
const int stepPin2 = 10; // TMC2208 Step Pin for Motor 2
const int dirPin2 = 11;  // TMC2208 Direction Pin for Motor 2

// Define the pushbutton pin
const int pushButtonPin1 = 21; // Replace with the pin where your pushbutton is connected
const int pushButtonPin2 = 20;

// Define the LCD pins
LiquidCrystal lcd(2, 3, 4, 5, 6, 7);

// Create AccelStepper objects for both motors
AccelStepper stepper1(1, stepPin1, dirPin1);
AccelStepper stepper2(1, stepPin2, dirPin2);

// Define the 4x4 matrix keypad
const byte ROWS = 4; //four rows
const byte COLS = 4; //four columns
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};

byte rowPins[ROWS] = {31, 33, 35, 37}; // Use the specified digital pins for row pins
byte colPins[COLS] = {39, 41, 43, 45}; // Connect to the column pinouts of the keypad
Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );

// State variables
enum State {
  NormalMode,
  StepsInputMode,
  DMSInputMode,
  trackingMode,
  delayMode,
  timerInputMode
};

State currentState = NormalMode;
volatile bool lcdState = true;
long stepsToMove = 420; // Default value, using 'long' for larger values
unsigned long lastLCDUpdateTime = 0;
const unsigned long lcdUpdateInterval = 500; // Update every 0.5 seconds
long timer1 = 0;
unsigned long startMillis = 0;
String inputBuffer = "";
bool motorRunning = false;
bool hoursToSteps = false;

long timer2 = 0;
bool timer2Input = false;
unsigned long delayMillis = 0;
int delayTime = 0;
unsigned long pauseMillis = 0;
int trackingTime = 0;

long degrees = 0;
long hours = 0;
long minutes = 0;
long seconds = 0;
long totalSeconds = 0;

void setup() {
  delay(200);
  attachInterrupt(digitalPinToInterrupt(pushButtonPin1), stop, HIGH);
  attachInterrupt(digitalPinToInterrupt(pushButtonPin2), lcdBegin, RISING);
  attachInterrupt(digitalPinToInterrupt(pushButtonPin2), lcdEnd, FALLING);
  if (digitalRead(pushButtonPin2) == false) {

  } else {
    lcd.clear();
    // Initialize the LCD
    lcd.begin(16, 2);
    lcd.setCursor(0, 0);
    lcd.print("      0 0       ");
    lcd.setCursor(0, 1);
    lcd.print("     (___/      ");
  }

  // Set the maximum speed and acceleration for both motors
  stepper1.setMaxSpeed(1000);
  stepper1.setAcceleration(500);
  stepper1.setSpeed(1000);

  stepper2.setMaxSpeed(1000);
  stepper2.setAcceleration(500);
  stepper2.setSpeed(1000);

  // Set the motors to stop initially
  stepper1.setCurrentPosition(0);
  stepper2.setCurrentPosition(0);

  // Set the pushbutton pin as an input
  pinMode(pushButtonPin1, INPUT);
  pinMode(pushButtonPin2, INPUT);

}
// 3.0948
void normalModeLoop() {
  char key = keypad.getKey();

  if (key == 'D') {
    stepper1.setSpeed(10);
    stepper1.move(stepsToMove);
    stepper1.runToPosition();

  }
  if (key == '#') {
    stepper2.setSpeed(10);
    stepper2.move(stepsToMove);
    stepper2.runToPosition();
  }

  unsigned long currentMillis = millis();
  if ((currentMillis - lastLCDUpdateTime >= lcdUpdateInterval) and (lcdState == true)) {
    lcd.setCursor(0, 0);
    lcd.print("Di:" + String(stepsToMove / 740.74)  +  "degrees   ");
    lcd.setCursor(0, 1);
    lcd.print("A" + String(stepper1.currentPosition()) + "B" + String(stepper2.currentPosition()) + "            ");
    lastLCDUpdateTime = currentMillis;

  }

  if (key == '0') {
    currentState = StepsInputMode;
    inputBuffer = "";
    lcd.setCursor(0, 0);
    lcd.print("Enter Dist.:");
    lcd.setCursor(0, 1);
    lcd.print("                ");
  } else if (key == '*') {
    currentState = DMSInputMode;
    inputBuffer = "";
    lcd.setCursor(0, 0);
    lcd.print("Enter :D  M S   ");
    lcd.setCursor(0, 1);
    lcd.print("                ");
  } else if (key == 'A') {
    motorRunning = true;
    stepper1.setSpeed(500);
  } else if (key == 'B') {
    motorRunning = true;
    stepper1.setSpeed(-500);
  } else if (key == 'C') {
    // Move the stepper motors to position 0 or "home"
    stepper1.moveTo(0);
    stepper1.runToPosition();
  }

  else if (key == '3') {
    motorRunning = true;
    stepper2.setSpeed(500);
  } else if (key == '6') {
    motorRunning = true;
    stepper2.setSpeed(-500);
  } else if (key == '9') {
    // Move the stepper motors to position 0 or "home"
    stepper2.moveTo(0);
    stepper2.runToPosition();
  }

  else if (key == '1') {
    lcd.setCursor(0, 0);
    lcd.print("Delay time:     ");
    delayMillis = millis();
    currentState = delayMode;
    delayLoop();
  }
  else if (key == '4') {
    stepsToMove = -stepsToMove;
  }
  else if (key == '2') {
    stepper1.setCurrentPosition(0);
  }
  else if (key == '5') {
    lcd.setCursor(0, 1);
    lcd.print("T1:" + String((timer1 / 60000)) + "T2:" + String((timer2 / 60000)));
    delay(3000);
  }
  else if (key == '8') {
    stepper1.setCurrentPosition(66667);
  }
  else if (key == '7') {
    currentState = timerInputMode;
    inputBuffer = "";
    timer2Input = false;
    lcd.setCursor(0, 0);
    lcd.print("tracking time:  ");
    lcd.setCursor(0, 1);
    lcd.print("                ");
  }

  else if (keypad.getState() == IDLE) {
    motorRunning = false;
  }
}

void stepsInputModeLoop() {
  char key = keypad.getKey();

  if (key >= '0' && key <= '9') {
    inputBuffer += key;
  } else if (key == '#') {
    stepsToMove = inputBuffer.toInt();
    currentState = NormalMode;
    inputBuffer = "";
    lcd.setCursor(0, 0);
    lcd.print("Dist: " + String(stepsToMove) + "      ");
    lcd.setCursor(0, 1);
    lcd.print("Pos1: " + String(stepper1.currentPosition()) + "          ");
  } else if (key == '*') {
    // Toggle the inputBuffer to be negative
    if (inputBuffer.length() > 0 && inputBuffer[0] != '-') {
      inputBuffer = '-' + inputBuffer;
    } else if (inputBuffer.length() > 0 && inputBuffer[0] == '-') {
      inputBuffer = inputBuffer.substring(1);
    }
    // Display the inputBuffer value on the LCD
    lcd.setCursor(0, 1);
    lcd.print("Input: " + inputBuffer + "   ");
  } else {
    lcd.setCursor(0, 1);
    lcd.print("Input: " + inputBuffer + "   ");
  }
}

void DMSInputModeLoop() {
  char key = keypad.getKey();

  if (key == 'C') {
    hoursToSteps = true;
    inputBuffer = "";
    lcd.setCursor(0, 0);
    lcd.print("Enter :h m s    ");
    lcd.setCursor(0, 1);
    lcd.print("                ");
  }

  if (key >= '0' && key <= '9') {
    inputBuffer += key;
  }
  else if (key == 'D') {
    inputBuffer += " ";
  } else if (key == '#') {
    // Convert DMS to steps and set stepsToMove for both motors
    stepsToMove = ((convertDMSToSteps()) / 4.86);
    currentState = NormalMode;
    inputBuffer = "";
    /*lcd.setCursor(0, 0);
      lcd.print("Dist: " + String(stepsToMove) + "     ");
      lcd.setCursor(0, 1);
      lcd.print("Curr.Pos 1: " + String(stepper1.currentPosition()) + "     ");*/
  } else if (key == '*') {
    // Toggle the inputBuffer to be negative
    if (inputBuffer.length() > 0 && inputBuffer[0] != '-') {
      inputBuffer = '-' + inputBuffer;
    } else if (inputBuffer.length() > 0 && inputBuffer[0] == '-') {
      inputBuffer = inputBuffer.substring(1);
    }
    // Display the inputBuffer value on the LCD
    lcd.setCursor(0, 1);
    lcd.print("INPUT: " + inputBuffer + "   ");
  } else {
    lcd.setCursor(0, 1);
    lcd.print("INPUT: " + inputBuffer + "   ");
  }
}

void timerInputModeLoop() {
  char key = keypad.getKey();

  if (key >= '0' && key <= '9') {
    inputBuffer += key;

  } else if (key == '#') {
    if (timer2Input == true) {
      timer2 = (inputBuffer.toInt() * 60000);
      currentState = NormalMode;
      inputBuffer = "";
      timer2Input = false;
    } else {
      timer1 = (inputBuffer.toInt() * 60000);
      currentState = NormalMode;
      inputBuffer = "";
    }
    // Convert DMS to steps and set stepsToMove for both motors

    /*lcd.setCursor(0, 0);
      lcd.print("Dist: " + String(stepsToMove) + "     ");
      lcd.setCursor(0, 1);
      lcd.print("Curr.Pos 1: " + String(stepper1.currentPosition()) + "     ");*/
  }  else if (key == 'C') {
    inputBuffer = "";
    timer2Input = true;
    lcd.setCursor(0, 0);
    lcd.print("Delay Time:     ");
  }
  else {
    lcd.setCursor(0, 1);
    lcd.print("INPUT: " + inputBuffer + "min       ");
  }
}


void delayLoop() {
  unsigned long currentMillis2 = millis();
  delayTime = (((timer2) - (currentMillis2 - delayMillis)) / 600);

  if (currentMillis2 - delayMillis > timer2) {
    startMillis = millis();
    currentState = trackingMode;
    normalModeLoop();
  } else {
    if (lcdState == true) {
      if ((delayTime < 100)) {
        lcd.setCursor(0, 1);
        lcd.print(String((delayTime * 0.6)) + "sec ");
        delay(1000);
      } else {
        lcd.setCursor(0, 1);
        lcd.print(String(delayTime / 100) + "min   ");
        delay(1000);
      }
    }
    if (currentState == NormalMode) {
      normalModeLoop();
    } else {
      delayLoop();//ervhberv
    }
  }
}


void trackingModeLoop() {

  unsigned long currentMillis2 = millis();
  trackingTime = (((timer1) - (currentMillis2 - startMillis)) / 1000);
  if ((currentState == NormalMode) or (currentMillis2 - startMillis > timer1)) {
    currentState = NormalMode;
  }
  else {
    if (currentMillis2 - pauseMillis > 5000 and (lcdState == true)) {
      if (trackingTime < 60) {
        noInterrupts();
        lcd.setCursor(13, 1);
        lcd.print("   ");
        lcd.setCursor(13, 1);
        lcd.print(trackingTime);
        pauseMillis = (millis() - 4000);
        interrupts();
      } else {
        //int print1 = (trackingTime);
        noInterrupts();
        lcd.setCursor(13, 1);
        lcd.print("  ");
        lcd.setCursor(13, 1);
        lcd.print(trackingTime / 60);
        interrupts();
        pauseMillis = millis();
      }
    }
    stepper1.runSpeed();
    trackingModeLoop();
  }
}

void stop() {
  stepper1.stop();
  stepper2.stop();
  stepper1.run();
  stepper2.run();
  delay(100);
  currentState = NormalMode;
  (normalModeLoop());
}



void lcdBegin() {
  lcdState = true;
  /*if ( currentState == delayMode) {
    delayLoop();
  } else if (currentState == trackingMode) {
    trackingModeLoop();
  } else {
    normalModeLoop();
  }*/
}

void lcdEnd() {
  lcdState = false;
  /*if ( currentState == delayMode) {
    delayLoop();
  } else if (currentState == trackingMode) {
    trackingModeLoop();
  } else {
    normalModeLoop();
  }*/
}

void loop() {
  if (currentState == NormalMode) {
    normalModeLoop();
  } else if (currentState == StepsInputMode) {
    stepsInputModeLoop();
  } else if (currentState == DMSInputMode) {
    DMSInputModeLoop();
  }
  else if (currentState == trackingMode) {
    lcd.setCursor(0, 0);
    lcd.print("wur the f* zitte");
    lcd.setCursor(0, 1);
    lcd.print("DIE ALIENS! ");
    stepper1.setSpeed(3.0948);
    trackingModeLoop();
  }
  else if (currentState == timerInputMode) {
    timerInputModeLoop();
  }

  if (motorRunning) {
    stepper1.runSpeed();
    stepper2.runSpeed();
  } else {
    stepper1.stop();
    stepper2.stop();
    stepper1.moveTo(stepper1.currentPosition());
    stepper2.moveTo(stepper2.currentPosition());
    stepper1.runToPosition();
    stepper2.runToPosition();
  }
}

long convertDMSToSteps() {
  if (hoursToSteps == true) {
    long hours = inputBuffer.substring(0, 2).toInt();
    long minutes = inputBuffer.substring(2, 4).toInt();
    long seconds = inputBuffer.substring(4, 6).toInt();
    long totalSeconds = (hours * 54000) + (minutes * 900) + (seconds * 15);
    return totalSeconds;
  } else {
    long degrees = inputBuffer.substring(0, 3).toInt();
    long minutes = inputBuffer.substring(3, 5).toInt();
    long seconds = inputBuffer.substring(5, 6).toInt();
    long totalSeconds = (degrees * 3600) + (minutes * 60) + seconds;
    return totalSeconds; // No need to multiply by STEPS_PER_SECOND as in the previous example
  }
}