#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
#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)
{
  long target = currentPositionSteps + steps;

  stepper.moveTo(target);

  while (stepper.distanceToGo() != 0)
  {
    // STOP conditions
    if(digitalRead(HOME_SWITCH)==HIGH && stepper.distanceToGo() < 0)
    {
      stepper.stop();
      break;
    }

    stepper.run();  // 🔥 handles acceleration automatically
  }

  currentPositionSteps = stepper.currentPosition();
}
  
// ------------------------------------------------
  
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();
}
  
// 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 = "";
  String lastVal = "";

  lcd.clear();
  lcd.print("Width mm:");

  while(true)
  {
    // ✅ only update when value changes
    if(val != lastVal)
    {
      lcd.setCursor(0,1);
      lcd.print("                    ");  // clear line once
      lcd.setCursor(0,1);
      lcd.print(val);
      lcd.print("_");

      lastVal = 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;

    delay(50); // small debounce + smoother display
  }
}

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

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

        break;   // exit preset menu 
      }
    }

    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("WIDTH:");
    lcd.print(displayMM());
    lcd.print("mm        ");
    
    lcd.setCursor(0,1);
    lcd.print("SET JOG:");
    lcd.print(jogDistanceMM);
    
    lcd.setCursor(0,3);
    lcd.print("A>^ B>v C>edit D>exi");
    
    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(" CURENT WIDTH:");
  lcd.print(displayMM());
  lcd.print("mm");
  
  lcd.setCursor(0,1);
  lcd.print(menuIndex==0?">SET WIDTH     A>UP ":" SET WIDTH     A>UP");
  
  lcd.setCursor(0,2);
  lcd.print(menuIndex==1?">MANUAL JOG    B>DWN":" MANUAL JOG    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(8000);      // set AccelStepper speed
  stepper.setAcceleration(3000);
  
  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')
  {
    lcd.clear();lcd.print("ENTER WIDTH:");
  
    String val="";
    String lastVal="";
  
    while(true)
    {
      if(val != lastVal)
      {
        lcd.setCursor(0,1);
        lcd.print("                    ");
        lcd.setCursor(0,1);
        lcd.print(val);
        lcd.print("_");

        lastVal = val;
      }

      lcd.setCursor(0,3);
      lcd.print("C>ent 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;

      delay(50);
    }
  
    lcd.clear();
  }
  
  // --- MANUAL JOG (now menuIndex 1) ---

  if(menuIndex==1 && key=='C')
  {
  manualMode();
  }
  
  if(menuIndex==2 && key=='C')
    presetMenu();
    drawMenu();
}