#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

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;

// ------------------------------------------------
// DISPLAY OFFSET (+100mm)
// ------------------------------------------------
long displayMM()
{
  return (currentPositionSteps / STEPS_PER_MM) + 100;
}

// ------------------------------------------------
// 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(ESTOP_PIN)==LOW) break;

    stepWithDelay(dir, delayTime);

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

// ------------------------------------------------
// MOVE ABSOLUTE
// ------------------------------------------------
void moveToMM(long mm)
{
  if(mm < 0) mm=0;
  if(mm > MAX_TRAVEL_MM) mm=MAX_TRAVEL_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.setCursor(0,3);
  lcd.print("Homing...");

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

  currentPositionSteps=0;
  lcd.clear();
}

// ------------------------------------------------
// DRAW MENU
// ------------------------------------------------
void drawMenu()
{
  lcd.setCursor(5,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("                ");
}

// ------------------------------------------------
// MANUAL MODE (STEP JOG ONLY)
// ------------------------------------------------
void manualMode()
{
  lcd.clear();
  lcd.print("Manual Mode");

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

    char key = keypad.getKey();

    if(key=='A')
      moveStepsRamped(1 * STEPS_PER_MM);

    if(key=='B')
      moveStepsRamped(-1 * STEPS_PER_MM);

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

  lcd.clear();
}

// ------------------------------------------------
// 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(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')
  {
    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("        ");

      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)
      {
        lcd.clear();
        lcd.print("Remove Board!");
        lcd.setCursor(0,1);
        lcd.print("Press C Confirm");

        while(true)
        {
          char confirmKey = keypad.getKey();
          if(confirmKey=='C')
          {
            long newMM = val.toInt() - 100;
            homeMachine();
            moveToMM(newMM);
            break;
          }
          if(confirmKey=='D') break;
        }
        break;
      }

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

    lcd.clear();
  }

  drawMenu();
}