#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>

// ---------------- PINS ----------------
#define STEP_PIN 26
#define DIR_PIN 27
#define EN_PIN 25

#define HOME_SWITCH 32
#define END_SWITCH 33
#define ESTOP_PIN 35

// -------------- SETTINGS --------------
#define STEPS_PER_MM 800
#define MAX_TRAVEL_MM 1000

#define START_DELAY_US 1200
#define MIN_DELAY_US   300
#define ACCEL_STEP     10

#define INACTIVITY_TIME 10000

// ----------- SOFT LIMITS --------------
#define DISPLAY_OFFSET 100
#define SOFT_MIN_DISPLAY 100     // 99 not possible due to offset
#define SOFT_MAX_DISPLAY 399

#define SOFT_MIN_MM (SOFT_MIN_DISPLAY - DISPLAY_OFFSET)   // 0
#define SOFT_MAX_MM (SOFT_MAX_DISPLAY - DISPLAY_OFFSET)   // 299

LiquidCrystal_I2C lcd(0x27,20,4);

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

byte rowPins[ROWS]={13,12,14,23};
byte colPins[COLS]={16,17,18,19};

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

// -------------- VARIABLES --------------
long currentPositionSteps = 0;
int menuIndex=0;
unsigned long lastActivity=0;
bool inSubMenu=false;

int jogDistanceMM = 5;

// ------------------------------------------------
// DISPLAY POSITION
// ------------------------------------------------
long displayMM()
{
  return (currentPositionSteps / STEPS_PER_MM) + DISPLAY_OFFSET;
}

// ------------------------------------------------
// STEP FUNCTION
// ------------------------------------------------
void stepWithDelay(bool dir, int delayTime)
{
  digitalWrite(DIR_PIN, dir);
  digitalWrite(STEP_PIN, HIGH);
  delayMicroseconds(delayTime);
  digitalWrite(STEP_PIN, LOW);
  delayMicroseconds(delayTime);

  if(dir) currentPositionSteps++;
  else currentPositionSteps--;
}

// ------------------------------------------------
// RAMPED MOVE
// ------------------------------------------------
void moveStepsRamped(long steps)
{
  bool dir = (steps > 0);
  long total = abs(steps);

  int delayTime = START_DELAY_US;

  for(long i=0; i<total; i++)
  {
    if(digitalRead(END_SWITCH)==LOW && dir==HIGH) break;
    if(digitalRead(HOME_SWITCH)==LOW && dir==LOW) break;
    if(digitalRead(ESTOP_PIN)==LOW) break;

    stepWithDelay(dir, delayTime);

    if(delayTime > MIN_DELAY_US)
      delayTime -= ACCEL_STEP;
  }
}

// ------------------------------------------------
// MOVE ABSOLUTE (WITH SOFT LIMITS)
// ------------------------------------------------
void moveToMM(long mm)
{
  if(mm < SOFT_MIN_MM) mm = SOFT_MIN_MM;
  if(mm > SOFT_MAX_MM) mm = SOFT_MAX_MM;

  long targetSteps = mm * STEPS_PER_MM;
  long diff = targetSteps - currentPositionSteps;

  lcd.setCursor(0,3);
  lcd.print("Moving...        ");

  moveStepsRamped(diff);

  lcd.setCursor(0,3);
  lcd.print("Done             ");
  delay(500);
}

// ------------------------------------------------
// HOMING
// ------------------------------------------------
void homeMachine()
{
  lcd.clear();
  lcd.print("Homing...");

  while(digitalRead(HOME_SWITCH)==HIGH)
  {
    if(digitalRead(ESTOP_PIN)==LOW) break;
    stepWithDelay(false, 800);
  }

  currentPositionSteps=0;
  lcd.clear();
}

// ------------------------------------------------
// CHANGE JOG DISTANCE
// ------------------------------------------------
void changeJogDistance()
{
  lcd.clear();
  lcd.print("Set Jog Dist mm");
  String val="";

  while(true)
  {
    lcd.setCursor(0,1);
    lcd.print("Current: ");
    lcd.print(jogDistanceMM);
    lcd.print(" mm   ");

    lcd.setCursor(0,2);
    lcd.print("New: ");
    lcd.print(val);
    lcd.print("      ");

    lcd.setCursor(0,3);
    lcd.print("#set *del Dexit");

    char k = keypad.getKey();

    if(isdigit(k)) val+=k;
    if(k=='*' && val.length()>0) val.remove(val.length()-1);

    if(k=='#' && val.length()>0)
    {
      int newVal = val.toInt();
      if(newVal > 0 && newVal <= SOFT_MAX_MM)
        jogDistanceMM = newVal;
      break;
    }

    if(k=='D') break;
  }

  lcd.clear();
}

// ------------------------------------------------
// MANUAL MODE
// ------------------------------------------------
void manualMode()
{
  inSubMenu=true;
  lcd.clear();
  lcd.print("Manual Mode");

  while(true)
  {
    lcd.setCursor(0,0);
    lcd.print("Pos: ");
    lcd.print(displayMM());
    lcd.print(" mm     ");

    lcd.setCursor(0,1);
    lcd.print("Jog: ");
    lcd.print(jogDistanceMM);
    lcd.print(" mm     ");

    lcd.setCursor(0,3);
    lcd.print("A+ B- Cchg Dexit");

    char key = keypad.getKey();

    if(key=='A')
    {
      long newPos = (currentPositionSteps / STEPS_PER_MM) + jogDistanceMM;
      if(newPos <= SOFT_MAX_MM)
        moveStepsRamped(jogDistanceMM * STEPS_PER_MM);
      else
      {
        lcd.setCursor(0,3);
        lcd.print("MAX LIMIT!       ");
        delay(800);
      }
    }

    if(key=='B')
    {
      long newPos = (currentPositionSteps / STEPS_PER_MM) - jogDistanceMM;
      if(newPos >= SOFT_MIN_MM)
        moveStepsRamped(-jogDistanceMM * STEPS_PER_MM);
      else
      {
        lcd.setCursor(0,3);
        lcd.print("MIN LIMIT!       ");
        delay(800);
      }
    }

    if(key=='C') changeJogDistance();
    if(key=='D') break;
  }

  lcd.clear();
  inSubMenu=false;
}

// ------------------------------------------------
// DRAW MENU
// ------------------------------------------------
void drawMenu()
{
  lcd.setCursor(0,0);
  lcd.print("Pos: ");
  lcd.print(displayMM());
  lcd.print(" mm     ");

  lcd.setCursor(0,1);
  lcd.print(menuIndex==0?">Manual Jog ":" Manual Jog ");

  lcd.setCursor(0,2);
  lcd.print(menuIndex==1?">Move To Pos":" Move To Pos");

  lcd.setCursor(0,3);
  lcd.print("Aup Bdn Cset");
}

// ------------------------------------------------
// SETUP
// ------------------------------------------------
void setup()
{
  pinMode(STEP_PIN,OUTPUT);
  pinMode(DIR_PIN,OUTPUT);
  pinMode(EN_PIN,OUTPUT);

  pinMode(HOME_SWITCH,INPUT_PULLUP);
  pinMode(END_SWITCH,INPUT_PULLUP);
  pinMode(ESTOP_PIN,INPUT_PULLUP);

  digitalWrite(EN_PIN,LOW);

  lcd.init();
  lcd.backlight();

  homeMachine();
  lastActivity = millis();
}

// ------------------------------------------------
// LOOP
// ------------------------------------------------
void loop()
{
  char key = keypad.getKey();

  if(key)
    lastActivity = millis();

  if(millis() - lastActivity > INACTIVITY_TIME)
  {
    menuIndex = 0;
    lcd.clear();
    lastActivity = millis();
  }

  if(!inSubMenu)
  {
    if(key=='A') menuIndex--;
    if(key=='B') menuIndex++;

    if(menuIndex<0) menuIndex=0;
    if(menuIndex>1) menuIndex=1;

    if(menuIndex==0 && key=='C')
      manualMode();

    if(menuIndex==1 && key=='C')
    {
      inSubMenu=true;
      lcd.clear();
      lcd.print("Move To Position");
      String val="";

      while(true)
      {
        lcd.setCursor(0,1);
        lcd.print("Current: ");
        lcd.print(displayMM());
        lcd.print(" mm     ");

        lcd.setCursor(0,2);
        lcd.print("New: ");
        lcd.print(val);
        lcd.print("      ");

        lcd.setCursor(0,3);
        lcd.print("Cset *del Dexit");

        char k = keypad.getKey();

        if(isdigit(k)) val+=k;
        if(k=='*' && val.length()>0) val.remove(val.length()-1);

        if(k=='C' && val.length()>0)
        {
          long newMM = val.toInt() - DISPLAY_OFFSET;

          if(newMM < SOFT_MIN_MM) newMM = SOFT_MIN_MM;
          if(newMM > SOFT_MAX_MM) newMM = SOFT_MAX_MM;

          moveToMM(newMM);
          break;
        }

        if(k=='D') break;
      }

      lcd.clear();
      inSubMenu=false;
    }

    drawMenu();
  }
}