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

// ---------------- PINS ----------------

#define STEP_PIN 26
#define DIR_PIN 23
#define EN_PIN 25
#define HOME_SWITCH 32
//#define END_SWITCH 33
//#define ESTOP_PIN 35

// -------------- SETTINGS --------------

#define STEPS_PER_MM 645.16
#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);

// ✅ ACCELSTEPPER INSTANCE (DRIVER MODE)
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
// -------------- 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,27};
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;
}
    
// ------------------------------------------------
bool confirmNoPCB()
{
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("   Remove PCB!");
  lcd.setCursor(0,1);
  lcd.print("Ensure unit is EMPTY");
  lcd.setCursor(0,3);
  lcd.print("  C=Yes  D=Cancel");
  
  while(true)
  {
    char k = keypad.getKey();

    if(k=='C')
    {
      lcd.clear();
      return true;
    }
    
    if(k=='D')
    {
      lcd.clear();
      return false;
    }
  }
}
// ------------------------------------------------
  
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++;
      stepper.setCurrentPosition(currentPositionSteps);
    }
    else
    {
      currentPositionSteps--;
      stepper.setCurrentPosition(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)==HIGH && dir==HIGH) break;
    if(digitalRead(HOME_SWITCH)==HIGH && 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()
  {
    if(!confirmNoPCB())
    return;
    
    lcd.clear();
    lcd.print("Homing...");
    
    while(digitalRead(HOME_SWITCH)==LOW)
    {
      //if(digitalRead(ESTOP_PIN)==LOW) break;
      stepWithDelay(false,100);    // Set home direction speed
    }
      
    currentPositionSteps=0;
    stepper.setCurrentPosition(0); 
    // ✅ SYNC
    
    lcd.clear();
    lcd.print("Homing Done");
    delay(800);
    lcd.clear();
  }
  
  // ------------------------------------------------
  // (ALL OTHER CODE UNCHANGED)
  // ------------------------------------------------
  // EDIT PRESET
  
void editPreset(int index)
{
  String val="";
  lcd.clear();
  lcd.print("Preset ");
  lcd.print(index+1);
    
  while(true)
  {
    lcd.setCursor(0,1);
    lcd.print("Product No.: ");
    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="";
    
  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;
  }
}

// ------------------------------------------------

void presetMenu()
{
  presetIndex = 0;
  int lastIndex = -1;

  lcd.clear();

  while(true)
  {
    if (presetIndex != lastIndex)
    {
      lcd.clear();

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

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

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

      lcd.setCursor(0,3);
      lcd.print("A^ Bv C=Go #Edit");

      lastIndex = presetIndex;
    }

    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;
        homeMachine();
        moveToMM(target);
      }
    }

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

    if(key=='D')
      break;

    delay(150); // 👈 slows refresh = stable display
  }

  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>up B>down C>edit");
    
    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    A>UP ":" Manual Jog    A>UP");
  lcd.setCursor(0,2);
  lcd.print(menuIndex==1?">Move To Pos":" Move To Pos   B>DWN");
  lcd.setCursor(0,3);
  lcd.print(menuIndex==2?">Presets       C>ENT":" Presets       C>ENT");
}

// ------------------------------------------------
void setup()
{
  pinMode(STEP_PIN,OUTPUT);
  pinMode(DIR_PIN,OUTPUT);
  pinMode(EN_PIN,OUTPUT);
  pinMode(HOME_SWITCH,INPUT_PULLDOWN);
  //pinMode(END_SWITCH,INPUT_PULLDOWN);
  
  //pinMode(ESTOP_PIN,INPUT_PULLUP);
  
  digitalWrite(EN_PIN,LOW);// ✅ ACCELSTEPPER SETTINGS (SAFE DEFAULTS)
  stepper.setMaxSpeed(2000);
  stepper.setAcceleration(1000);
  
  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.setCursor(4,1);
      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();
}