#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

#define MAX_PRESETS 50

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;

int jogDistanceMM = 5;

// -------- PRESET STORAGE --------
int presetProduct[MAX_PRESETS];
int presetWidth[MAX_PRESETS];
int presetIndex = 0;

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

// ------------------------------------------------
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--;
}

// ------------------------------------------------
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;
  }
}

// ------------------------------------------------
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);
}

// ------------------------------------------------
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();
}

// ------------------------------------------------
// EDIT PRESET
// ------------------------------------------------
void editPreset(int index)
{
  String val="";
  
  lcd.clear();
  lcd.print("Preset ");
  lcd.print(index+1);

  // Product Number
  while(true)
  {
    lcd.setCursor(0,1);
    lcd.print("Prod: ");
    lcd.print(val);
    lcd.print("      ");

    lcd.setCursor(0,3);
    lcd.print("# next D exit");

    char k = keypad.getKey();

    if(isdigit(k)) val+=k;

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

    if(k=='#' && val.length()>0)
    {
      presetProduct[index]=val.toInt();
      break;
    }

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

  val="";

  // Width
  while(true)
  {
    lcd.clear();
    lcd.print("Width mm:");

    lcd.setCursor(0,1);
    lcd.print(val);

    lcd.setCursor(0,3);
    lcd.print("# save D exit");

    char k = keypad.getKey();

    if(isdigit(k)) val+=k;

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

    if(k=='#' && val.length()>0)
    {
      presetWidth[index]=val.toInt();
      break;
    }

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

// ------------------------------------------------
// PRESET MENU
// ------------------------------------------------
void presetMenu()
{
  presetIndex=0;

  while(true)
  {
    lcd.clear();

    lcd.setCursor(0,0);
    lcd.print("Preset ");
    lcd.print(presetIndex+1);

    lcd.setCursor(0,1);
    lcd.print("Prod:");
    lcd.print(presetProduct[presetIndex]);

    lcd.setCursor(0,2);
    lcd.print("W:");
    lcd.print(presetWidth[presetIndex]);
    lcd.print(" mm");

    lcd.setCursor(0,3);
    lcd.print("Aup Bdn Cgo Dexit");

    char key = keypad.getKey();

    if(key=='A')
    {
      presetIndex--;
      if(presetIndex<0) presetIndex=0;
    }

    if(key=='B')
    {
      presetIndex++;
      if(presetIndex>=MAX_PRESETS) presetIndex=MAX_PRESETS-1;
    }

    if(key=='C')
    {
      if(presetWidth[presetIndex] > 0)
      {
        long target = presetWidth[presetIndex] - 100;

        if(target < 0) target = 0;
        if(target > MAX_TRAVEL_MM) target = MAX_TRAVEL_MM;

        homeMachine();
        moveToMM(target);
      }
    }
   

    if(key=='#')
      editPreset(presetIndex);

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

  lcd.clear();
}

// ------------------------------------------------
void changeJogDistance()
{
  lcd.clear();
  lcd.print("Set Jog Dist");

  String val="";

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

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

    lcd.setCursor(0,3);
    lcd.print("# set D exit");

    char k=keypad.getKey();

    if(isdigit(k)) val+=k;

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

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

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

  lcd.clear();
}

// ------------------------------------------------
void manualMode()
{
  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.setCursor(0,3);
    lcd.print("A+ B- C ent D exit");

    char key=keypad.getKey();

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

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

    if(key=='C')
      changeJogDistance();

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

  lcd.clear();
}

// ------------------------------------------------
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(menuIndex==2?">Presets":" Presets");
}

// ------------------------------------------------
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();
}

// ------------------------------------------------
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>2) menuIndex=2;

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

  if(menuIndex==1 && key=='C')
  {
    lcd.clear();
    lcd.print("Enter Position");

    String val="";

    while(true)
    {
      lcd.setCursor(0,1);
      lcd.print("New:");
      lcd.print(val);

      lcd.setCursor(0,3);
      lcd.print("C ent * clr D exit");

      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()-100;
        homeMachine();
        moveToMM(newMM);
        break;
      }

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

    lcd.clear();
  }

  if(menuIndex==2 && key=='C')
    presetMenu();

  drawMenu();
}